// Cancel the action
 void preemptCB()
 {
   ROS_INFO_STREAM_NAMED("pick_place_moveit","Preempted");
   action_server_.setPreempted();
 }
Beispiel #2
0
    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_);
      }
    }