Action server: Waiting for the end of the action to confirm it has been carried out

asked 2021-06-14 05:28:20 -0500

RB_Code gravatar image

I've built an action server to control a gripper, only now I wanted before sending the message saying if the action was executed or failed, to check if it really achieved the goal.

What I put was a 4.5 seconds delay that waits for the gripper to finish the movement and then check if it reached the goal or not. How can I do exactly the same without having to set a time?

ros::Duration(4.5).sleep(); //4.5s 

if ((((goal->order) == 1) and ((Pose_G <= 14.1+0.5) and (Pose_G >= 14.1-0.5))) or (((goal->order) == 2) and ((Pose_G <=0.5) and (Pose_G >= -0.5))) or  (((goal->order) == 3) and ((Pose_G <= (goal->pose)+0.5) and (Pose_G >= (goal->pose)-0.5))))
{
  success = true;  
}
else
{
  ROS_ERROR("FAILED goal");
  success = false;  
}

if(success)
{
  result_.move = true;
  ROS_INFO("%s: Succeeded_action", action_name_.c_str());
  // set the action state to succeeded
  as_.setSucceeded(result_);
}

}

edit retag flag offensive close merge delete