Fixed bug in plugin manager. Fixed and cleaned up pca and semantic pl…

…ugins. Cleaned up configs and launch files.
leggedrobotics · Dec 4, 2023 · 0207803 · 0207803
1 parent ed0f049
commit 0207803
Show file tree

Hide file tree

Showing 24 changed files with 233 additions and 240 deletions.
diff --git a/elevation_mapping_cupy/config/setups/turtle_bot/plugin_config.yaml b/elevation_mapping_cupy/config/setups/turtle_bot/plugin_config.yaml
@@ -42,19 +42,26 @@ semantic_filter:
   enable: True
   fill_nan: False
   is_height_layer: False
-  layer_name: "sem_fil"
+  layer_name: "max_categories"
   extra_params:
-    classes: ['grass','tree','fence','dirt']
+    classes: ['^sem_.*$']
 
 semantic_traversability:
   type: "semantic_traversability"
   enable: False
   fill_nan: False
   is_height_layer: False
-  layer_name: "sem_traversability"
+  layer_name: "semantic_traversability"
   extra_params:
     layers: ['traversability','robot_centric_elevation']
     thresholds: [0.3,0.5]
     type: ['traversability', 'elevation']
 
-
+features_pca:
+  type: "features_pca"
+  enable: True
+  fill_nan: False
+  is_height_layer: False
+  layer_name: "pca"
+  extra_params:
+    process_layer_names: ["^feat_.*$"]
diff --git a/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_image_semantics.yaml b/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_image_semantics.yaml
diff --git a/...tups/turtle_bot/turtle_bot_semantics.yaml → ...urtle_bot/turtle_bot_semantics_image.yaml b/...tups/turtle_bot/turtle_bot_semantics.yaml → ...urtle_bot/turtle_bot_semantics_image.yaml
@@ -22,28 +22,23 @@ subscribers:
     camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized'
     channel_info_topic_name: '/front_cam/channel_info'
     data_type: image
-  # fixed_semantic_cam: # for semantic images
-  #   topic_name: '/front_cam/semantic_image'
-  #   camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized'
-  #   channels: ["test", "test2", "test3"]
-  #   data_type: image
   front_cam_pointcloud:
     topic_name: '/camera/depth/points'
     data_type: pointcloud
   feat_front:
-    topic_name: /elevation_mapping/feat_f
-    camera_info_topic_name: "/camera/depth/camera_info"
-    channels: [ 'feat_0','feat_1','feat_2','feat_3','feat_4','feat_5','feat_6','feat_7','feat_8','feat_9' ]
+    topic_name: '/front_cam/semantic_seg_feat'
+    camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized'
+    channel_info_topic_name: '/front_cam/feat_channel_info'
     data_type: image
 
 
 #### Publishers ########
 publishers:
   elevation_map_raw:
-    layers: ['elevation', 'traversability', 'variance','rgb','sem_fil', 'person', 'chair', 'sofa']
+    layers: ['elevation', 'traversability', 'variance','rgb','max_categories', 'pca']
     basic_layers: ['elevation']
     fps: 5.0
   elevation_map_filter:
-    layers: ['min_filter', 'smooth', 'inpaint', 'elevation','rgb','sem_fil']
+    layers: ['min_filter', 'smooth', 'inpaint', 'elevation','rgb','max_categories']
     basic_layers: ['min_filter']
     fps: 3.0
