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

Browse Source
This commit is contained in:
Robot 2019-11-11 15:39:31 +08:00
parent 588c95d1fe
commit 5c9fdfce8d
1 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
wpb_home_behaviors/src/wpb_home_grab_server.cpp
View File

@ -156,6 +156,7 @@ static float fTargetGrabY = 0.0; //抓取时目标物品的y坐标
static std::vector<stBoxMarker> vObj;
static stBoxMarker boxLastObject;
static int nObjDetectCounter = 0;
static int nTimeOut = 0;
void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
{
@ -281,7 +282,12 @@ void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
}
if (planeIndices->indices.size() == 0)
{
std::cout << "Could not find a plane in the scene." << std::endl;
nTimeOut ++;
if(nTimeOut > 10)
nStep = STEP_DONE;
}
else
{
// Copy the points of the plane to a new cloud.
@ -433,6 +439,9 @@ void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
{
nObjDetectCounter = 0;
ROS_WARN("[FIND_OBJ] nObjCnt <= 0 No object detected ...");
nTimeOut ++;
if(nTimeOut > 10)
nStep = STEP_DONE;
}
}
else
@ -628,6 +637,8 @@ void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
{
result_msg.data = "done";
result_pub.publish(result_msg);
nTimeOut = 0;
nStep = STEP_WAIT;
}
}
@ -745,6 +756,7 @@ void BehaviorCB(const std_msgs::String::ConstPtr &msg)
nFindIndex = msg->data.find("grab start");
if( nFindIndex >= 0 )
{
nTimeOut = 0;
VelCmd(0,0,0);
nStep = STEP_FIND_PLANE;
ROS_WARN("[grab_start] ");
@ -754,6 +766,7 @@ void BehaviorCB(const std_msgs::String::ConstPtr &msg)
if( nFindIndex >= 0 )
{
ROS_WARN("[grab_stop] ");
nTimeOut = 0;
nStep = STEP_WAIT;
geometry_msgs::Twist vel_cmd;
vel_cmd.linear.x = 0;