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

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 goalCB(GoalHandle gh)
  {
    // Ensures that the joints in the goal match the joints we are commanding.
    ROS_DEBUG("Received goal: goalCB");
    ROS_INFO("Received goal: goalCB");
    if (!is_joint_set_ok(joint_names_, gh.getGoal()->trajectory.joint_names))
    {
      ROS_ERROR("Joints on incoming goal don't match our joints");
      gh.setRejected();
      return;
    }

    // Cancels the currently active goal.
    if (has_active_goal_)
    {
      ROS_DEBUG("Received new goal, canceling current goal");
      // Stops the controller.
      trajectory_msgs::JointTrajectory empty;
      empty.joint_names = joint_names_;
      pub_controller_command_.publish(empty);

      // Marks the current goal as canceled.
      active_goal_.setCanceled();
      has_active_goal_ = false;
    }

    gh.setAccepted();
    active_goal_ = gh;
    has_active_goal_ = true;

    // Sends the trajectory along to the controller
    ROS_INFO("Publishing trajectory; sending commands to the joint controller ");
    current_traj_ = active_goal_.getGoal()->trajectory;
    pub_controller_command_.publish(current_traj_);
  }

void BaseTrajectoryActionController::goalCB(GoalHandle gh)
{
  ROS_DEBUG("goalCB");

  std::vector<std::string> joint_names(joints_.size());
  for (size_t j = 0; j < joints_.size(); ++j)
    joint_names[j] = joints_[j]->name;

  // Ensures that the joints in the goal match the joints we are commanding.
  if (!setsEqual(joint_names, gh.getGoal()->trajectory.joint_names))
  {
    ROS_ERROR("Joints on incoming goal don't match our joints");
    gh.setRejected();
    return;
  }

  // Cancels the currently active goal.
  if (active_goal_)
  {
    // Marks the current goal as canceled.
    active_goal_->setCanceled();
  }

  starting(); // reset trajectory

  gh.setAccepted();

  // Sends the trajectory along to the controller
  active_goal_ = boost::shared_ptr<GoalHandle>(new GoalHandle(gh));
  commandTrajectory(share_member(gh.getGoal(),gh.getGoal()->trajectory), active_goal_);
}

void CartesianImpedanceAction::goalCB(GoalHandle gh) {
  // cancel active goal
  if (active_goal_.isValid() && (active_goal_.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)) {
    active_goal_.setCanceled();
  }

  Goal g = gh.getGoal();

  bool valid = true;

  for (size_t i = 0; i < g->trajectory.points.size(); i++) {
    valid = valid && checkImpedance(g->trajectory.points[i].impedance);
  }

  if (!valid) {
    cartesian_trajectory_msgs::CartesianImpedanceResult res;
    res.error_code = cartesian_trajectory_msgs::CartesianImpedanceResult::INVALID_GOAL;
    gh.setRejected(res);
  }

  CartesianImpedanceTrajectory* trj_ptr =  new CartesianImpedanceTrajectory;
  *trj_ptr = g->trajectory;
  CartesianImpedanceTrajectoryConstPtr trj_cptr = CartesianImpedanceTrajectoryConstPtr(trj_ptr);

  if (g->trajectory.header.stamp < rtt_rosclock::host_now()) {
    valid = false;
    cartesian_trajectory_msgs::CartesianImpedanceResult res;
    res.error_code = cartesian_trajectory_msgs::CartesianImpedanceResult::OLD_HEADER_TIMESTAMP;
    gh.setRejected(res);
  }

  if (valid) {
    port_cartesian_trajectory_command_.write(trj_cptr);

    gh.setAccepted();
    active_goal_ = gh;
  }
}

  void goalCB(GoalHandle gh)
  {
    // Ensures that the joints in the goal match the joints we are commanding.
    ROS_DEBUG("Received goal: goalCB");
    if (!setsEqual(joint_names_, gh.getGoal()->trajectory.joint_names))
    {
      ROS_ERROR("Joints on incoming goal don't match our joints");
      gh.setRejected();
      return;
    }

    // Cancels the currently active goal.
    if (has_active_goal_)
    {
      ROS_DEBUG("Received new goal, canceling current goal");
      // Stops the controller.
      trajectory_msgs::JointTrajectory empty;
      empty.joint_names = joint_names_;
      pub_controller_command_.publish(empty);

      // Marks the current goal as canceled.
      active_goal_.setCanceled();
      has_active_goal_ = false;
    }

    // Sends the trajectory along to the controller
    if (withinGoalConstraints(last_controller_state_, goal_constraints_, gh.getGoal()->trajectory))
    {
      ROS_INFO_STREAM("Already within goal constraints");
      gh.setAccepted();
      gh.setSucceeded();
    }
    else
    {
      gh.setAccepted();
      active_goal_ = gh;
      has_active_goal_ = true;

      ROS_INFO("Publishing trajectory");

      current_traj_ = active_goal_.getGoal()->trajectory;
      pub_controller_command_.publish(current_traj_);
    }
  }

 void cancelCB(GoalHandle gh)
 {
   ROS_ERROR("Action cancel not supported for grasp execution action");
   gh.setRejected();
 }

  void goalCB(GoalHandle gh)
  {
    GripperMessage gMsg;
    SimpleMessage request;
    SimpleMessage reply;

    ROS_DEBUG("Received grasping goal");

    while (!(robot_->isConnected()))
    {
      ROS_DEBUG("Reconnecting");
      robot_->makeConnect();
    }
      

    switch(gh.getGoal()->goal)
    {
      case GraspHandPostureExecutionGoal::PRE_GRASP:

        gh.setAccepted();
        ROS_WARN("Pre-grasp is not supported by this gripper");
        gh.setSucceeded();
        break;

      case GraspHandPostureExecutionGoal::GRASP:
      case GraspHandPostureExecutionGoal::RELEASE:

        gh.setAccepted();
        switch(gh.getGoal()->goal)
        {
          case GraspHandPostureExecutionGoal::GRASP:
            ROS_INFO("Executing a gripper grasp");
            gMsg.init(GripperOperationTypes::CLOSE);
            break;
          case GraspHandPostureExecutionGoal::RELEASE:
            ROS_INFO("Executing a gripper release");
            gMsg.init(GripperOperationTypes::OPEN);
            break;
        }
        gMsg.toRequest(request);
        this->robot_->sendAndReceiveMsg(request, reply);

        switch(reply.getReplyCode())
        {
          case ReplyTypes::SUCCESS:
            ROS_INFO("Robot gripper returned success");
            gh.setSucceeded();
            break;
          case ReplyTypes::FAILURE:
            ROS_ERROR("Robot gripper returned failure");
            gh.setCanceled();
            break;
        }
        break;

          default:
            gh.setRejected();
            break;

    }
  }

