From 015b1466f417702888781c1dec28814deb20353c Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 30 May 2018 19:09:57 +0200 Subject: [PATCH] MoveRel: handle unknown links with hard failure --- core/src/stages/move_relative.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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);