// Cancel the action
void preemptCB()
{
ROS_INFO_STREAM_NAMED("pick_place_moveit","Preempted");
action_server_.setPreempted();
}
void executeCB(const race_move_arms::CarryarmGoalConstPtr &goal)
{
// make sure that the action hasn't been canceled
if (!as_.isActive())
return;
bool success = true;
bool rightArm = true; // right arm
ROS_INFO("Executing, creating carryarm action");
// start executing the action
if (goal->carrypose == 1) // both arms are not in carrypose!
{
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
success = false;
}
else
{
ROS_INFO("Executing arm trajectory..");
switch (goal->carryarm)
{
case 0:
ROS_INFO("Using right arm");
success = moveRightArm();
break;
case 1:
ROS_INFO("Using left arm");
success = moveLeftArm();
break;
case 2:
ROS_INFO("Using both arms");
rightArm = true;
success = moveArmToAbove(rightArm);
success &= moveBothArms();
break;
default:
ROS_INFO("Unknown carryarm command, ignoring");
break;
}
}
}
else if (goal->carrypose == 2) // the other arm is already in carrypose!
{
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
success = false;
}
else
{
ROS_INFO("Executing arm trajectory..");
switch (goal->carryarm)
{
case 0:
ROS_INFO("Using right arm");
success = moveBothArms(); // trivial case
break;
case 1:
ROS_INFO("Using left arm");
//move right arm to side first
rightArm = true;
success = moveArmToAbove(rightArm);
success &= moveBothArms(); // trivial case
break;
default:
ROS_INFO("Unknown carryarm command, ignoring");
break;
}
}
}
else if (goal->carrypose == 3) // move arm to above
{
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
success = false;
}
else
{
ROS_INFO("Executing arm trajectory..");
switch (goal->carryarm)
{
case 0:
ROS_INFO("Using right arm");
rightArm = true;
success = moveArmToAbove(rightArm);
break;
case 1:
ROS_INFO("Using left arm");
rightArm = false;
success = moveArmToAbove(rightArm);
break;
case 2:
ROS_INFO("Using both arms");
rightArm = true;
success = moveArmToAbove(rightArm);
rightArm = false;
success &= moveArmToAbove(rightArm);
break;
default:
ROS_INFO("Unknown carryarm command, ignoring");
break;
}
}
}
else if (goal->carrypose == 4) // move arm to side
{
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
success = false;
}
else
{
ROS_INFO("Executing arm trajectory..");
switch (goal->carryarm)
{
case 0:
ROS_INFO("Using right arm");
rightArm = true;
//success = moveArmToAbove(rightArm);
success = moveArmToSide(rightArm);
break;
case 1:
ROS_INFO("Using left arm");
rightArm = true;
success = moveArmToAbove(rightArm);
rightArm = false;
//success = moveArmToAbove(rightArm);
success &= moveArmToSide(rightArm);
success &= moveRightArm();
break;
case 2:
ROS_INFO("Using both arms");
rightArm = true;
//success = moveArmToAbove(rightArm);
success = moveArmToSide(rightArm);
rightArm = false;
//success &= moveArmToAbove(rightArm);
success &= moveArmToSide(rightArm);
break;
default:
ROS_INFO("Unknown carryarm command, ignoring");
break;
}
}
}
if(success)
{
result_.result = 1;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
else
{
result_.result = 0;
ROS_INFO("%s: Failed", action_name_.c_str());
// set the action state to failed
//as_.setSucceeded(result_);
//as_.setAborted(result_);
as_.setAborted(result_);
}
}