diff --git a/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics_pointcloud.yaml b/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics_pointcloud.yaml
@@ -0,0 +1,28 @@
+#### Plugins ########
+plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/setups/turtle_bot/plugin_config.yaml'
+
+pointcloud_channel_fusions:
+  rgb: 'color'
+  default: 'average'
+
+image_channel_fusions:
+  rgb: 'color'
+  default: 'exponential'
+
+#### Subscribers ########
+subscribers:
+  front_cam_pointcloud:
+    topic_name: '/camera/depth/points'
+    data_type: pointcloud
+
+
+#### Publishers ########
+publishers:
+  elevation_map_raw:
+    layers: ['elevation', 'traversability', 'variance','rgb','max_categories']
+    basic_layers: ['elevation']
+    fps: 5.0
+  elevation_map_filter:
+    layers: ['min_filter', 'smooth', 'inpaint', 'elevation','rgb','max_categories']
+    basic_layers: ['min_filter']
+    fps: 3.0
diff --git a/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch b/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch
@@ -1,7 +1,7 @@
 <launch>
     <!-- Elevation mapping node -->
     <node pkg="elevation_mapping_cupy" type="elevation_mapping_node" name="elevation_mapping" output="screen">
-        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/parameters.yaml"/>
-        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/sensor_parameter.yaml"/>
+        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/core/core_param.yaml"/>
+        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/core/example_setup.yaml"/>
     </node>
-</launch>
+</launch>
diff --git a/elevation_mapping_cupy/launch/turtlesim_features_image_example.launch b/elevation_mapping_cupy/launch/turtlesim_features_image_example.launch
diff --git a/...nch/turtlesim_segmentation_example.launch → ...tlesim_plane_decomposition_example.launch b/...nch/turtlesim_segmentation_example.launch → ...tlesim_plane_decomposition_example.launch
diff --git a/elevation_mapping_cupy/launch/turtlesim_semantic_example.launch b/elevation_mapping_cupy/launch/turtlesim_semantic_example.launch
diff --git a/elevation_mapping_cupy/launch/turtlesim_semantic_image_example.launch b/elevation_mapping_cupy/launch/turtlesim_semantic_image_example.launch
@@ -10,13 +10,10 @@
     <node pkg="semantic_sensor" type="image_node.py" name="front_cam" args="front_cam_image"
           output="screen">
         <rosparam command="load" file="$(find semantic_sensor)/config/sensor_parameter.yaml"/>
-        <!-- <rosparam command="load" file="$(find elevation_mapping_cupy)/config/setups/turtle_bot/turtle_bot_image_semantics.yaml"/> -->
     </node>
 
         <node pkg="elevation_mapping_cupy" type="elevation_mapping_node" name="elevation_mapping" output="screen">
             <rosparam command="load" file="$(find elevation_mapping_cupy)/config/core/core_param.yaml"/>
-            <!-- <rosparam command="load" file="$(find elevation_mapping_cupy)/config/semantic_parameters.yaml"/> -->
-            <!-- <rosparam command="load" file="$(find elevation_mapping_cupy)/config/setups/turtle_bot/image_semantic_sensor_parameter.yaml"/> -->
-            <rosparam command="load" file="$(find elevation_mapping_cupy)/config/setups/turtle_bot/turtle_bot_semantics.yaml"/>
+            <rosparam command="load" file="$(find elevation_mapping_cupy)/config/setups/turtle_bot/turtle_bot_semantics_image.yaml"/>
         </node>
 </launch>
diff --git a/elevation_mapping_cupy/launch/turtlesim_semantic_pointcloud_example.launch b/elevation_mapping_cupy/launch/turtlesim_semantic_pointcloud_example.launch
@@ -6,14 +6,17 @@
     <include file="$(find elevation_mapping_cupy)/launch/turtlesim_init.launch">
         <arg name="rviz_config" value="$(find elevation_mapping_cupy)/rviz/turtle_example.rviz"/>
     </include>
-    <node pkg="semantic_sensor" type="pointcloud_node.py" name="semantic_pointcloud" args="front_cam"
+    <!-- <node pkg="semantic_sensor" type="pointcloud_node.py" name="semantic_pointcloud" args="front_cam"
           output="screen">
         <rosparam command="load" file="$(find elevation_mapping_cupy)/config/sensor_parameter.yaml"/>
-    </node>
+    </node> -->
 
