MoveTo: support map<string,double> joint target

There was a corresponding setGoal method already, but no implementation...
This commit is contained in:
v4hn 2020-08-11 17:22:53 +02:00
parent ee6c50ad31
commit 310ca3ef15

View File

@ -120,6 +120,20 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint
} catch (const boost::bad_any_cast&) {
}
try {
const std::map<std::string, double>& joint_map = boost::any_cast<std::map<std::string, double>>(goal);
const auto& accepted = jmg->getJointModelNames();
for (const auto& joint : joint_map) {
if (std::find(accepted.begin(), accepted.end(), joint.first) == accepted.end())
throw InitStageException(*this,
"Joint '" + joint.first + "' is not part of group '" + jmg->getName() + "'");
state.setVariablePosition(joint.first, joint.second);
}
state.update();
return true;
} catch (const boost::bad_any_cast&) {
}
return false;
}