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

//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 executeCB(const tilting_servo::servoGoalConstPtr &goal)
	{
		if(start == true)
		{
			servo_move(Comport, servo_id, goal->angle, goal->speed);
			prev_goal = goal->angle;
			start = false;
		}
		if(as_.isNewGoalAvailable())
			as_.acceptNewGoal();
		if(goal->angle != prev_goal || goal->speed != prev_speed)
		{
		servo_move(Comport, servo_id, goal->angle, goal->speed);
		prev_goal = goal->angle;
		prev_speed = goal->speed;
		}				
		while((as_.isNewGoalAvailable() == false) && ros::ok())
		{	
			feedback_.angle = read_angle(Comport, servo_id);
			if(feedback_.angle >= min_angle - 10 && feedback_.angle <= max_angle + 10)			
			as_.publishFeedback(feedback_);
		}
		
		// check that preempt has not been requested by the client
		if (as_.isPreemptRequested() || !ros::ok())
		{
			// set the action state to preempted
			as_.setPreempted();
		}


	}

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 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.");
	 }
      }

    void execute_callback(const behavior_tree_core::BTGoalConstPtr &goal)
    {
        // publish info to the console for the user
        ROS_INFO("Starting Action");

        // start executing the action
        int i = 0;
        while (i < 5)
        {
            // check that preempt has not been requested by the client
            if (as_.isPreemptRequested())
            {
                ROS_INFO("Action Halted");

                // set the action state to preempted
                as_.setPreempted();
                break;
            }
            ROS_INFO("Executing Action");

            ros::Duration(0.5).sleep();  // waiting for 0.5 seconds
            i++;
        }

        if (i == 5)
        {
            set_status(SUCCESS);
        }
    }

  void executeCB(const speedydug_servers::SpinBucketGoalConstPtr &goal)
  {
	  // helper variables
	  bucket_detected_ = false;
      ir_detected_ = false;
      feedback_.success = false;
	  float z;
	  ros::Duration d(0.05);

	  if( goal->left ){
		  z = ANG_VEL;
	  }
	  else {
		  z = -1.0 * ANG_VEL;
      }
	  ROS_INFO(goal->left ? "true" : "false");
	  ROS_INFO("z = %f", z);

	  while(!bucket_detected_ && !ir_detected_ )
	  {
		  // check that preempt has not been requested by the client
		  if (as_.isPreemptRequested() || !ros::ok())
		  {
			  twist_.angular.z = 0;
			  twist_.linear.x = 0;
			  twist_pub_.publish(twist_);
			  ROS_INFO("%s: Preempted", action_name_.c_str());
			  // set the action state to preempted
			  as_.setPreempted();
			  break;
		  }

		  twist_.angular.z = z;
		  twist_.linear.x = 0;
		  twist_pub_.publish(twist_);
		  as_.publishFeedback(feedback_);
		  d.sleep();
	  }

      if(bucket_detected_ )
      {
          twist_.angular.z = 0;
          twist_.linear.x = 0;
          twist_pub_.publish(twist_);
          result_.success = true;
          ROS_INFO("%s: Succeeded", action_name_.c_str());
          // set the action state to succeeded
          as_.setSucceeded(result_);
      } else if(ir_detected_) {
          twist_.angular.z = 0;
          twist_.linear.x = 0;
          twist_pub_.publish(twist_);
          result_.success = false;
          ROS_INFO("%s: Succeeded", action_name_.c_str());
          // set the action state to succeeded
          as_.setSucceeded(result_);
      }
  }

  void timer_callback(const ros::TimerEvent &) {
    if (!c3trajectory) return;

    ros::Time now = ros::Time::now();

    if (actionserver.isPreemptRequested()) {
      current_waypoint = c3trajectory->getCurrentPoint();
      current_waypoint.r.qdot = subjugator::Vector6d::Zero();  // zero velocities
      current_waypoint_t = now;

      // don't try to make output c3 continuous when cancelled - instead stop as quickly as possible
      c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
      c3trajectory_t = now;
    }

    if (actionserver.isNewGoalAvailable()) {
      boost::shared_ptr<const uf_common::MoveToGoal> goal = actionserver.acceptNewGoal();
      current_waypoint = subjugator::C3Trajectory::Waypoint(
          Point_from_PoseTwist(goal->posetwist.pose, goal->posetwist.twist), goal->speed,
          !goal->uncoordinated);
      current_waypoint_t = now;  // goal->header.stamp;
      this->linear_tolerance = goal->linear_tolerance;
      this->angular_tolerance = goal->angular_tolerance;
      c3trajectory_t = now;
    }

    while ((c3trajectory_t + traj_dt < now) and ros::ok()) {
      // ROS_INFO("Acting");

      c3trajectory->update(traj_dt.toSec(), current_waypoint,
                           (c3trajectory_t - current_waypoint_t).toSec());
      c3trajectory_t += traj_dt;
    }

    PoseTwistStamped msg;
    msg.header.stamp = c3trajectory_t;
    msg.header.frame_id = fixed_frame;
    msg.posetwist = PoseTwist_from_PointWithAcceleration(c3trajectory->getCurrentPoint());
    trajectory_pub.publish(msg);

    PoseStamped posemsg;
    posemsg.header.stamp = c3trajectory_t;
    posemsg.header.frame_id = fixed_frame;
    posemsg.pose = Pose_from_Waypoint(current_waypoint);
    waypoint_pose_pub.publish(posemsg);

    if (actionserver.isActive() &&
        c3trajectory->getCurrentPoint().is_approximately(
            current_waypoint.r, max(1e-3, linear_tolerance), max(1e-3, angular_tolerance)) &&
        current_waypoint.r.qdot == subjugator::Vector6d::Zero()) {
      actionserver.setSucceeded();
    }
  }

		/*!
		* \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 executeCB()
  void executeCB(const amazon_challenge_bt_actions::BTGoalConstPtr &goal)
  {
    // helper variables
    ros::Rate r(1);
    bool success = true;

    // publish info to the console for the user
    ROS_INFO("Starting Action");

    // start executing the action
    for(int i=1; i<=20; i++)
    {

    ROS_INFO("Executing Action");

    //motion_proxy_ptr->post.move(0.1, 0.0, 0.0);

      // check that preempt has not been requested by the client
      if (as_.isPreemptRequested() || !ros::ok())
      {
        ROS_INFO("Action Halted");


      // motion_proxy_ptr->stopMove();


        // set the action state to preempted
        as_.setPreempted();
        success = false;
        break;
      }
      feedback_.status = RUNNING;
      // publish the feedback
      as_.publishFeedback(feedback_);
      // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes

      r.sleep();
   }

    if(success)
    {
      result_.status = feedback_.status;
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // set the action state to succeeded
      as_.setSucceeded(result_);
    }
  }

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

//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("callback activated");
    double yaw_desired, yaw_current, travel_distance, spin_angle;
    nav_msgs::Path paths;
    geometry_msgs::Pose pose_desired;
    paths = goal->input;

    int npts = paths.poses.size();
    ROS_INFO("received path request with %d poses",npts);    

	int move = 0;
	for (int i=0;i<npts;i++) { //visit each subgoal
		// odd notation: drill down, access vector element, drill some more to get pose
		pose_desired = paths.poses[i].pose; //get first pose from vector of poses        
        	get_yaw_and_dist(g_current_pose, pose_desired,travel_distance, yaw_desired);
        	ROS_INFO("pose %d: desired yaw = %f; desired (x,y) = (%f,%f)",i,yaw_desired, 		 	
				pose_desired.position.x,pose_desired.position.y); 
        	ROS_INFO("current (x,y) = (%f, %f)",g_current_pose.position.x,g_current_pose.position.y);
        	ROS_INFO("travel distance = %f",travel_distance); 
		ROS_INFO("pose %d: desired yaw = %f",i,yaw_desired);        
		yaw_current = convertPlanarQuat2Phi(g_current_pose.orientation); //our current yaw--should use a sensor
		spin_angle = yaw_desired - yaw_current; // spin this much
		//spin_angle = min_spin(spin_angle);// but what if this angle is > pi?  then go the other way
		do_spin(spin_angle); // carry out this incremental action
		// we will just assume that this action was successful--really should have sensor feedback here
		g_current_pose.orientation = pose_desired.orientation; // assumes got to desired orientation precisely
		move = pose_desired.position.x;
		
		ROS_INFO("Move:  %d", move);
		do_move(move);
		
		if(as_.isPreemptRequested()){
		   do_halt();
		   ROS_WARN("Srv: goal canceled.");
		   result_.output = -1;
		   as_.setAborted(result_);
		   return;
		}
        }
	result_.output = 0;
	as_.setSucceeded(result_);
	ROS_INFO("Move finished.");
}

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);
    //do work here: this is where your interesting code goes
    feedback_.feedback_status = coordinator::ManipTaskFeedback::RECEIVED_NEW_TASK;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing
    //get pickup pose:
    feedback_.feedback_status = coordinator::ManipTaskFeedback::PERCEPTION_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing

    feedback_.feedback_status = coordinator::ManipTaskFeedback::PICKUP_PLANNING_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing

    feedback_.feedback_status = coordinator::ManipTaskFeedback::PICKUP_MOTION_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing    

    feedback_.feedback_status = coordinator::ManipTaskFeedback::DROPOFF_PLANNING_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing      

    feedback_.feedback_status = coordinator::ManipTaskFeedback::DROPOFF_MOTION_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing      


    // each iteration, check if cancellation has been ordered
    if (as_.isPreemptRequested()) {
        ROS_WARN("goal cancelled!");
        result_.manip_return_code = coordinator::ManipTaskResult::ABORTED;
        as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
        return; // done with callback
    }

    //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
}

	// Go to this pose
	bool go_home(tf::Pose& pose_) {


		tf::Vector3 start_p(pose_.getOrigin());
		tf::Quaternion start_o(pose_.getRotation());

		msg_pose.pose.position.x = start_p.x();
		msg_pose.pose.position.y = start_p.y();
		msg_pose.pose.position.z = start_p.z();
		msg_pose.pose.orientation.w = start_o.w();
		msg_pose.pose.orientation.x = start_o.x();
		msg_pose.pose.orientation.y = start_o.y();
		msg_pose.pose.orientation.z = start_o.z();
		pub_.publish(msg_pose);
		sendNormalForce(0);

		ros::Rate thread_rate(2);
		ROS_INFO("Homing...");
		while(ros::ok()) {
			double oerr =(ee_pose.getRotation() - start_o).length() ;
			double perr = (ee_pose.getOrigin() - start_p).length();
			feedback_.progress = 0.5*(perr+oerr);
			as_.publishFeedback(feedback_);
			ROS_INFO_STREAM("Error: "<<perr<<", "<<oerr);
			if(perr< 0.02 && oerr < 0.02) {
				break;
			}
			if (as_.isPreemptRequested() || !ros::ok() )
			{
				sendNormalForce(0);
				sendPose(ee_pose);
				ROS_INFO("%s: Preempted", action_name_.c_str());
				// set the action state to preempted
				as_.setPreempted();
				return false;
			}
			thread_rate.sleep();
		}
		return ros::ok();

	}

    // move platform server receives a new goal
    void moveGoalCB()
    {
        set_terminal_state_ = true;

        move_goal_.nav_goal = as_.acceptNewGoal()->nav_goal;
        ROS_INFO_STREAM_NAMED(logger_name_, "Received Goal #" <<move_goal_.nav_goal.header.seq);

        if (as_.isPreemptRequested() ||!ros::ok())
        {
            ROS_WARN_STREAM_NAMED(logger_name_, "Preempt Requested on goal #" << move_goal_.nav_goal.header.seq);
            if (planning_)
                ac_planner_.cancelGoalsAtAndBeforeTime(ros::Time::now());
            if (controlling_)
                ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
            move_result_.result_state = 0;
            move_result_.error_string = "Preempt Requested!!!";
            as_.setPreempted(move_result_);
            return;
        }

        // Convert move goal to plan goal
        pose_goal_.pose_goal = move_goal_.nav_goal;

        if (planner_state_sub.getNumPublishers()==0)
        {
            ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - planner is down");
            planning_ = false;
            move_result_.result_state = 0;
            move_result_.error_string = "Planner is down";
            as_.setAborted(move_result_);
        }
        else
        {
            ac_planner_.sendGoal(pose_goal_, boost::bind(&MovePlatformAction::planningDoneCB, this, _1, _2),
                                 actionlib::SimpleActionClient<oea_planner::planAction>::SimpleActiveCallback(),
                                 actionlib::SimpleActionClient<oea_planner::planAction>::SimpleFeedbackCallback());
            planning_ = true;
            ROS_DEBUG_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " sent to planner");
        }
        return;
    }

    void executeCB(const learning_actionlib::FibonacciGoalConstPtr &goal)
    {
        // helper variables
        ros::Rate r(1);
        bool success = true;

        // push_back the seeds for the fibonacci sequence
        feedback_.sequence.clear();
        feedback_.sequence.push_back(0);
        feedback_.sequence.push_back(1);

        // publish info to the console for the user
        ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);

        // start executing the action
        for(int i=1; i<=goal->order; i++)
        {
            // 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;
                break;
            }
            feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
            // publish the feedback
            as_.publishFeedback(feedback_);
            // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
            r.sleep();
        }

        if(success)
        {
            result_.sequence = feedback_.sequence;
            ROS_INFO("%s: Succeeded", action_name_.c_str());
            // set the action state to succeeded
            as_.setSucceeded(result_);
        }
    }