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

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 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();
		}


	}

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

	// Go down until hit the table. For safety min_height is specified. If no table found until this height, returns false.
	// vertical_speed with which to move downwards
	// thr_force - normal force threshold at which table is assumed to be detected
	bool find_table_for_rolling(double min_height, double vertical_speed, double thr_force) {
		double rate = 200;
		thr_force = fabs(thr_force);
		ros::Rate thread_rate(rate);

		double startz = ee_pose.getOrigin().z();

		msg_pose.pose.position.x = ee_pose.getOrigin().x();
		msg_pose.pose.position.y = ee_pose.getOrigin().y();
		msg_pose.pose.position.z = startz;
		msg_pose.pose.orientation.x = ee_pose.getRotation().x();
		msg_pose.pose.orientation.y = ee_pose.getRotation().y();
		msg_pose.pose.orientation.z = ee_pose.getRotation().z();
		msg_pose.pose.orientation.w = ee_pose.getRotation().w();

		// Publish attractors if running in simulation or with fixed values
		if ((action_mode == ACTION_LASA_FIXED) || (action_mode == ACTION_BOXY_FIXED)) {
		    static tf::TransformBroadcaster br;
			tf::Transform  table;
		    table.setOrigin(tf::Vector3 (ee_pose.getOrigin().x(),ee_pose.getOrigin().y(),ee_pose.getOrigin().z() - min_height));
		    table.setRotation(tf::Quaternion (ee_pose.getRotation().x(),ee_pose.getRotation().y(),ee_pose.getRotation().z(),ee_pose.getRotation().w()));
		    br.sendTransform(tf::StampedTransform(table, ros::Time::now(), world_frame, "/attractor"));
		}

		ROS_INFO_STREAM("Finding table up to max dist. "<<min_height<<" with vertical speed "<<vertical_speed<<" and threshold force "<<thr_force<<"N.");
		while(ros::ok()) {
			msg_pose.pose.position.z = msg_pose.pose.position.z - vertical_speed/rate;
			pub_.publish(msg_pose);

			// Go down until force reaches the threshold
			if(fabs(ee_ft[2]) > thr_force) {
				break;
			}
			if(fabs(ee_pose.getOrigin().z()-startz) > min_height) {
				ROS_INFO("Max distance reached");
				return false;
			}
			thread_rate.sleep();
			feedback_.progress = ee_ft[2];
			as_.publishFeedback(feedback_);
		}
		if(!ros::ok()) {
			return false;
		}
		tf::Vector3 table(ee_pose.getOrigin());
		ROS_INFO_STREAM("Table found at height "<<table[2]);
		msg_pose.pose.position.z = table[2];

		pub_.publish(msg_pose);
		sendAndWaitForNormalForce(0);


		return true;
	}

      bool polling( const std::vector<double> &j1 ) {
	 std::vector<double> j2;
	 j2.resize(6);
	 if(staubli.GetRobotJoints(j2)){
	    feedback_.j = j2;
	    as_.publishFeedback(feedback_);
	    double error = fabs(j1[0]-j2[0])+ fabs(j1[1]-j2[1])+ fabs(j1[2]-j2[2])+
	       fabs(j1[3]-j2[3])+ fabs(j1[4]-j2[4])+ fabs(j1[5]-j2[5]);
	    //	    ROS_INFO( "Error to target %lf", error );
	    return error < ERROR_EPSILON || staubli.IsJointQueueEmpty();
	 }else {
	    ROS_ERROR("Error when determining end of movement.");
	    return false;
	 }
      }

  //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 GoToSelectedBall::executeCB(const scheduler::SchedulerGoalConstPtr &goal){
	ROS_INFO("enter executeCB, goal = %i", goal->value);

	if(goal->value == 0){
		state_ = STOP;
	}
	else if(goal->value == 1){
		state_ = FIRST_STEP_COLLECT;

	}
	else if(goal->value == 2){
		// TODO: sprawdza, czy jest ustawiona pozycja pileczki, albo przesylac ja razem z goalem
		// TODO: dopisac serwer do jazdy do przodu a nie na sleep tak jak teraz
		state_ = SECOND_STEP_COLLECT;
		goForward(0);
		ROS_INFO("enter SECOND_STEP_COLLECT");
		publishAngle();
		ac.waitForResult();


		float dist = getDistanceFromSelectedBall();
		onHoover();
		goForward(dist -0.3);

		ros::Duration(4.0).sleep();

		goForward(-(dist-0.3));
		ros::Duration(5.0).sleep();

		goForward(0);
		
		offHoover();
		ROS_INFO("leave SECOND_STEP_COLLECT");
	}


	as_.publishFeedback(feedback_);
	result_.value = feedback_.value;

	as_.setSucceeded(result_);
	ROS_INFO("leave executeCB");
}

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

	// 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();

	}

	// Roll with "force" and horizontal "speed" until the length "range"
	bool rolling(double force, double speed, double range) {

		ROS_INFO_STREAM("Rolling with force "<<force<<", speed "<<speed<<", range "<<range);
		force = fabs(force);

		sendNormalForce(-force);
		msg_pose.pose.position.x  = ee_pose.getOrigin().x();
		msg_pose.pose.position.y  = ee_pose.getOrigin().y();
		msg_pose.pose.position.z  = ee_pose.getOrigin().z();

		tf::Quaternion q = ee_pose.getRotation();
		msg_pose.pose.orientation.x = q.x();
		msg_pose.pose.orientation.y = q.y();
		msg_pose.pose.orientation.z = q.z();
		msg_pose.pose.orientation.w = q.w();

		double center = ee_pose.getOrigin().y();
		double rate = 200;
		ros::Rate thread_rate(rate);
		int count=0;
		while(ros::ok()) {
			msg_pose.pose.position.y = msg_pose.pose.position.y + speed/rate;
			feedback_.progress = msg_pose.pose.position.y;
			as_.publishFeedback(feedback_);
			if( fabs(msg_pose.pose.position.y - center) >  range) {
				ROS_INFO("Change direction");
				speed *= -1;
				if(++count > 5) {
					break;
				}
			}
			pub_.publish(msg_pose);
			thread_rate.sleep();
		}

		msg_pose.pose.position.z = ee_pose.getOrigin().z() + 0.15;
		pub_.publish(msg_pose);
		sendNormalForce(0);

		return true;
	}

    void jointStateCallback(const sensor_msgs::JointState::ConstPtr &msg) {
        size_t msgSize = msg->name.size(), jointInfoSize = _jointInfo.name.size(), msgSizeP = msg->position.size(),
                msgSizeV = msg->velocity.size(), msgSizeE = msg->effort.size();
        for (int i = 0; i < msgSize; i++) {
            for (int j = 0; j < jointInfoSize; j++) {
                if (msg->name[i] == _jointInfo.name[j]) {
                    if (i < msgSizeP) _jointInfo.position[j] = msg->position[i];
                    if (i < msgSizeV) _jointInfo.velocity[j] = msg->velocity[i];
                    if (i < msgSizeE) _jointInfo.effort[j] = msg->effort[i];
                }
            }
        }

        if(_publishFeedback) {
            size_t size = _feedback.joint_names.size();

            _feedback.actual.positions.clear();
            _feedback.actual.velocities.clear();
            _feedback.actual.effort.clear();

            _feedback.error.positions.clear();
            _feedback.error.velocities.clear();
            _feedback.error.effort.clear();

            for (int i = 0; i < size; ++i) {
                feedback_t singleJointFeedback = getFeedback(_feedback.joint_names[i], _feedback.desired.positions[i],
                                                             0.0,
                                                             _feedback.desired.velocities[i]);
                _feedback.actual.positions.push_back(singleJointFeedback.actual.position);
                _feedback.actual.velocities.push_back(singleJointFeedback.actual.velocity);
                _feedback.actual.effort.push_back(singleJointFeedback.actual.effort);

                _feedback.error.positions.push_back(singleJointFeedback.error.position);
                _feedback.error.velocities.push_back(singleJointFeedback.error.velocity);
                _feedback.error.effort.push_back(singleJointFeedback.error.effort);
            }
            _actionServer.publishFeedback(_feedback);
        }
    }

    // returns the status to the client (Behavior Tree)
    void set_status(int status)
    {
        // Set The feedback and result of BT.action
        feedback_.status = status;
        result_.status = feedback_.status;
        // publish the feedback
        as_.publishFeedback(feedback_);
        // setSucceeded means that it has finished the action (it has returned SUCCESS or FAILURE).
        as_.setSucceeded(result_);

        switch (status)  // Print for convenience
        {
        case SUCCESS:
            ROS_INFO("Action %s Succeeded", ros::this_node::getName().c_str() );
            break;
        case FAILURE:
            ROS_INFO("Action %s Failed", ros::this_node::getName().c_str() );
            break;
        default:
            break;
        }
    }

