C++ SimpleActionServer::setAborted方法代码示例

		/*!
		* \brief Executes the callback from the actionlib
		*
		* Set the current goal to aborted after receiving a new goal and write new goal to a member variable. Wait for the goal to finish and set actionlib status to succeeded.
		* \param goal JointTrajectoryGoal
		*/
		void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
		{			
			ROS_INFO("sdh: executeCB");
			if (!isInitialized_)
			{
				ROS_ERROR("%s: Rejected, sdh not initialized", action_name_.c_str());
				as_.setAborted();
				return;
			}

			while (hasNewGoal_ == true ) usleep(10000);

			// \todo TODO: use joint_names for assigning values
			targetAngles_.resize(DOF_);
			targetAngles_[0] = goal->trajectory.points[0].positions[2]*180.0/pi_; // sdh_knuckle_joint
			targetAngles_[1] = goal->trajectory.points[0].positions[5]*180.0/pi_; // sdh_finger22_joint
			targetAngles_[2] = goal->trajectory.points[0].positions[6]*180.0/pi_; // sdh_finger23_joint
			targetAngles_[3] = goal->trajectory.points[0].positions[0]*180.0/pi_; // sdh_thumb2_joint
			targetAngles_[4] = goal->trajectory.points[0].positions[1]*180.0/pi_; // sdh_thumb3_joint
			targetAngles_[5] = goal->trajectory.points[0].positions[3]*180.0/pi_; // sdh_finger12_joint
			targetAngles_[6] = goal->trajectory.points[0].positions[4]*180.0/pi_; // sdh_finger13_joint
			ROS_INFO("received new position goal: [['sdh_thumb_2_joint', 'sdh_thumb_3_joint', 'sdh_knuckle_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint']] = [%f,%f,%f,%f,%f,%f,%f,%f]",goal->trajectory.points[0].positions[0],goal->trajectory.points[0].positions[1],goal->trajectory.points[0].positions[2],goal->trajectory.points[0].positions[3],goal->trajectory.points[0].positions[4],goal->trajectory.points[0].positions[5],goal->trajectory.points[0].positions[6]);
		
			hasNewGoal_ = true;
			
			usleep(500000); // needed sleep until sdh starts to change status from idle to moving
			
			bool finished = false;
			while(finished == false)
			{
				if (as_.isNewGoalAvailable())
				{
					ROS_WARN("%s: Aborted", action_name_.c_str());
					as_.setAborted();
					return;
				}
				for ( size_t i = 0; i < state_.size(); i++ )
		 		{
		 			ROS_DEBUG("state[%d] = %d",i,state_[i]);
		 			if (state_[i] == 0)
		 			{
		 				finished = true;
		 			}
		 			else
		 			{	
		 				finished = false;
		 			}
		 		}
		 		usleep(10000);
				//feedback_ = 
				//as_.send feedback_
			}

			// set the action state to succeeded			
			ROS_INFO("%s: Succeeded", action_name_.c_str());
			//result_.result.data = "succesfully received new goal";
			//result_.success = 1;
			//as_.setSucceeded(result_);
			as_.setSucceeded();
		}

