Action server: Waiting for the end of the action to confirm it has been carried out
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_);
}
}