Work in progress for adding FusionInfo pub and sub.

leggedrobotics · Nov 29, 2023 · 4dbbcb2 · 4dbbcb2
1 parent 2a5bd37
commit 4dbbcb2
Show file tree

Hide file tree

Showing 17 changed files with 353 additions and 147 deletions.
diff --git a/elevation_mapping_cupy/config/setups/anymal/anymal_parameters.yaml b/elevation_mapping_cupy/config/setups/anymal/anymal_parameters.yaml
@@ -55,30 +55,30 @@ enable_normal_color: false                      # If true, the map contains 'col
 
 #### Traversability filter ########
 use_chainer: false                              # Use chainer as a backend of traversability filter or pytorch. If false, it uses pytorch. pytorch requires ~2GB more GPU memory compared to chainer but runs faster.
-weight_file: '$(rospack find elevation_mapping_cupy)/config/weights.dat'               # Weight file for traversability filter
+weight_file: '$(rospack find elevation_mapping_cupy)/config/core/weights.dat'               # Weight file for traversability filter
 
 #### Upper bound ########
 use_only_above_for_upper_bound: false
 
 #### Plugins ########
-plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/anymal_plugin_config.yaml'
+# plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/anymal_plugin_config.yaml'
 
 #### Publishers ########
 # topic_name:
 #   layers:               # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names
 #   basic_layers:         # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`.
 #   fps:                  # Publish rate. Use smaller value than `map_acquire_fps`.
 
-publishers:
-  elevation_map_raw:
-    layers: ['elevation', 'traversability', 'variance']
-    basic_layers: ['elevation', 'traversability']
-    fps: 5.0
+# publishers:
+#   elevation_map_raw:
+#     layers: ['elevation', 'traversability', 'variance']
+#     basic_layers: ['elevation', 'traversability']
+#     fps: 5.0
 
-  semantic_map_raw:
-    layers: ['elevation', 'traversability']
-    basic_layers: ['elevation', 'traversability']
-    fps: 5.0
+#   semantic_map_raw:
+#     layers: ['elevation', 'traversability']
+#     basic_layers: ['elevation', 'traversability']
+#     fps: 5.0
 
 #### Initializer ########
 initialize_method: 'linear'                                         # Choose one from 'nearest', 'linear', 'cubic'

diff --git a/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml b/elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml
@@ -1,15 +1,46 @@
+plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/setups/anymal/anymal_plugin_config.yaml'
+
+#### Publishers ########
+# topic_name:
+#   layers:               # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names
+#   basic_layers:         # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`.
+#   fps:                  # Publish rate. Use smaller value than `map_acquire_fps`.
+
+publishers:
+  elevation_map_raw:
+    layers: ['elevation', 'traversability', 'variance']
+    basic_layers: ['elevation', 'traversability']
+    fps: 5.0
+
+  semantic_map_raw:
+    layers: ['elevation', 'traversability']
+    basic_layers: ['elevation', 'traversability']
+    fps: 5.0
+
 #### Subscribers ########
 subscribers:
-  front_depth:
+  front_upper_depth:
+    channels: []
+    fusion: []
+    topic_name: /depth_camera_front_upper/point_cloud_self_filtered
+    data_type: pointcloud
+
+  front_lower_depth:
     channels: []
     fusion: []
-    topic_name: /depth_camera_front/point_cloud_self_filtered
+    topic_name: /depth_camera_front_lower/point_cloud_self_filtered
     data_type: pointcloud
 
-  rear_depth:
+  rear_upper_depth:
     channels: []
     fusion: []
-    topic_name: /depth_camera_rear/point_cloud_self_filtered
+    topic_name: /depth_camera_rear_upper/point_cloud_self_filtered
+    data_type: pointcloud
+
+  rear_lower_depth:
+    channels: []
+    fusion: []
+    topic_name: /depth_camera_rear_lower/point_cloud_self_filtered
     data_type: pointcloud
 
   left_depth:
@@ -24,11 +55,11 @@ subscribers:
     topic_name: /depth_camera_right/point_cloud_self_filtered
     data_type: pointcloud
 
-  velodyne:
-    channels: []
-    fusion: []
-    topic_name: /point_cloud_filter/lidar/point_cloud_filtered
-    data_type: pointcloud
+  # velodyne:
+  #   channels: []
+  #   fusion: []
+  #   topic_name: /point_cloud_filter/lidar/point_cloud_filtered
+  #   data_type: pointcloud
 
   front_bpearl:
     channels: []

diff --git a/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics.yaml b/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics.yaml
@@ -2,34 +2,52 @@
 plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/setups/turtle_bot/plugin_config.yaml'
 
 pointcloud_channel_fusion:
