diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index 380acd3..494d932 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -145,7 +145,11 @@ bool ActionServer::validateJoints(GoalHandle& gh, Result& res) return true; res.error_code = Result::INVALID_JOINTS; - res.error_string = "Invalid joint names for goal"; + res.error_string = "Invalid joint names for goal\n"; + res.error_string += "Expected: "; + std::for_each(goal_joints.begin(), goal_joints.end(), [&res](std::string joint){res.error_string += joint + ", ";}); + res.error_string += "\nFound: "; + std::for_each(joint_set_.begin(), joint_set_.end(), [&res](std::string joint){res.error_string += joint + ", ";}); return false; } @@ -183,7 +187,7 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res) } if (std::fabs(velocity) > max_velocity_) { - res.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_); + res.error_string = "Received a goal with velocities that are higher than max_velocity_ " + std::to_string(max_velocity_); return false; } } @@ -338,4 +342,4 @@ void ActionServer::trajectoryThread() has_goal_ = false; lk.unlock(); } -} \ No newline at end of file +}