From 49ff2d214e6ee471e104eeb86e54730fb448d6dd Mon Sep 17 00:00:00 2001 From: gavanderhoorn Date: Thu, 23 Oct 2014 12:08:14 +0200 Subject: [PATCH] Only accept goals after reception of controller feedback. Fix #85. --- .../src/joint_trajectory_action.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/industrial_robot_client/src/joint_trajectory_action.cpp b/industrial_robot_client/src/joint_trajectory_action.cpp index b0c8031b..aebfde70 100644 --- a/industrial_robot_client/src/joint_trajectory_action.cpp +++ b/industrial_robot_client/src/joint_trajectory_action.cpp @@ -116,6 +116,18 @@ void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle & gh) { ROS_INFO("Received new goal"); + // reject all goals as long as we haven't heard from the remote controller + if (!trajectory_state_recvd_) + { + ROS_ERROR("Joint trajectory action rejected: waiting for (initial) feedback from controller"); + control_msgs::FollowJointTrajectoryResult rslt; + rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL; + gh.setRejected(rslt, "Waiting for (initial) feedback from controller"); + + // no point in continuing: already rejected + return; + } + if (!gh.getGoal()->trajectory.points.empty()) { if (industrial_utils::isSimilar(joint_names_, gh.getGoal()->trajectory.joint_names))