-  rgb: 'pointcloud_color'
-  default: 'pointcloud_average'
-  sheep: 'pointcloud_average'
-  sofa: 'pointcloud_class_average'
-  person: 'pointcloud_class_average'
+  rgb: 'color'
+  default: 'average'
+  sheep: 'average'
+  sofa: 'class_average'
+  person: 'class_average'
 
 #### Subscribers ########
 subscribers:
-  front_cam:
-    channels: ['rgb', 'sheep','sofa',"person" ]
-    fusion: [ 'color','class_average','class_average','class_average' ]
-    topic_name: '/elevation_mapping/pointcloud_semantic'
-    semantic_segmentation: True
-    publish_segmentation_image: True
-    segmentation_model: 'lraspp_mobilenet_v3_large' # detectron_coco_panoptic_fpn_R_101_3x, lraspp_mobilenet_v3_large
-    show_label_legend: False
-    data_type: pointcloud
+  color_cam: # for color camera
+    # channels: ['rgb']
+    # fusion: ['color']
+    topic_name: '/camera/rgb/image_raw'
+    camera_info_topic_name: '/camera/depth/camera_info'
+    data_type: image
+  # front_cam:
+    # channels: ['rgb', 'sheep','sofa',"person" ]
+    # fusion: [ 'color','class_average','class_average','class_average' ]
+    # topic_name: '/elevation_mapping/pointcloud_semantic'
+    # semantic_segmentation: True
+    # publish_segmentation_image: True
+    # segmentation_model: 'lraspp_mobilenet_v3_large' # detectron_coco_panoptic_fpn_R_101_3x, lraspp_mobilenet_v3_large
+    # show_label_legend: False
 
-    cam_info_topic: "/camera/depth/camera_info"
-    image_topic: "/camera/rgb/image_raw"
-    depth_topic: "/camera/depth/image_raw"
-    cam_frame: "camera_rgb_optical_frame"
+    # cam_info_topic: "/camera/rgb/camera_info"
+    # image_topic: "/camera/rgb/image_raw"
+    # cam_frame: "camera_rgb_optical_frame"
+    # data_type: image
+    # depth_topic: "/camera/depth/image_raw"
+  semantic_cam: # for color camera
+    # channels: ['rgb']
+    # fusion: ['color']
+    topic_name: '/front_cam/semantic_image'
+    camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized'
+    fusion_info_topic_name: '/front_cam/fusion_info'
+    data_type: image
+  front_cam_pointcloud:
+    channels: []
+    fusion: []
+    topic_name: '/camera/depth/points'
+    data_type: pointcloud
 
 
 #### Publishers ########
 publishers:
   elevation_map_raw:
-    layers: ['elevation', 'traversability', 'variance','rgb','sem_fil', 'person', 'sheep', 'sofa']
+    layers: ['elevation', 'traversability', 'variance','rgb','sem_fil', 'person', 'chair', 'sofa']
     basic_layers: ['elevation']
     fps: 5.0
   elevation_map_filter:

diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp
@@ -49,6 +49,7 @@
 
 #include <elevation_map_msgs/CheckSafety.h>
 #include <elevation_map_msgs/Initialize.h>
+#include <elevation_map_msgs/FusionInfo.h>
 
 #include "elevation_mapping_cupy/elevation_mapping_wrapper.hpp"
 
@@ -70,13 +71,21 @@ class ElevationMappingNode {
   using CameraSync = message_filters::Synchronizer<CameraPolicy>;
   using CameraSyncPtr = std::shared_ptr<CameraSync>;
 
+  using FusionInfoSubscriber = message_filters::Subscriber<elevation_map_msgs::FusionInfo>;
+  using FusionInfoSubscriberPtr = std::shared_ptr<FusionInfoSubscriber>;
+  using CameraFusionPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo, elevation_map_msgs::FusionInfo>;
+  using CameraFusionSync = message_filters::Synchronizer<CameraFusionPolicy>;
+  using CameraFusionSyncPtr = std::shared_ptr<CameraFusionSync>;
+
  private:
   void readParameters();
   void setupMapPublishers();
   void pointcloudCallback(const sensor_msgs::PointCloud2& cloud, const std::string& key);
   void inputImage(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg,
                   const std::vector<std::string>& channels, const std::vector<std::string>& fusion_methods);
   void imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg);
+  void imageFusionCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const elevation_map_msgs::FusionInfoConstPtr& fusion_info_msg);
+  // void multiLayerImageCallback(const elevation_map_msgs::MultiLayerImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg);
   void publishAsPointCloud(const grid_map::GridMap& map) const;
   bool getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response);
   bool checkSafety(elevation_map_msgs::CheckSafety::Request& request, elevation_map_msgs::CheckSafety::Response& response);
