diff --git a/examples/ex_joint_goal.py b/examples/ex_joint_goal.py index cece348..d756138 100755 --- a/examples/ex_joint_goal.py +++ b/examples/ex_joint_goal.py @@ -38,6 +38,8 @@ def main(): node.declare_parameter("synchronous", True) # If non-positive, don't cancel. Only used if synchronous is False node.declare_parameter("cancel_after_secs", 0.0) + # Planner ID + node.declare_parameter("planner_id", "RRTConnectkConfigDefault") # Create callback group that allows execution of callbacks in parallel without restrictions callback_group = ReentrantCallbackGroup() @@ -51,6 +53,9 @@ def main(): group_name=panda.MOVE_GROUP_ARM, callback_group=callback_group, ) + moveit2.planner_id = ( + node.get_parameter("planner_id").get_parameter_value().string_value + ) # Spin the node in background thread(s) and wait a bit for initialization executor = rclpy.executors.MultiThreadedExecutor(2) diff --git a/examples/ex_pose_goal.py b/examples/ex_pose_goal.py index cbe7a70..401fd19 100755 --- a/examples/ex_pose_goal.py +++ b/examples/ex_pose_goal.py @@ -29,6 +29,8 @@ def main(): node.declare_parameter("synchronous", True) # If non-positive, don't cancel. Only used if synchronous is False node.declare_parameter("cancel_after_secs", 0.0) + # Planner ID + node.declare_parameter("planner_id", "RRTConnectkConfigDefault") # Create callback group that allows execution of callbacks in parallel without restrictions callback_group = ReentrantCallbackGroup() @@ -42,6 +44,9 @@ def main(): group_name=panda.MOVE_GROUP_ARM, callback_group=callback_group, ) + moveit2.planner_id = ( + node.get_parameter("planner_id").get_parameter_value().string_value + ) # Spin the node in background thread(s) and wait a bit for initialization executor = rclpy.executors.MultiThreadedExecutor(2) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 5285cc3..6d7684e 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -1933,8 +1933,8 @@ def __init_move_action_goal( move_action_goal.request.path_constraints = Constraints() # move_action_goal.request.trajectory_constraints = "Ignored" # move_action_goal.request.reference_trajectories = "Ignored" - # move_action_goal.request.pipeline_id = "Ignored" - # move_action_goal.request.planner_id = "Ignored" + move_action_goal.request.pipeline_id = "" + move_action_goal.request.planner_id = "" move_action_goal.request.group_name = group_name move_action_goal.request.num_planning_attempts = 5 move_action_goal.request.allowed_planning_time = 0.5 @@ -2056,6 +2056,22 @@ def allowed_planning_time(self) -> float: def allowed_planning_time(self, value: float): self.__move_action_goal.request.allowed_planning_time = value + @property + def pipeline_id(self) -> int: + return self.__move_action_goal.request.pipeline_id + + @pipeline_id.setter + def pipeline_id(self, value: str): + self.__move_action_goal.request.pipeline_id = value + + @property + def planner_id(self) -> int: + return self.__move_action_goal.request.planner_id + + @planner_id.setter + def planner_id(self, value: str): + self.__move_action_goal.request.planner_id = value + def init_joint_state( joint_names: List[str],