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