@@ -101,7 +110,9 @@ class ElevationMappingNode {
   std::vector<ros::Subscriber> pointcloudSubs_;
   std::vector<ImageSubscriberPtr> imageSubs_;
   std::vector<CameraInfoSubscriberPtr> cameraInfoSubs_;
+  std::vector<FusionInfoSubscriberPtr> fusionInfoSubs_;
   std::vector<CameraSyncPtr> cameraSyncs_;
+  std::vector<CameraFusionSyncPtr> cameraFusionSyncs_;
   std::vector<ros::Publisher> mapPubs_;
   tf::TransformBroadcaster tfBroadcaster_;
   ros::Publisher alivePub_;

diff --git a/elevation_mapping_cupy/launch/anymal.launch b/elevation_mapping_cupy/launch/anymal.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/anymal_parameters.yaml"/>
-        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/anymal_sensor_parameter.yaml"/>
+        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/setups/anymal/anymal_parameters.yaml"/>
+        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/setups/anymal/anymal_sensor_parameter.yaml"/>
     </node>
 </launch>
diff --git a/elevation_mapping_cupy/launch/turtlesim_init.launch b/elevation_mapping_cupy/launch/turtlesim_init.launch
@@ -3,13 +3,14 @@
     <arg name="rviz_config" default="$(find elevation_mapping_cupy)/rviz/turtle_semantic_example.rviz"/>
     <arg name="model" default="waffle" doc="model type [burger, waffle, waffle_pi]"/>
     <arg name="x_pos" default="-2.0"/>
-    <arg name="y_pos" default="-0.5"/>
+    <arg name="y_pos" default="1.5"/>
     <arg name="z_pos" default="0.0"/>
 
     <param name="/use_sim_time" type="bool" value="$(arg use_sim_time)"/>
 
     <include file="$(find gazebo_ros)/launch/empty_world.launch">
-        <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_world.world"/>
+        <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_house.world"/>
+        <!-- <arg name="world_name" value="$(find elevation_mapping_cupy)/config/worlds/house.world"/> -->
         <arg name="paused" value="false"/>
         <arg name="use_sim_time" value="true"/>
         <arg name="gui" value="false"/>

diff --git a/elevation_mapping_cupy/launch/turtlesim_semantic_image_example.launch b/elevation_mapping_cupy/launch/turtlesim_semantic_image_example.launch
@@ -7,16 +7,16 @@
     </include>
 
 
-    <node pkg="semantic_sensor" type="image_node.py" name="front_cam" args="front_cam"
+    <node pkg="semantic_sensor" type="image_node.py" name="front_cam" args="front_cam_image"
           output="screen">
-        <!-- <rosparam command="load" file="$(find elevation_mapping_cupy)/config/setups/turtle_bot/image_semantics.yaml"/> -->
-        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/setups/turtle_bot/turtle_bot_image_semantics.yaml"/>
+        <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_image_semantics.yaml"/>
+            <rosparam command="load" file="$(find elevation_mapping_cupy)/config/setups/turtle_bot/turtle_bot_semantics.yaml"/>
         </node>
 </launch>
diff --git a/elevation_mapping_cupy/rviz/turtle_example.rviz b/elevation_mapping_cupy/rviz/turtle_example.rviz
@@ -6,7 +6,7 @@ Panels:
       Expanded:
         - /Global Options1
       Splitter Ratio: 0.5768463015556335
-    Tree Height: 658
+    Tree Height: 229
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -22,10 +22,9 @@ Panels:
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: Image
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -57,14 +56,11 @@ Visualization Manager:
       Color: 200; 200; 200
       Color Layer: rgb
       Color Transformer: ColorLayer
-      ColorMap: default
       Enabled: true
-      Grid Cell Decimation: 1
-      Grid Line Thickness: 0.10000000149011612
       Height Layer: elevation
       Height Transformer: ""
       History Length: 1
-      Invert ColorMap: false
+      Invert Rainbow: false
       Max Color: 238; 238; 236
       Max Intensity: 1
       Min Color: 32; 74; 135
@@ -73,7 +69,7 @@ Visualization Manager:
       Show Grid Lines: true
       Topic: /elevation_mapping/elevation_map_raw
       Unreliable: false
-      Use ColorMap: true
+      Use Rainbow: true
       Value: true
     - Alpha: 1
       Class: rviz/RobotModel
@@ -178,6 +174,30 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /camera/rgb/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      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
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 255; 255; 255
@@ -222,17 +242,19 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5947964191436768
+      Pitch: 0.5147963762283325
       Target Frame: base_footprint
-      Yaw: 3.0983715057373047
+      Yaw: 3.1033706665039062
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
   Height: 1043
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001f700000359fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000359000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f0000053bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000560000053b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005afc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005830000035900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f70000035dfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001aa000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001eb000000d30000001600fffffffb0000000a0049006d00610067006501000002c4000000d40000001600ffffff000000010000015f0000053bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000560000053b000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005afc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000005830000035d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -242,5 +264,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1920
-  X: 1920
-  Y: 0
+  X: 2280
+  Y: 105