Ejemplo n.º 1
0
void ragnar_simulator::RagnarSimulator::goalCB(JointTractoryActionServer::GoalHandle& gh)
{
  ROS_INFO("Recieved new goal request");
  if (has_active_goal_)
  {
    ROS_WARN("Received new goal, canceling current one");
    active_goal_.setAborted();
    has_active_goal_ = false;
  }

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

  const trajectory_msgs::JointTrajectory& traj = active_goal_.getGoal()->trajectory;
  setTrajectory(traj);
}
void ragnar_drivers::RagnarTrajectoryStreamer::goalCB(JointTractoryActionServer::GoalHandle& gh)
{
  ROS_INFO("Recieved new goal request");
  if (has_active_goal_)
  {
    ROS_WARN("Received new goal, canceling current one");
    trajectoryStop();
    active_goal_.setAborted();
    has_active_goal_ = false;
  }

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

  const trajectory_msgs::JointTrajectory& traj = active_goal_.getGoal()->trajectory;
  if (!traj.points.empty())
  {
    target_pt_ = traj.points.back();
  }
  jointTrajectoryCB( trajectory_msgs::JointTrajectoryConstPtr(new trajectory_msgs::JointTrajectory(traj)) );
}
void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle & gh)
{
    ROS_INFO("Received new goal");

    if (!gh.getGoal()->trajectory.points.empty())
    {
        if (industrial_utils::isSimilar(joint_names_, gh.getGoal()->trajectory.joint_names))
        {

            // Cancels the currently active goal.
            if (has_active_goal_)
            {
                ROS_WARN("Received new goal, canceling current goal");
                abortGoal();
            }

            // Sends the trajectory along to the controller
            if (withinGoalConstraints(last_trajectory_state_, gh.getGoal()->trajectory))
            {

                ROS_INFO_STREAM("Already within goal constraints, setting goal succeeded");
                gh.setAccepted();
                gh.setSucceeded();
                has_active_goal_ = false;

            }
            else
            {
                gh.setAccepted();
                active_goal_ = gh;
                has_active_goal_ = true;

                ROS_INFO("Publishing trajectory");

                current_traj_ = active_goal_.getGoal()->trajectory;
                pub_trajectory_command_.publish(current_traj_);
            }
        }
        else
        {
            ROS_ERROR("Joint trajectory action failing on invalid joints");
            control_msgs::FollowJointTrajectoryResult rslt;
            rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
            gh.setRejected(rslt, "Joint names do not match");
        }
    }
    else
    {
        ROS_ERROR("Joint trajectory action failed on empty trajectory");
        control_msgs::FollowJointTrajectoryResult rslt;
        rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
        gh.setRejected(rslt, "Empty trajectory");
    }

    // Adding some informational log messages to indicate unsupported goal constraints
    if (gh.getGoal()->goal_time_tolerance.toSec() > 0.0)
    {
        ROS_WARN_STREAM("Ignoring goal time tolerance in action goal, may be supported in the future");
    }
    if (!gh.getGoal()->goal_tolerance.empty())
    {
        ROS_WARN_STREAM(
            "Ignoring goal tolerance in action, using paramater tolerance of " << goal_threshold_ << " instead");
    }
    if (!gh.getGoal()->path_tolerance.empty())
    {
        ROS_WARN_STREAM("Ignoring goal path tolerance, option not supported by ROS-Industrial drivers");
    }
}