From f7d2c98fac210619580b8b32014e81daffb7f086 Mon Sep 17 00:00:00 2001 From: KazuyaOguma Date: Tue, 11 Feb 2025 21:31:14 +0900 Subject: [PATCH] fix: parameter builder for MoveItPy --- .../moveit_py_configs_builder.py | 76 ++++++++++++++----- 1 file changed, 59 insertions(+), 17 deletions(-) diff --git a/moveit_py_configs_utils/moveit_py_configs_utils/moveit_py_configs_builder.py b/moveit_py_configs_utils/moveit_py_configs_utils/moveit_py_configs_builder.py index f392c9b..c1bc748 100644 --- a/moveit_py_configs_utils/moveit_py_configs_utils/moveit_py_configs_builder.py +++ b/moveit_py_configs_utils/moveit_py_configs_utils/moveit_py_configs_builder.py @@ -377,23 +377,24 @@ def trajectory_execution( def planning_scene_monitor( self, - publish_planning_scene: bool = True, - publish_geometry_updates: bool = True, - publish_state_updates: bool = True, - publish_transforms_updates: bool = True, - publish_robot_description: bool = False, - publish_robot_description_semantic: bool = False, + name="planning_scene_monitor", + robot_description="robot_description", + joint_state_topic="/joint_states", + attached_collision_object_topic="/moveit_cpp/planning_scene_monitor", + publish_planning_scene_topic="/moveit_cpp/publish_planning_scene", + monitored_planning_scene_topic="/moveit_cpp/monitored_planning_scene", + wait_for_initial_state_timeout=10.0, ): self.__moveit_configs.planning_scene_monitor = { - # TODO: Fix parameter namespace upstream -- see planning_scene_monitor.cpp:262 - # "planning_scene_monitor": { - "publish_planning_scene": publish_planning_scene, - "publish_geometry_updates": publish_geometry_updates, - "publish_state_updates": publish_state_updates, - "publish_transforms_updates": publish_transforms_updates, - "publish_robot_description": publish_robot_description, - "publish_robot_description_semantic": publish_robot_description_semantic, - # } + "planning_scene_monitor_options": { + "name": name, + "robot_description": robot_description, + "joint_state_topic": joint_state_topic, + "attached_collision_object_topic": attached_collision_object_topic, + "publish_planning_scene_topic": publish_planning_scene_topic, + "monitored_planning_scene_topic": monitored_planning_scene_topic, + "wait_for_initial_state_timeout": wait_for_initial_state_timeout, + } } return self @@ -419,6 +420,10 @@ def planning_pipelines( default_planning_pipeline: str = None, pipelines: List[str] = None, load_all: bool = True, + planning_attempts: int = 1, + max_velocity_scaling_factor: float = 1.0, + max_acceleration_scaling_factor: float = 1.0, + planning_time: float = 1.0, ): """Load planning pipelines parameters. @@ -457,19 +462,56 @@ def planning_pipelines( "planning_pipelines": { "pipeline_names": pipelines, }, - "default_planning_pipeline": default_planning_pipeline, + "plan_request_params": { + "planning_attempts": planning_attempts, + "planning_pipeline": default_planning_pipeline, + "max_velocity_scaling_factor": max_velocity_scaling_factor, + "max_acceleration_scaling_factor": max_acceleration_scaling_factor, + }, } + for pipeline in pipelines: parameter_file = config_folder / (pipeline + "_planning.yaml") if not parameter_file.exists(): parameter_file = default_folder / (pipeline + "_planning.yaml") - self.__moveit_configs.planning_pipelines[pipeline] = load_yaml(parameter_file) + pipeline_config = load_yaml(parameter_file) + self.__moveit_configs.planning_pipelines[pipeline] = pipeline_config + # config = { + # "planning_attempts": planning_attempts, + # "planning_pipeline": pipeline, + # "planned_id": planner_config["type"], + # "max_velocity_scaling_factor": max_velocity_scaling_factor, + # "max_acceleration_scaling_factor": max_acceleration_scaling_factor, + # "planning_time": planning_time, + # } + # self.__moveit_configs.planning_pipelines[planner_name] = config + # for planner_name, planner_config in pipeline_config["planner_configs"]: + # config = { + # "planning_attempts": planning_attempts, + # "planning_pipeline": pipeline, + # "planned_id": planner_config["type"], + # "max_velocity_scaling_factor": max_velocity_scaling_factor, + # "max_acceleration_scaling_factor": max_acceleration_scaling_factor, + # "planning_time": planning_time, + # } + # self.__moveit_configs.planning_pipelines[planner_name] = config # Special rule to add ompl planner_configs if "ompl" in self.__moveit_configs.planning_pipelines: ompl_config = self.__moveit_configs.planning_pipelines["ompl"] if "planner_configs" not in ompl_config: ompl_config.update(load_yaml(default_folder / "ompl_defaults.yaml")) + for planner_name in ompl_config["planner_configs"].keys(): + planner_config = ompl_config["planner_configs"][planner_name] + config = { + "planning_attempts": planning_attempts, + "planning_pipeline": "ompl", + "planned_id": planner_config["type"], + "max_velocity_scaling_factor": max_velocity_scaling_factor, + "max_acceleration_scaling_factor": max_acceleration_scaling_factor, + "planning_time": planning_time, + } + self.__moveit_configs.planning_pipelines[planner_name] = config return self