//	metoda jest uruchamiana w osobnym watku i nie blokuje calbackow
void Explore::executeCB(const scheduler::SchedulerGoalConstPtr &goal){
	ROS_INFO("enter executeCB, goal = %i", goal->value);

	if(goal->value == 1){
		explore_ = true;

	}
	else if(goal->value == 0){
		//	koniec ekslporacji
		stopExplore();
		explore_state_ = STOP;
		explore_ = false;
	}

	feedback_.value = 0;

	as_.publishFeedback(feedback_);
	result_.value = feedback_.value;

	as_.setSucceeded(result_);
	ROS_INFO("leave executeCB");

}

  void executeCB(const learning_actionlib::FibonacciGoalConstPtr& goal) {
    ros::Rate r(1);
    bool success = true;
    feedback_.sequence.clear();
    feedback_.sequence.push_back(0);
    feedback_.sequence.push_back(1);

    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]);

    for (int i = 1; i <= goal->order; i++) {
      if (as_.isPreemptRequested() || !ros::ok()) {
        ROS_INFO("%s: Preempted", action_name_.c_str());

        as_.setPreempted();
        success = false;
        break;
      }

      feedback_.sequence.push_back(feedback_.sequence[i] +
                                   feedback_.sequence[i - 1]);

      as_.publishFeedback(feedback_);

      r.sleep();
    }

    if (success) {
      result_.sequence = feedback_.sequence;
      ROS_INFO("%s: Succeeded", action_name_.c_str());

      as_.setSucceeded(result_);
    }
  }