+    <node pkg="semantic_sensor" type="pointcloud_node.py" name="front_cam" args="front_cam" output="screen">
+        <rosparam command="load" file="$(find semantic_sensor)/config/sensor_parameter.yaml"/>
+    </node>
 
     <node pkg="elevation_mapping_cupy" type="elevation_mapping_node" name="elevation_mapping" output="screen">
-        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/parameters.yaml"/>
-        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/sensor_parameter.yaml"/>
+        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/core/core_param.yaml"/>
+        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/setups/turtle_bot/turtle_bot_semantics_pointcloud.yaml"/>
     </node>
 </launch>
diff --git a/...ping_cupy/launch/turtlesim_example.launch → ...py/launch/turtlesim_simple_example.launch b/...ping_cupy/launch/turtlesim_example.launch → ...py/launch/turtlesim_simple_example.launch
diff --git a/elevation_mapping_cupy/rviz/turtle_example.rviz b/elevation_mapping_cupy/rviz/turtle_example.rviz
@@ -1,12 +1,14 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 138
+    Help Height: 70
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /Global Options1
+        - /ElevationMapRaw1
+        - /Feature1
       Splitter Ratio: 0.5768463015556335
-    Tree Height: 229
+    Tree Height: 234
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -24,7 +26,7 @@ Panels:
   - Class: rviz/Time
     Name: Time
     SyncMode: 0
-    SyncSource: Image
+    SyncSource: Color
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -69,7 +71,7 @@ Visualization Manager:
       Show Grid Lines: true
       Topic: /elevation_mapping/elevation_map_raw
       Unreliable: false
-      Use Rainbow: true
+      Use Rainbow: false
       Value: true
     - Alpha: 1
       Class: rviz/RobotModel
@@ -180,19 +182,31 @@ Visualization Manager:
       Max Value: 1
       Median window: 5
       Min Value: 0
-      Name: Image
+      Name: Color
       Normalize Range: true
       Queue Size: 2
       Transport Hint: raw
       Unreliable: false
       Value: true
     - Class: rviz/Image
       Enabled: true
-      Image Topic: /front_cam/elevation_mapping/semantic_image_front
+      Image Topic: /front_cam/semantic_image_debug
       Max Value: 1
       Median window: 5
       Min Value: 0
-      Name: Image
+      Name: Segmentation
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /front_cam/semantic_seg_feat_im
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Feature
       Normalize Range: true
       Queue Size: 2
       Transport Hint: raw
@@ -242,19 +256,23 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5147963762283325
+      Pitch: 0.4647962152957916
       Target Frame: base_footprint
-      Yaw: 3.1033706665039062
+      Yaw: 3.113370418548584
     Saved: ~
 Window Geometry:
+  Color:
+    collapsed: false
   Displays:
     collapsed: false
-  Height: 1043
+  Feature:
+    collapsed: false
+  Height: 1306
   Hide Left Dock: false
   Hide Right Dock: true
-  Image:
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f700000464fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000016b000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0043006f006c006f007201000001ac000000fd0000001600fffffffb00000018005300650067006d0065006e0074006100740069006f006e01000002af000000e40000001600fffffffb0000000e00460065006100740075007200650100000399000001060000001600ffffff000000010000015f0000053bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000560000053b000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005afc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000005830000046400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Segmentation:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001f70000035dfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001aa000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001eb000000d30000001600fffffffb0000000a0049006d00610067006501000002c4000000d40000001600ffffff000000010000015f0000053bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000560000053b000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005afc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000005830000035d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -265,4 +283,4 @@ Window Geometry:
     collapsed: true
   Width: 1920
   X: 2280
-  Y: 105
+  Y: 68
diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py
@@ -731,7 +731,7 @@ def get_map_with_name_ref(self, name, data):
                     self.elevation_map,
                     self.layer_names,
                     self.semantic_map.semantic_map,
-                    self.semantic_map.param,
+                    self.semantic_map.layer_names,
                     self.base_rotation,
                     self.semantic_map.elements_to_shift,
                 )