/*
Service call when we change the state of a goal/plan/action
*/
bool changeState(supervisor_msgs::ChangeState::Request &req, supervisor_msgs::ChangeState::Response &res){
if(req.type == "action"){
return actionState(req.action, req.state);
}else if(req.type == "plan"){
if(req.state == "PROGRESS"){
return newPlan(req.plan, false);
}else if(req.state == "SHARE"){
return sharePlan();
}else if(req.state == "PROGRESS_SHARE"){
return newPlan(req.plan, true);
}else if(req.state == "ABORT"){
return abortPlan(req.agent);
}
}else if(req.type == "goal"){
if(req.state == "NEW"){
return newGoal(req.goal);
}else if(req.state == "PROGRESS"){
return startGoal(req.goal);
}else if(req.state == "ABORT"){
return abortGoal(req.goal, req.agent);
}
}
}
void JointTrajectoryAction::watchdog(const ros::TimerEvent &e)
{
// Some debug logging
if (!last_trajectory_state_)
{
ROS_DEBUG("Waiting for subscription to joint trajectory state");
}
if (!trajectory_state_recvd_)
{
ROS_DEBUG("Trajectory state not received since last watchdog");
}
// Aborts the active goal if the controller does not appear to be active.
if (has_active_goal_)
{
if (!trajectory_state_recvd_)
{
// last_trajectory_state_ is null if the subscriber never makes a connection
if (!last_trajectory_state_)
{
ROS_WARN("Aborting goal because we have never heard a controller state message.");
}
else
{
ROS_WARN_STREAM(
"Aborting goal because we haven't heard from the controller in " << WATCHD0G_PERIOD_ << " seconds");
}
abortGoal();
}
}
// Reset the trajectory state received flag
trajectory_state_recvd_ = false;
}
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");
}
}
