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

	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 Explore::goalCB(){
//	explore_ = true;
    // accept the new goal
    //goal_ =
	explore_state_ = FULL_ROTATE;
	as_.acceptNewGoal();
  }

  // Action server sends goals here
  void goalCB()
  {
    ROS_INFO_STREAM_NAMED("pick_place_moveit","Received goal -----------------------------------------------");

    pick_place_goal_ = action_server_.acceptNewGoal();
    base_link_ = pick_place_goal_->frame;

    processGoal(pick_place_goal_->pickup_pose, pick_place_goal_->place_pose);
  }

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

 void goalCB()
 {
   ROS_INFO("[pick and place] Received goal!");
   goal_ = as_.acceptNewGoal();
   arm_link = goal_->frame;
   gripper_open = goal_->gripper_open;
   gripper_closed = goal_->gripper_closed;
   z_up = goal_->z_up;
   
   if (goal_->topic.length() < 1)
     pickAndPlace(goal_->pickup_pose, goal_->place_pose);
   else
     pick_and_place_sub_ = nh_.subscribe(goal_->topic, 1, &PickAndPlaceServer::sendGoalFromTopic, this);
 }

  void GoalCB() {
    ROS_INFO("%s: received new goal.", action_name_.c_str());
    
    blobTracker.clear();
    comps.clear();
    hierarchy.clear();
    contours.clear();
    numCC.clear();
    
    // accept the new goal
    goal_ = *server_.acceptNewGoal();
    
    if (goal_.numSearchGoals <= 0)
      return;

    num_feature_goals_ = goal_.numSearchGoals;
    feature_mean_goals_.clear();
    feature_var_goals_.clear();
    feature_mean_goals_.resize(num_feature_goals_);
    feature_var_goals_.resize(num_feature_goals_);
    numCC.resize(num_feature_goals_);

    comps = vector<vector<ConnectedComp> >();
    contours = vector<vector<vector<Point> > >();
    hierarchy = vector<vector<Vec4i> >();
    comps.resize(num_feature_goals_);
    contours.resize(num_feature_goals_);
    hierarchy.resize(num_feature_goals_);
    blobTracker.resize(num_feature_goals_);

    for(int i=0; i<num_feature_goals_; i++)
      {
	comps[i].resize(maxNumComponents);
	blobTracker[i] = FeatureBlobTracker(maxNumComponents, width, height);
	blobTracker[i].initialize();
	feature_mean_goals_[i] = goal_.signal[i].mean[0];
	feature_var_goals_[i] = goal_.signal[i].variance[0];
      }
    
    got_goals = true;
    ROS_INFO("%d features for color feature search received.", num_feature_goals_);
  }

    // 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 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 timer_callback(const ros::TimerEvent &)
  {
    mil_msgs::MoveToResult actionresult;

    // Handle disabled, killed, or no odom before attempting to produce trajectory
    std::string err = "";
    if (disabled)
      err = "c3 disabled";
    else if (kill_listener.isRaised())
      err = "killed";
    else if (!c3trajectory)
      err = "no odom";

    if (!err.empty())
    {
      if (c3trajectory)
        c3trajectory.reset();  // On revive/enable, wait for odom before station keeping

      // Cancel all goals while killed/disabled/no odom
      if (actionserver.isNewGoalAvailable())
        actionserver.acceptNewGoal();
      if (actionserver.isActive())
      {
        actionresult.error = err;
        actionresult.success = false;
        actionserver.setAborted(actionresult);
      }
      return;
    }

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

    auto old_waypoint = current_waypoint;

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

      waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::GREEN);

      // Check if waypoint is valid
      std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult = waypoint_validity_.is_waypoint_valid(
          Pose_from_Waypoint(current_waypoint), current_waypoint.do_waypoint_validation);
      actionresult.error = WAYPOINT_ERROR_TO_STRING.at(checkWPResult.second);
      actionresult.success = checkWPResult.first;
      if (checkWPResult.first == false && waypoint_check_)  // got a point that we should not move to
      {
        waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::RED);
        if (checkWPResult.second ==
            WAYPOINT_ERROR_TYPE::UNKNOWN)  // if unknown, check if there's a huge displacement with the new waypoint
        {
          auto a_point = Pose_from_Waypoint(current_waypoint);
          auto b_point = Pose_from_Waypoint(old_waypoint);
          // If moved more than .5m, then don't allow
          if (abs(a_point.position.x - b_point.position.x) > .5 || abs(a_point.position.y - b_point.position.y) > .5)
          {
            ROS_ERROR("can't move there! - need to rotate");
            current_waypoint = old_waypoint;
          }
        }
        // if point is occupied, reject move
        if (checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED)
        {
          ROS_ERROR("can't move there! - waypoint is occupied");
          current_waypoint = old_waypoint;
        }
        // if point is above water, reject move
        if (checkWPResult.second == WAYPOINT_ERROR_TYPE::ABOVE_WATER)
        {
          ROS_ERROR("can't move there! - waypoint is above water");
          current_waypoint = old_waypoint;
        }
        if (checkWPResult.second == WAYPOINT_ERROR_TYPE::NO_OGRID)
        {
          ROS_ERROR("WaypointValidity - Did not recieve any ogrid");
        }
      }
    }
    if (actionserver.isPreemptRequested())
    {
      current_waypoint = c3trajectory->getCurrentPoint();
      current_waypoint.do_waypoint_validation = false;
      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;
    }

    // Remember the previous trajectory
    auto old_trajectory = c3trajectory->getCurrentPoint();

    while (c3trajectory_t + traj_dt < now)
