// 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_); } }