void Navigator::executeCB(const actionlib::SimpleActionServer<navigator::navigatorAction>::GoalConstPtr& goal) {
    int destination_id = goal->location_code;
    geometry_msgs::PoseStamped destination_pose;
    int navigation_status;

    if (destination_id==navigator::navigatorGoal::COORDS) {
        destination_pose=goal->desired_pose;
    }
    
    switch(destination_id) {
        case navigator::navigatorGoal::HOME: 
              //specialized function to navigate to pre-defined HOME coords
               navigation_status = navigate_home(); 
               if (navigation_status==navigator::navigatorResult::DESIRED_POSE_ACHIEVED) {
                   ROS_INFO("reached home");
                   result_.return_code = navigator::navigatorResult::DESIRED_POSE_ACHIEVED;
                   navigator_as_.setSucceeded(result_);
               }
               else {
                   ROS_WARN("could not navigate home!");
                   navigator_as_.setAborted(result_);
               }
               break;
        case navigator::navigatorGoal::TABLE: 
              //specialized function to navigate to pre-defined TABLE coords
               navigation_status = navigate_to_table(); 
               if (navigation_status==navigator::navigatorResult::DESIRED_POSE_ACHIEVED) {
                   ROS_INFO("reached table");
                   result_.return_code = navigator::navigatorResult::DESIRED_POSE_ACHIEVED;
                   navigator_as_.setSucceeded(result_);
               }
               else {
                   ROS_WARN("could not navigate to table!");
                   navigator_as_.setAborted(result_);
               }
               break;
        case navigator::navigatorGoal::COORDS: 
              //more general function to navigate to specified pose:
              destination_pose=goal->desired_pose;
               navigation_status = navigate_to_pose(destination_pose); 
               if (navigation_status==navigator::navigatorResult::DESIRED_POSE_ACHIEVED) {
                   ROS_INFO("reached desired pose");
                   result_.return_code = navigator::navigatorResult::DESIRED_POSE_ACHIEVED;
                   navigator_as_.setSucceeded(result_);
               }
               else {
                   ROS_WARN("could not navigate to desired pose!");
                   navigator_as_.setAborted(result_);
               }
               break;               
               
        default:
             ROS_WARN("this location ID is not implemented");
             result_.return_code = navigator::navigatorResult::DESTINATION_CODE_UNRECOGNIZED; 
             navigator_as_.setAborted(result_);
            }
  
}

		/*!
		* \brief Executes the callback from the actionlib.
		*
		* Set the current goal to aborted after receiving a new goal and write new goal to a member variable. Wait for the goal to finish and set actionlib status to succeeded.
		* \param goal JointTrajectoryGoal
		*/
		void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
		{
			ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
			if (!isInitialized_)
			{
				ROS_ERROR("%s: Rejected, powercubes not initialized", action_name_.c_str());
				as_.setAborted();
				return;
			}
			// saving goal into local variables
			traj_ = goal->trajectory;
			traj_point_nr_ = 0;
			traj_point_ = traj_.points[traj_point_nr_];
			finished_ = false;
			
			// stoping arm to prepare for new trajectory
			std::vector<double> VelZero;
			VelZero.resize(ModIds_param_.size());
			PCube_->MoveVel(VelZero);

			// check that preempt has not been requested by the client
			if (as_.isPreemptRequested())
			{
				ROS_INFO("%s: Preempted", action_name_.c_str());
				// set the action state to preempted
				as_.setPreempted();
			}
			
			usleep(500000); // needed sleep until powercubes starts to change status from idle to moving
			
			while(finished_ == false)
			{
				if (as_.isNewGoalAvailable())
				{
					ROS_WARN("%s: Aborted", action_name_.c_str());
					as_.setAborted();
					return;
				}
		   		usleep(10000);
				//feedback_ = 
				//as_.send feedback_
			}

			// set the action state to succeed			
			//result_.result.data = "executing trajectory";
			ROS_INFO("%s: Succeeded", action_name_.c_str());
			// set the action state to succeeded
			as_.setSucceeded(result_);
		}

      void setJointsCB( const staubliTX60::SetJointsGoalConstPtr &goal ) {
	 //staubli.ResetMotion();
	 ros::Rate rate(10);
	 bool success = true;
	 ROS_INFO("Set Joints Action Cmd received \n");
	 if( staubli.MoveJoints(goal->j,
				goal->params.movementType,
				goal->params.jointVelocity,
				goal->params.jointAcc,
				goal->params.jointDec,
				goal->params.endEffectorMaxTranslationVel, 
				goal->params.endEffectorMaxRotationalVel,
				goal->params.distBlendPrev,
				goal->params.distBlendNext
				) )
	   {
	    ROS_INFO("Cmd received, moving to desired joint angles.");
	    while(true){
	       if (as_.isPreemptRequested() || !ros::ok()) {
		  ROS_INFO("%s: Preempted", action_name_.c_str());
		  // set the action state to preempted
		  staubli.ResetMotion();
		  as_.setPreempted();
		  success = false;
		  break;
	       }
	       if( polling(goal->j) ) break;
	       rate.sleep();
	    }
	    if(success) as_.setSucceeded(result_);
	 }else {
	    as_.setAborted();
	    ROS_ERROR("Cannot move to specified joints' configuration.");
	 }
      }