//.........这里部分代码省略.........

 void goalCB()
 {
   // accept the new goal
   has_goal_ = as_.acceptNewGoal()->approach;
 }

  void goalCB()
  {
    // ---------------------------------------------------------------------------------------------
    // Accept the new goal
    goal_ = action_server_.acceptNewGoal();

    ROS_INFO("[block logic] Received goal! %f, %s", goal_->block_size, goal_->frame.c_str());

    block_size = goal_->block_size;
    arm_link = goal_->frame;

    if (initialized_)
    {
      addBlocks(msg_);
    }

    // --------------------------------------------------------------------------------------------
    // Start pose - choose one that is preferrably not in the goal region
    geometry_msgs::Pose start_pose;
    bool found_pose = false;

    // Check if there is only 1 block detected
    if( !msg_->poses.size() )
    {
      // no blocks, what to do?
      ROS_WARN("[block logic] No blocks found");
    }
    else if( msg_->poses.size() == 1 )
    {
      start_pose = msg_->poses[0];
      found_pose = true;
      ROS_INFO("[block logic] Only 1 block, using it");
    }
    else
    {
      // Search for block that meets our criteria
      for(int i = 0; i < msg_->poses.size(); ++i)
      {
        if( msg_->poses[i].position.y > -0.12 && msg_->poses[i].position.y < 0.2 ) // start of goal region
        {
          start_pose = msg_->poses[i];
          found_pose = true;
          ROS_INFO_STREAM("[block logic] Chose this block:\n" << start_pose);
          break;
        }
      }
      if( !found_pose )
      {
        ROS_INFO("[block logic] Did not find a good block, default to first");
        start_pose = msg_->poses[0];
        found_pose = true;
      }
    }

    // --------------------------------------------------------------------------------------------
    // End pose is just chosen place on board
    geometry_msgs::Pose end_pose;
    end_pose.orientation = msg_->poses[0].orientation; // keep the same orientation
    end_pose.position.x = 0.225;
    end_pose.position.y = 0.18;
    end_pose.position.z = msg_->poses[0].position.z;

    // --------------------------------------------------------------------------------------------
    // Move that block
    if( found_pose )
    {
      moveBlock(start_pose, end_pose);
    }
  }

void Explore::goalCB(){

	explore_state_ = FULL_ROTATE;
	as_.acceptNewGoal();
  }