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

  void watchdog(const ros::TimerEvent &e)
  {
    ros::Time now = ros::Time::now();

    // Aborts the active goal if the controller does not appear to be active.
    if (has_active_goal_)
    {
      bool should_abort = false;
      if (!last_controller_state_)
      {
        should_abort = true;
        ROS_WARN("Aborting goal because we have never heard a controller state message.");
      }
      else if ((now - last_controller_state_->stamp) > ros::Duration(5.0))
      {
        should_abort = true;
        ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds",
                 (now - last_controller_state_->stamp).toSec());
      }

      if (should_abort)
      {
        // Marks the current goal as aborted.
        active_goal_.setAborted();
        has_active_goal_ = false;
      }
    }
  }

void ServerGoalHandleDestructionTester::goalCallback(GoalHandle gh)
{
  ROS_ERROR_NAMED("actionlib", "In callback");
  //assign to our stored goal handle
  *gh_ = gh;

  TestGoal goal = *gh.getGoal();

  switch (goal.goal)
  {
    case 1:
      gh.setAccepted();
      gh.setSucceeded(TestResult(), "The ref server has succeeded");
      break;
    case 2:
      gh.setAccepted();
      gh.setAborted(TestResult(), "The ref server has aborted");
      break;
    case 3:
      gh.setRejected(TestResult(), "The ref server has rejected");
      break;
    default:
      break;
  }

  ros::shutdown();
}

  void controllerStateCB(const pr2_controllers_msgs::JointControllerStateConstPtr &msg)
  {
    last_controller_state_ = msg;
    ros::Time now = ros::Time::now();

    if (!has_active_goal_)
      return;

    // grab the goal threshold off the param server
    if(!node_.getParam("position_servo_position_tolerance", goal_threshold_))
        ROS_ERROR("No position_servo_position_tolerance given in namespace: '%s')", node_.getNamespace().c_str());


    // Ensures that the controller is tracking my setpoint.
    if (fabs(msg->set_point - active_goal_.getGoal()->command.position) > stall_velocity_threshold_)
    {
      if (now - goal_received_ < ros::Duration(1.0))
      {
        return;
      }
      else
      {
        ROS_ERROR("Cancelling goal: Controller is trying to achieve a different setpoint.");
        active_goal_.setCanceled();
        has_active_goal_ = false;
      }
    }


    pr2_controllers_msgs::Pr2GripperCommandFeedback feedback;
    feedback.position = msg->process_value;
    feedback.effort = msg->command;
    feedback.reached_goal = false;
    feedback.stalled = false;

    pr2_controllers_msgs::Pr2GripperCommandResult result;
    result.position = msg->process_value;
    result.effort = msg->command;
    result.reached_goal = false;
    result.stalled = false;

    // check if we've reached the goal
    if (fabs(msg->process_value - active_goal_.getGoal()->command.position) < goal_threshold_)
    {
      feedback.reached_goal = true;

      result.reached_goal = true;
      active_goal_.setSucceeded(result);
      has_active_goal_ = false;
    }
    else
    {
      // Determines if the gripper has stalled.
      if (fabs(msg->process_value_dot) > stall_velocity_threshold_)
      {
        last_movement_time_ = ros::Time::now();
      }
      else if ((ros::Time::now() - last_movement_time_).toSec() > stall_timeout_ &&
               active_goal_.getGoal()->command.max_effort != 0.0)
      {
        feedback.stalled = true;

        result.stalled = true;
        active_goal_.setAborted(result);
        has_active_goal_ = false;
      }
    }
    active_goal_.publishFeedback(feedback);
  }