StagesWrapper: Add std::map setGoal overload.

Adding a further overload of the `setGoal()` function that is exposed to
the python api. This should provide an interface for passing in dicts as
joint name and -angle configurations.
This commit is contained in:
cpetersmeier 2020-10-06 12:07:16 +02:00 committed by Robert Haschke
parent 49b2ff8ce5
commit ede5fe396f
2 changed files with 4 additions and 2 deletions

View File

@ -174,9 +174,10 @@ class TestStages(unittest.TestCase):
self._check(stage, "group", "group")
self._check(stage, "ik_frame", PoseStamped())
stage.setGoal(PoseStamped())
# TODO: fails
# stage.setGoal(PointStamped())
stage.setGoal(PointStamped())
stage.setGoal(RobotState())
stage.setGoal("named pose")
stage.setGoal(dict(joint1 = 1.0, joint2 = 2.0))
self._check(stage, "path_constraints", Constraints())
def test_MoveRelative(self):

View File

@ -175,6 +175,7 @@ void export_stages() {
.def<void (MoveTo::*)(const geometry_msgs::PoseStamped&)>("setGoal", &MoveTo::setGoal)
.def<void (MoveTo::*)(const geometry_msgs::PointStamped&)>("setGoal", &MoveTo::setGoal)
.def<void (MoveTo::*)(const moveit_msgs::RobotState&)>("setGoal", &MoveTo::setGoal)
.def<void (MoveTo::*)(const std::map<std::string, double>&)>("setGoal", &MoveTo::setGoal)
.def<void (MoveTo::*)(const std::string&)>("setGoal", &MoveTo::setGoal);
bp::implicitly_convertible<std::auto_ptr<MoveTo>, std::auto_ptr<PropagatingEitherWay>>();