ROS_Robot/wpb_home
1
0
Fork 0
mirror of https://github.com/6-robot/wpb_home.git synced 2025-09-15 12:58:59 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

update grab_server for simulation

Browse Source
This commit is contained in:
zhangwanjie 2020-06-21 01:58:49 +08:00
parent 5925782b90
commit 7946834824
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
wpb_home_behaviors/src/wpb_home_grab_server.cpp
View File

@ -185,7 +185,9 @@ void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
if( if(
p.x < 1.2 && p.x < 1.2 &&
p.y > -1.0 && p.y > -1.0 &&
p.y < 1.0 p.y < 1.0 &&
p.z > 0.6 &&
p.z < 1.5
) )
{ {
new_point_cloud.push_back(p); new_point_cloud.push_back(p);
@ -578,7 +580,7 @@ void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
} }
nTimeDelayCounter ++; nTimeDelayCounter ++;
VelCmd(0,0,0); VelCmd(0,0,0);
if(nTimeDelayCounter > 50) if(nTimeDelayCounter > 200)
{ {
fMoveTargetX = fObjGrabX -0.55 + grab_forward_offset; fMoveTargetX = fObjGrabX -0.55 + grab_forward_offset;
fMoveTargetY = 0; fMoveTargetY = 0;
@ -602,7 +604,7 @@ void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
nTimeDelayCounter++; nTimeDelayCounter++;
VelCmd(0,0,0); VelCmd(0,0,0);
if(nTimeDelayCounter > 30) if(nTimeDelayCounter > 50)
{ {
nTimeDelayCounter = 0; nTimeDelayCounter = 0;
nStep = STEP_OBJ_UP; nStep = STEP_OBJ_UP;
@ -621,7 +623,7 @@ void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
} }
nTimeDelayCounter++; nTimeDelayCounter++;
VelCmd(0,0,0); VelCmd(0,0,0);
if(nTimeDelayCounter > 10) if(nTimeDelayCounter > 50)
{ {
fMoveTargetX = -(fTargetGrabX-0.4); fMoveTargetX = -(fTargetGrabX-0.4);
fMoveTargetY = 0; fMoveTargetY = 0;