//executeCB implementation: this is a member method that will get registered with the action server
// argument type is very long.  Meaning:
// actionlib is the package for action servers
// SimpleActionServer is a templated class in this package (defined in the "actionlib" ROS package)
// <example_action_server::demoAction> customizes the simple action server to use our own "action" message 
// defined in our package, "example_action_server", in the subdirectory "action", called "demo.action"
// The name "demo" is prepended to other message types created automatically during compilation.
// e.g.,  "demoAction" is auto-generated from (our) base name "demo" and generic name "Action"
void ExampleActionServer::executeCB(const actionlib::SimpleActionServer<example_action_server::demoAction>::GoalConstPtr& goal) {
    //ROS_INFO("in executeCB");
    //ROS_INFO("goal input is: %d", goal->input);
    //do work here: this is where your interesting code goes
    
    //....

    // for illustration, populate the "result" message with two numbers:
    // the "input" is the message count, copied from goal->input (as sent by the client)
    // the "goal_stamp" is the server's count of how many goals it has serviced so far
    // if there is only one client, and if it is never restarted, then these two numbers SHOULD be identical...
    // unless some communication got dropped, indicating an error
    // send the result message back with the status of "success"

    g_count++; // keep track of total number of goals serviced since this server was started
    result_.output = g_count; // we'll use the member variable result_, defined in our class
    result_.goal_stamp = goal->input;
    
    // the class owns the action server, so we can use its member methods here
   
    // DEBUG: if client and server remain in sync, all is well--else whine and complain and quit
    // NOTE: this is NOT generically useful code; server should be happy to accept new clients at any time, and
    // no client should need to know how many goals the server has serviced to date
    if (g_count != goal->input) {
        ROS_WARN("hey--mismatch!");
        ROS_INFO("g_count = %d; goal_stamp = %d", g_count, result_.goal_stamp);
        g_count_failure = true; //set a flag to commit suicide
        ROS_WARN("informing client of aborted goal");
        as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
    }
    else {
         as_.setSucceeded(result_); // tell the client that we were successful acting on the request, and return the "result" message
    }
}

//executeCB implementation: this is a member method that will get registered with the action server
// argument type is very long.  Meaning:
// actionlib is the package for action servers
// SimpleActionServer is a templated class in this package (defined in the "actionlib" ROS package)
// <Action_Server::gazebo.action> customizes the simple action server to use our own "action" message 
// defined in our package, "Action_Server", in the subdirectory "action", called "Action.action"
// The name "Action" is prepended to other message types created automatically during compilation.
// e.g.,  "gazebo.action" is auto-generated from (our) base name "Action" and generic name "Action"
  void ExampleActionServer::executeCB(const actionlib::SimpleActionServer<Action_Server::gazebo.action>::GoalConstPtr& goal) {
    ROS_INFO("in executeCB");
    ROS_INFO("goal input is: %d", goal->input);
    //do work here: this is where your interesting code goes
    ros::Rate timer(1.0); // 1Hz timer
    countdown_val_ = goal->input;
    //implement a simple timer, which counts down from provided countdown_val to 0, in seconds
    while (countdown_val_>0) {
     ROS_INFO("countdown = %d",countdown_val_);
     
       // each iteration, check if cancellation has been ordered
     if (as_.isPreemptRequested()){	
      ROS_WARN("goal cancelled!");
      result_.output = countdown_val_;
          as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
          return; // done with callback
        }
        
 	   //if here, then goal is still valid; provide some feedback
 	   feedback_.fdbk = countdown_val_; // populate feedback message with current countdown value
 	   as_.publishFeedback(feedback_); // send feedback to the action client that requested this goal
       countdown_val_--; //decrement the timer countdown
       timer.sleep(); //wait 1 sec between loop iterations of this timer
     }
    //if we survive to here, then the goal was successfully accomplished; inform the client
    result_.output = countdown_val_; //value should be zero, if completed countdown
    as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
  }

