added two plugins: semantic_filter and robot_centric_elevation, I changed a bit the plugin manager to accept different arguments. The datatype has been changed to float32, removed lonomy related files, added a few tests

Author: gerni17

elevation_mapping_cupy/config/parameters.yaml

@@ -40,9 +40,9 @@ max_unsafe_n: 10                                # if the number of cells under s
 overlap_clear_range_xy: 4.0                     # xy range [m] for clearing overlapped area. this defines the valid area for overlap clearance. (used for multi floor setting)
 overlap_clear_range_z: 2.0                      # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting)
 
-map_frame: 'enu'                               # The map frame where the odometry source uses.
+map_frame: 'odom'                               # The map frame where the odometry source uses.
 base_frame: 'base_footprint'                              # The robot's base frame. This frame will be a center of the map.
-corrected_map_frame: 'enu'
+corrected_map_frame: 'corrected_odom'
 
 #### Feature toggles ########
 enable_edge_sharpen: true
@@ -81,14 +81,14 @@ publishers:
     basic_layers: ['elevation', 'traversability']
     fps: 2.0
   elevation_map_filter:
-    layers: ['min_filter', 'smooth', 'inpaint', 'upper_bound','rgb','sem_fil']
+    layers: ['min_filter', 'smooth', 'inpaint', 'upper_bound','rgb']
     basic_layers: ['min_filter']
     fps: 3.0
 
 #### Initializer ########
 initialize_method: 'linear'                                         # Choose one from 'nearest', 'linear', 'cubic'
-initialize_frame_id: ['base_footprint']   # One tf (like ['footprint'] ) initializes a square around it.
-initialize_tf_offset: [0.0, 0.0, 0.0, 0.0]                          # z direction. Should be same number as initialize_frame_id.
+initialize_frame_id: ['base_footprint'] # One tf (like ['footprint']   # One tf (like ['footprint'] ) initializes a square around it.
+initialize_tf_offset: [0.0]                          # z direction. Should be same number as initialize_frame_id.
 dilation_size_initialize: 5                                         # dilation size after the init.
 initialize_tf_grid_size: 0.4                                        # This is not used if number of tf is more than 3.
 use_initializer_at_start: true                                      # Use initializer when the node starts.

elevation_mapping_cupy/config/plugin_config.yaml

@@ -34,7 +34,17 @@ smooth_filter_1:
   layer_name: "smooth_1"
   extra_params:
     input_layer_name: "inpaint"
-
+robot_centric_elevation:                                  # Use the same name as your file name.
+#  type: "robot_centric_elevation"
+  enable: False                                # weather to load this plugin
+  fill_nan: False                              # Fill nans to invalid cells of elevation layer.
+  is_height_layer: True                       # If this is a height layer (such as elevation) or not (such as traversability)
+  layer_name: "robot_centric_elevation"                 # The layer name.
+  extra_params:                               # This params are passed to the plugin class on initialization.
+#    add_value: 2.0                            # Example param
+    resolution: 0.04
+    threshold: 1.1
+    use_threshold: True
 semantic_filter:
   type: "semantic_filter"
   enable: False

elevation_mapping_cupy/config/sensor_parameter.yaml

@@ -1,8 +1,10 @@
 subscribers:
-#  sensor_name:
-#    channels: ['feat_0','feat_1']
-#    fusion: ['average','average']
-#    topic_name: '/elevation_mapping/pointcloud_semantic'
+#  sensor_name: #if you have multiple sensors assign different names
+#    channels: ['features','person'] # names of the channels, for sem seg it should be the same name as the
+                                    # pretrained network uses
+#    fusion: ['average','class_average'] # the fusion algorithm type used.
+                                    # Pick between: ['color', 'average','class_average']
+#    topic_name: '/elevation_mapping/pointcloud_semantic' # of the published pointcloud subscribed by the el_map
   front_cam:
     channels: ['rgb','feat_0','feat_1','grass','tree','fence','person']
     fusion: ['color','average','average','class_average','class_average','class_average','class_average']
@@ -20,11 +22,12 @@ subscribers:
       input_size: [80, 160]
       projection_type: "nonlinear"
 
-    cam_info_topic: "/zed2i/zed_node/depth/camera_info"
-    image_topic: "/zed2i/zed_node/left/image_rect_color"
-    depth_topic: "/zed2i/zed_node/depth/depth_registered"
-    cam_frame: "zed2i_right_camera_optical_frame"
-    confidence_topic: "/zed2i/zed_node/confidence/confidence_map"
+    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
+    confidence: False
+    confidence_topic: "/camera/depth/image_raw"
     confidence_threshold: 10
     feature_extractor: False
 

elevation_mapping_cupy/launch/semantic_elevation_lonomy_single.launch

@@ -0,0 +1,19 @@
+<launch>
+  <arg name="use_sim_time" default="true" />
+  <arg name="rviz_config" default="$(find elevation_mapping_cupy)/rviz/turtle_semantic_example.rviz" />
+  <param name="/use_sim_time" type="bool" value="$(arg use_sim_time)" />
+
+
+  <!-- Start gazebo simulation. -->
+  <include file="$(find turtlebot3_gazebo)/launch/turtlebot3_world.launch"/>
+
+  <!-- Publish turtlebot3 tf's. -->
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="waffle_state_publisher"/>
+
+  <node pkg="semantic_pointcloud" 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 name="rviz" pkg="rviz" type="rviz" args="-d $(arg rviz_config)"/>
+
+</launch>

elevation_mapping_cupy/launch/turtlesim_pointcloud.launch

@@ -0,0 +1,19 @@
+<launch>
+    <arg name="use_sim_time" default="true" />
+    <arg name="rviz_config" default="$(find elevation_mapping_cupy)/rviz/turtle_semantic_example.rviz" />
+    <param name="/use_sim_time" type="bool" value="$(arg use_sim_time)" />
+
+
+   <include file="$(find elevation_mapping_cupy)/launch/turtlesim_pointcloud.launch"/>
+
+
+
+    <!-- Launch elevation mapping node. -->
+    <node pkg="elevation_mapping_cupy" type="elevation_mapping_node" name="elevation_mapping" output="screen" >
+        <param name="/use_sim_time" type="bool" value="$(arg use_sim_time)" />
+        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/parameters.yaml" />
+        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/sensor_parameter.yaml" />
+    </node>
+
+
+</launch>