void InternalSpaceSplineTrajectoryAction::goalCB(GoalHandle gh) {
  if (!goal_active_) {
    trajectory_msgs::JointTrajectory* trj_ptr =
        new trajectory_msgs::JointTrajectory;
    Goal g = gh.getGoal();

    control_msgs::FollowJointTrajectoryResult res;

    RTT::Logger::log(RTT::Logger::Debug) << "Received trajectory contain "
                                         << g->trajectory.points.size()
                                         << " points" << RTT::endlog();

    // fill remap table
    for (unsigned int i = 0; i < numberOfJoints_; i++) {
      int jointId = -1;
      for (unsigned int j = 0; j < g->trajectory.joint_names.size(); j++) {
        if (g->trajectory.joint_names[j] == jointNames_[i]) {
          jointId = j;
          break;
        }
      }
      if (jointId < 0) {
        RTT::Logger::log(RTT::Logger::Error)
            << "Trajectory contains invalid joint" << RTT::endlog();
        res.error_code =
            control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
        gh.setRejected(res, "");
        return;
      } else {
        remapTable_[i] = jointId;
      }
    }

    // Sprawdzenie ograniczeń w jointach INVALID_GOAL
    bool invalid_goal = false;
    for (unsigned int i = 0; i < numberOfJoints_; i++) {
      for (int j = 0; j < g->trajectory.points.size(); j++) {
        if (g->trajectory.points[j].positions[i] > upperLimits_[remapTable_[i]]
            || g->trajectory.points[j].positions[i]
                < lowerLimits_[remapTable_[i]]) {
          RTT::Logger::log(RTT::Logger::Debug)
              << "Invalid goal [" << i << "]: " << upperLimits_[remapTable_[i]]
              << ">" << g->trajectory.points[j].positions[i] << ">"
              << lowerLimits_[remapTable_[i]] << RTT::endlog();
          invalid_goal = true;
        }
      }
    }

    if (invalid_goal) {
      RTT::Logger::log(RTT::Logger::Debug)
          << "Trajectory contains invalid goal!" << RTT::endlog();
      res.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
      gh.setRejected(res, "");
      goal_active_ = false;
      return;
    }

    // Remap joints
    trj_ptr->header = g->trajectory.header;
    trj_ptr->points.resize(g->trajectory.points.size());

    for (unsigned int i = 0; i < g->trajectory.points.size(); i++) {
      trj_ptr->points[i].positions.resize(
          g->trajectory.points[i].positions.size());
      for (unsigned int j = 0; j < g->trajectory.points[i].positions.size();
          j++) {
        trj_ptr->points[i].positions[j] =
            g->trajectory.points[i].positions[remapTable_[j]];
      }

      trj_ptr->points[i].velocities.resize(
          g->trajectory.points[i].velocities.size());
      for (unsigned int j = 0; j < g->trajectory.points[i].velocities.size();
          j++) {
        trj_ptr->points[i].velocities[j] =
            g->trajectory.points[i].velocities[remapTable_[j]];
      }

      trj_ptr->points[i].accelerations.resize(
          g->trajectory.points[i].accelerations.size());
      for (unsigned int j = 0; j < g->trajectory.points[i].accelerations.size();
          j++) {
        trj_ptr->points[i].accelerations[j] = g->trajectory.points[i]
            .accelerations[remapTable_[j]];
      }

      trj_ptr->points[i].time_from_start = g->trajectory.points[i]
          .time_from_start;
    }

    // Sprawdzenie czasu w nagłówku OLD_HEADER_TIMESTAMP
    if (g->trajectory.header.stamp < rtt_rosclock::host_now()) {
      RTT::Logger::log(RTT::Logger::Debug) << "Old header timestamp"
                                           << RTT::endlog();
      res.error_code =
          control_msgs::FollowJointTrajectoryResult::OLD_HEADER_TIMESTAMP;
      gh.setRejected(res, "");
    }

//.........这里部分代码省略.........