void TaskActionServer::executeCB(const actionlib::SimpleActionServer<coordinator::ManipTaskAction>::GoalConstPtr& goal) {
    ROS_INFO("in executeCB: received manipulation task");
    ROS_INFO("object code is: %d", goal->object_code);
    ROS_INFO("perception_source is: %d", goal->perception_source);
    g_status_code = 0; //coordinator::ManipTaskFeedback::RECEIVED_NEW_TASK;
    g_action_code = 1; //start with perceptual processing
    //do work here: this is where your interesting code goes
    while ((g_status_code != SUCCESS)&&(g_status_code != ABORTED)) { //coordinator::ManipTaskResult::MANIP_SUCCESS) {
        feedback_.feedback_status = g_status_code;
        as_.publishFeedback(feedback_);
        //ros::Duration(0.1).sleep();
        ROS_INFO("executeCB: g_status_code = %d",g_status_code);
        // each iteration, check if cancellation has been ordered

        if (as_.isPreemptRequested()) {
            ROS_WARN("goal cancelled!");
            result_.manip_return_code = coordinator::ManipTaskResult::ABORTED;
            g_action_code = 0;
            g_status_code = 0;
            as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
            return; // done with callback
        }
        //here is where we step through states:
        switch (g_action_code) {

            case 1:  
                ROS_INFO("starting new task; should call object finder");
                g_status_code = 1; //
                ROS_INFO("executeCB: g_action_code, status_code = %d, %d",g_action_code,g_status_code);
                ros::Duration(2.0).sleep();
                g_action_code = 2;                
                break;

            case 2: // also do nothing...but maybe comment on status? set a watchdog?
                g_status_code = 2;
                ROS_INFO("executeCB: g_action_code, status_code = %d, %d",g_action_code,g_status_code);
                ros::Duration(2.0).sleep();
                g_action_code = 3;
                break;

            case 3:
                g_status_code = SUCCESS; //coordinator::ManipTaskResult::MANIP_SUCCESS; //code 0
                ROS_INFO("executeCB: g_action_code, status_code = %d, %d",g_action_code,g_status_code);
                g_action_code = 0; // back to waiting state--regardless
                break;
            default:
                ROS_WARN("executeCB: error--case not recognized");
                break;
        }        
    ros::Duration(0.5).sleep();    
        
    }
    ROS_INFO("executeCB: manip success; returning success");
        //if we survive to here, then the goal was successfully accomplished; inform the client
        result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
        as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
            g_action_code = 0;
            g_status_code = 0;
    return;
}

    void ControlDoneCB(const actionlib::SimpleClientGoalState& state, const oea_controller::controlPlatformResultConstPtr &result)
    {
        controlling_ = false;
        ROS_DEBUG_STREAM_NAMED(logger_name_, "Control Action finished: " << state.toString());

        move_result_.result_state = result->result_state;
        move_result_.error_string = result->error_string;

        if (move_result_.result_state)
        {
            as_.setSucceeded(move_result_);
            ROS_INFO_NAMED(logger_name_, "Goal was successful :)");
        }
        else
        {
             ROS_WARN_NAMED(logger_name_, "Goal was NOT successful :)");

            // if is preempted => as_ was already set, cannot set again
            if (state.toString() != "PREEMPTED")
            {
                as_.setAborted(move_result_);
                ROS_DEBUG_NAMED(logger_name_, "Goal was Aborted");
            }
            else
            {
                if (set_terminal_state_)
                {
                     as_.setPreempted(move_result_);
                     ROS_DEBUG_NAMED(logger_name_, "Goal was Preempted");
                }
            }
        }
    }

	//void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal) {
	void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {	
		if(isInitialized_) {	
			ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
			// saving goal into local variables
			traj_ = goal->trajectory;
			traj_point_nr_ = 0;
			traj_point_ = traj_.points[traj_point_nr_];
			GoalPos_ = traj_point_.positions[0];
			finished_ = false;
			
			// stoping axis to prepare for new trajectory
			CamAxis_->Stop();

			// check that preempt has not been requested by the client
			if (as_.isPreemptRequested())
			{
				ROS_INFO("%s: Preempted", action_name_.c_str());
				// set the action state to preempted
				as_.setPreempted();
			}
			
			usleep(2000000); // needed sleep until drive starts to change status from idle to moving
			
			while (not finished_)
			{
				if (as_.isNewGoalAvailable())
				{
					ROS_WARN("%s: Aborted", action_name_.c_str());
					as_.setAborted();
					return;
				}
		   		usleep(10000);
				//feedback_ = 
				//as_.send feedback_
			}

			// set the action state to succeed			
			//result_.result.data = "executing trajectory";
			ROS_INFO("%s: Succeeded", action_name_.c_str());
			// set the action state to succeeded
			as_.setSucceeded(result_);
		
		} else {
			as_.setAborted();
			ROS_WARN("Camera_axis not initialized yet!");
		}
	}

 void executeFollowTrajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) 
 {
     ROS_INFO("Received new goal trajectory with %lu points",goal->trajectory.points.size());
     spawnTrajector(goal->trajectory);
     // only set to succeeded if component could reach position. this is currently not the care for e.g. by emergency stop, hardware error or exceeds limit.
     if(rejected_)
         as_follow_.setAborted(); //setRejected not implemented in simpleactionserver ?
     else
     {
         if(failure_)
             as_follow_.setAborted();
         else
             as_follow_.setSucceeded();
     }
     rejected_ = false;
     failure_ = false;
 }

    /**
     * @brief Done callback for the move_base client, checks for errors and aborts exploration task if necessary
     * @param state State from the move_base client
     * @param result Result from the move_base client
     */
    void doneMovingCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result){

        if (state == actionlib::SimpleClientGoalState::ABORTED){
            ROS_ERROR("Failed to move");
            as_.setAborted();
        }else if(state == actionlib::SimpleClientGoalState::SUCCEEDED){
            moving_ = false;
        }

    }

    /***********************************Callback**************************************************/
    void goalCallback(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {
        std::vector<std::string> jointNames = goal->trajectory.joint_names;
        if (checkIfValid(jointNames)) {
            size_t commandSize = goal->trajectory.points.size();
            for (int i = 0; i < commandSize - 1; ++i) {
                sensor_msgs::JointState command;
                command.name = goal->trajectory.joint_names;
                command.position = goal->trajectory.points[i].positions;
                command.velocity = goal->trajectory.points[i].velocities;
                command.effort = goal->trajectory.points[i].effort;


                _jointCommand.publish(command);

                ROS_INFO_STREAM(goal->trajectory.points[i]);
                clrFeedback();
                _feedback.joint_names = jointNames;
                _feedback.desired.effort = goal->trajectory.points[i].effort;
                _feedback.desired.velocities = goal->trajectory.points[i].velocities;
                _feedback.desired.positions = goal->trajectory.points[i].positions;
                waitForExecution();

            }
            sensor_msgs::JointState command;
            command.name = goal->trajectory.joint_names;
            command.position = goal->trajectory.points[commandSize - 1].positions;

            for(int i = 0; i < command.name.size(); ++i)
            {
                command.velocity.push_back(0.2);
            }

            command.effort = goal->trajectory.points[commandSize - 1].effort;
            rosInfo("Last");
            _jointCommand.publish(command);

            clrFeedback();
            _feedback.joint_names = jointNames;
            _feedback.desired.effort = goal->trajectory.points[commandSize - 1].effort;
            _feedback.desired.velocities = goal->trajectory.points[commandSize - 1].velocities;
            _feedback.desired.positions = goal->trajectory.points[commandSize - 1].positions;
            waitForExecution();

            _result.error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
            _result.error_string = "Cool";
            _actionServer.setSucceeded(_result);
        }
        else {
            _result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
            _result.error_string = "Not cool";
            _actionServer.setAborted(_result);
        }
    }

    void planningDoneCB(const actionlib::SimpleClientGoalState& state, const oea_planner::planResultConstPtr &result)
    {
        planning_ = false;
        ROS_DEBUG_STREAM_NAMED(logger_name_, "Plan Action finished: " << state.toString());

        move_result_.result_state = result->result_state;

        if (move_result_.result_state) //if plan OK
        {
            planned_path_goal_.plan_goal = result->planned_path; // goal for the controller is result of the planner

            if (ctrl_state_sub.getNumPublishers()==0)
            {
                ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - Controller is down");
                controlling_ = false;
                move_result_.result_state = 0;
                move_result_.error_string = "Controller is down!!!";
                as_.setAborted(move_result_);
            }
            else
            {
                ac_control_.sendGoal(planned_path_goal_, boost::bind(&MovePlatformAction::ControlDoneCB, this, _1, _2),
                                     actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleActiveCallback(),
                                     actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleFeedbackCallback()); //boost::bind(&MovePlatformAction::ControlFeedbackCB, this,_1));
                controlling_ = true;
                ROS_DEBUG_STREAM_NAMED(logger_name_,"Goal #" << move_goal_.nav_goal.header.seq << " sent to Controller");
            }
        }
        else //if plan NOT OK
        {
            ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());

            move_result_.error_string = "Planning Failed: " + result->error_string;
            ROS_WARN_STREAM_NAMED(logger_name_, "Aborting because " << move_result_.error_string);

            as_.setAborted(move_result_);
        }
        return;

    }

 void goalCB()
 {
   // accept the new goal
   feedback_.forward_distance = 0.0;
   feedback_.turn_distance = 0.0;
   
   result_.forward_distance = 0.0;
   result_.turn_distance = 0.0;
   
   goal_ = as_.acceptNewGoal();
   
   if (!turnOdom(goal_->turn_distance))
   { 
     as_.setAborted(result_);
     return;
   }
   
   if (driveForwardOdom(goal_->forward_distance))
     as_.setSucceeded(result_);
   else
     as_.setAborted(result_);
 }

    void action_execute_stop_callback(const s8_motor_controller::StopGoalConstPtr & goal) {
        ROS_INFO("STOP");

        if(is_stopping) {
            ROS_FATAL("Stop action callback executed but is already stopping");
        }

        stop();
        is_stopping = true;

        const int timeout = 10; // 10 seconds.
        const int rate_hz = 10;

        ros::Rate rate(rate_hz);

        const int encoder_callback_ticks_treshold = 10;

        int ticks = 0;

        while(!is_still && ticks <= timeout * rate_hz && ticks_since_last_encoder_callback < encoder_callback_ticks_treshold) {
            rate.sleep();
            ticks++;
            ticks_since_last_encoder_callback++;
        }

        if(ticks_since_last_encoder_callback >= encoder_callback_ticks_treshold) {
            ROS_INFO("No encoder callback in %d ticks. Assuming still.", ticks_since_last_encoder_callback);
            is_still = true;
        }

        if(!is_still) {
            ROS_WARN("Unable to stop. Stop action failed.");
            s8_motor_controller::StopResult stop_action_result;
            stop_action_result.stopped = false;
            stop_action.setAborted(stop_action_result);
        } else {
            s8_motor_controller::StopResult stop_action_result;
            stop_action_result.stopped = true;
            stop_action.setSucceeded(stop_action_result);
            ROS_INFO("Stop action succeeded");
        }

        is_stopping = false;
        ignore_twist = true;
        ignored_twist_cnt = 0;
    }