Update Sensor Format

moveit · Jul 11, 2022 · 83171c6 · 83171c6
1 parent 19267f6
commit 83171c6
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 5 deletions.
diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/src/perception_config.cpp b/moveit_setup_assistant/moveit_setup_app_plugins/src/perception_config.cpp
@@ -89,12 +89,13 @@ std::vector<SensorParameters> PerceptionConfig::load3DSensorsYAML(const std::fil
     if (sensors_node && sensors_node.IsSequence())
     {
       // Loop over the sensors available in the file
-      for (const YAML::Node& sensor_name : sensors_node)
+      for (const YAML::Node& sensor_name_node : sensors_node)
       {
-        const YAML::Node& sensor = doc[sensor_name.as<std::string>()];
+        std::string sensor_name = sensor_name_node.as<std::string>();
+        const YAML::Node& sensor = doc[sensor_name];
 
         SensorParameters sensor_map;
-        sensor_map["name"] = sensor_name.as<std::string>();
+        sensor_map["name"] = sensor_name;
         for (YAML::const_iterator sensor_it = sensor.begin(); sensor_it != sensor.end(); ++sensor_it)
         {
           sensor_map[sensor_it->first.as<std::string>()] = sensor_it->second.as<std::string>();

diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/templates/config/sensors_3d.yaml b/moveit_setup_assistant/moveit_setup_app_plugins/templates/config/sensors_3d.yaml
@@ -2,15 +2,19 @@
 # The name of this file shouldn't be changed, or else the Setup Assistant won't detect it.
 # Values may not be ideal defaults, original source unclear.
 sensors:
-  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+  - kinect_pointcloud
+  - kinect_depthimage
+kinect_pointcloud:
+    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
     point_cloud_topic: /head_mount_kinect/depth_registered/points
     max_range: 5.0
     point_subsample: 1
     padding_offset: 0.1
     padding_scale: 1.0
     max_update_rate: 1.0
     filtered_cloud_topic: filtered_cloud
-  - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+kinect_depthimage:
+    sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
     image_topic: /head_mount_kinect/depth_registered/image_raw
     queue_size: 5
     near_clipping_plane_distance: 0.3