diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index badf8af6..6999a6b6 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -103,12 +103,12 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning const auto& accepted = jmg->getJointModelNames(); auto& robot_state = scene->getCurrentStateNonConst(); const auto& joints = boost::any_cast>(goal); - for (auto j : joints) { + for (const auto& j : joints) { int index = robot_state.getRobotModel()->getVariableIndex(j.first); auto jm = scene->getRobotModel()->getJointModel(index); if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end()) { - ROS_WARN_STREAM_NAMED("MoveRelative", "Ignoring joint " << j.first << " that is not part of group " << group); - continue; + ROS_WARN_STREAM_NAMED("MoveRelative", "Cannot plan joint target for joint '" << j.first << "' that is not part of group '" << group << "'"); + return false; } robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second); robot_state.enforceBounds(jm);