load sensors_3d.yaml (moveit#1387)

moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py

@@ -28,6 +28,7 @@
     moveit_configs.planning_pipelines
     moveit_configs.trajectory_execution
     moveit_configs.planning_scene_monitor
+    moveit_configs.sensors_3d
     moveit_configs.move_group_capabilities
     moveit_configs.joint_limits
     moveit_configs.moveit_cpp
@@ -100,8 +101,10 @@ class MoveItConfigs:
     planning_pipelines: Dict = field(default_factory=dict)
     # A dictionary contains parameters for trajectory execution & moveit controller managers.
     trajectory_execution: Dict = field(default_factory=dict)
-    # A dictionary that have the planning scene monitor's parameters.
+    # A dictionary that has the planning scene monitor's parameters.
     planning_scene_monitor: Dict = field(default_factory=dict)
+    # A dictionary that has the sensor 3d configuration parameters.
+    sensors_3d: Dict = field(default_factory=dict)
     # A dictionary containing move_group's non-default capabilities.
     move_group_capabilities: Dict = field(default_factory=dict)
     # A dictionary containing the overridden position/velocity/acceleration limits.
@@ -119,6 +122,7 @@ def to_dict(self):
         parameters.update(self.planning_pipelines)
         parameters.update(self.trajectory_execution)
         parameters.update(self.planning_scene_monitor)
+        parameters.update(self.sensors_3d)
         parameters.update(self.joint_limits)
         parameters.update(self.moveit_cpp)
         parameters.update(self.cartesian_limits)
@@ -338,6 +342,23 @@ def planning_scene_monitor(
         }
         return self
 
+    def sensors_3d(self, file_path: Optional[str] = None):
+        """Load sensors_3d paramerss.
+
+        :param file_path: Absolute or relative path to the sensors_3d yaml file (w.r.t. robot_name_moveit_config).
+        :return: Instance of MoveItConfigsBuilder with robot_description_planning loaded.
+        """
+        sensors_path = self._package_path / (
+            file_path or self.__config_dir_path / "sensors_3d.yaml"
+        )
+        if sensors_path.exists():
+            sensors_data = load_yaml(sensors_path)
+            # TODO(mikeferguson): remove the second part of this check once
+            # https://github.com/ros-planning/moveit_resources/pull/141 has made through buildfarm
+            if len(sensors_data["sensors"]) > 0 and sensors_data["sensors"][0]:
+                self.__moveit_configs.sensors_3d = sensors_data
+        return self
+
     def planning_pipelines(
         self,
         default_planning_pipeline: str = None,
@@ -429,6 +450,8 @@ def to_moveit_configs(self):
             self.trajectory_execution()
         if not self.__moveit_configs.planning_scene_monitor:
             self.planning_scene_monitor()
+        if not self.__moveit_configs.sensors_3d:
+            self.sensors_3d()
         if not self.__moveit_configs.joint_limits:
             self.joint_limits()
         # TODO(JafarAbdi): We should have a default moveit_cpp.yaml as port of a moveit config package