added elevation mapping config

smb_msf_graph/config/elevation_mapping/default.yaml

@@ -0,0 +1,22 @@
+robot_pose_with_covariance_topic: ""
+track_point_x: 0
+track_point_y: 0
+track_point_z: 0
+min_update_rate: 0.5
+length_in_x: 25
+length_in_y: 25
+position_x: 0
+position_y: 0
+resolution: 0.1
+min_variance: 0.000009  #0.00005 --> changes fast
+max_variance: 0.002
+mahalanobis_distance_threshold: 2.5
+multi_height_noise: 0.0001
+visibility_cleanup_rate: 1.0
+scanning_duration: 0.3
+enable_visibility_cleanup: true
+sensor_processor:
+  beam_angle: 0.0006
+  beam_constant: 0.0015
+  min_radius: 0.018
+  type: laser

smb_msf_graph/config/elevation_mapping/vlp16.yaml

@@ -0,0 +1,12 @@
+input_sources:
+  elevation_mapping_sensor:
+    type: pointcloud
+    topic: /rslidar/points
+    queue_size: 1
+    publish_on_update: true
+    sensor_processor:
+      type: laser
+robot_base_frame_id: base_link
+map_frame_id: odom_graph_msf
+track_point_frame_id: base_link
+sensor_frame_id: rslidar

smb_msf_graph/launch/smb_msf_graph.launch

@@ -20,6 +20,7 @@
         <arg name="lidar_odometry_topic_name" value="$(arg lidar_odometry_topic_name)"/>
     </include>
 
+    <!-- Check whether Open3D-SLAM should be launched -->
     <group if="$(arg launch_o3d_slam)">
         <!-- Launch online_slam -->
         <include file="$(find open3d_slam_ros)/launch/development.launch" if="$(arg launch_o3d_slam)">

smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch

@@ -1,37 +1,50 @@
-<?xml version="1.0" encoding="UTF-8"?> 
+<?xml version="1.0" encoding="UTF-8"?>
 <launch>
 
     <!-- Parameters -->
-    <arg name="use_sim_time"              value="true" />
+    <arg name="use_sim_time" value="true"/>
 
     <!-- Parameters -->
-    <arg name="imu_topic_name"            default="/imu" />
-    <arg name="lidar_odometry_topic_name" default="/mapping/scan2map_odometry"/>
-    <arg name="launch_o3d_slam" default="true"/>
+    <arg name="imu_topic_name" default="/imu"/>
     <arg name="cloud_topic" default="/rslidar/points"/>
+    <arg name="lidar_odometry_topic_name" default="/mapping/scan2map_odometry"/>
+    <arg name="launch_o3d_slam" default="false"/>
 
     <!-- Config Files -->
     <arg name="core_graph_param_file" default="$(find smb_estimator_graph)/config/core/core_graph_params.yaml"/>
     <arg name="smb_graph_param_file" default="$(find smb_estimator_graph)/config/smb_specific/smb_graph_params.yaml"/>
 
     <!-- Alias to smb_estimator_graph replay script -->
     <include file="$(find smb_estimator_graph)/launch/smb_estimator_graph_replay.launch">
-        <arg name="use_sim_time" value="$(arg use_sim_time)" />
+        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
         <!-- Topics -->
-        <arg name="imu_topic_name" value="$(arg imu_topic_name)" />
-        <arg name="lidar_odometry_topic_name" value="$(arg lidar_odometry_topic_name)" />
+        <arg name="imu_topic_name" value="$(arg imu_topic_name)"/>
+        <arg name="lidar_odometry_topic_name" value="$(arg lidar_odometry_topic_name)"/>
         <!-- Config Files -->
-        <arg name="core_graph_param_file" value="$(arg core_graph_param_file)" />
-        <arg name="smb_graph_param_file"  value="$(arg smb_graph_param_file)" />
+        <arg name="core_graph_param_file" value="$(arg core_graph_param_file)"/>
+        <arg name="smb_graph_param_file" value="$(arg smb_graph_param_file)"/>
     </include>
 
     <!-- Load Description -->
     <include file="$(find smb_description)/launch/load.launch" if="$(arg use_sim_time)"/>
 
     <!-- Launch Open3D SLAM -->
     <include file="$(find smb_slam)/launch/online_slam.launch" if="$(arg launch_o3d_slam)">
-        <arg name="use_sim_time" value="$(arg use_sim_time)" />
-        <arg name="cloud_topic" value="$(arg cloud_topic)" />
+        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+        <arg name="cloud_topic" value="$(arg cloud_topic)"/>
     </include>
 
-</launch>
+    <!-- Elevation mapping node -->
+    <arg name="default_elevation_mapping_parameter_file" default="$(dirname)/../config/elevation_mapping/default.yaml"/>
+    <arg name="elevation_mapping_parameter_file" default="$(dirname)/../config/elevation_mapping/vlp16.yaml"/>
+
+    <!-- Elevation Mapping -->
+    <node pkg="elevation_mapping" type="elevation_mapping" name="elevation_mapping" output="log">
+        <rosparam command="load" file="$(arg default_elevation_mapping_parameter_file)"/>
+        <rosparam command="load" file="$(arg elevation_mapping_parameter_file)"/>
+    </node>
+
+    <!-- Static TF Publisher between world_graph_msf and odom -->
+    <!--    <node pkg="tf" type="static_transform_publisher" name="world_graph_msf_to_odom" args="0 0 0 0 0 0 world_graph_msf odom 100" />-->
+
+</launch>

smb_msf_graph/package.xml

@@ -12,5 +12,6 @@
 	<exec_depend>compslam_lio</exec_depend>
 	<exec_depend>open3d_slam_ros</exec_depend>
 	<exec_depend>smb_slam</exec_depend>
+	<exec_depend>elevation_mapping</exec_depend>
 
 </package>

smb_msf_graph/tutorial/config/1_smb_graph_params.yaml

@@ -1,14 +1,14 @@
 # Sensor Params
 sensor_params:
   ## Config
-  # TODO: Change here
-  useWheelOdometry: false
   useLioOdometry: false
-  # TODO: End of change
+  useWheelOdometrybetween: false
+  useWheelLinearVelocities: false
   useVioOdometry: false
   ## Rates
   lioOdometryRate: 10
-  wheelOdometryRate: 50
+  wheelOdometryBetweenRate: 50
+  wheelLinearVelocitiesRate: 50
   vioOdometryRate: 50
 
 # Alignment Parameters
@@ -20,7 +20,8 @@ noise_params:
   ## LiDAR
   lioPoseUnaryNoiseDensity: [ 0.05, 0.05, 0.05, 0.01, 0.01, 0.01 ] # StdDev, ORDER RPY(rad) - XYZ(meters)
   ## Wheel
-  wheelPoseBetweenNoiseDensity: [ 0.1, 0.1, 0.1, 0.06, 0.06, 0.06 ] # StdDev, ORDER RPY(rad) - XYZ(meters)
+  wheelPoseBetweenNoiseDensity: [ 0.2, 0.2, 0.2, 0.005, 0.005, 0.005 ] # StdDev, [ 10.0, 10.0, 10.0, 2.0, 2.0, 2.0 ]
+  wheelLinearVelocitiesNoiseDensity: [ 0.1, 0.1, 0.1 ] # StdDev, ORDER VxVyVz(meters/sec)
   ## VIO
   vioPoseBetweenNoiseDensity: [ 100.0, 100.0, 100.0, 0.1, 0.1, 0.1 ] # StdDev, ORDER RPY(rad) - XYZ(meters)
   vioPoseUnaryNoiseDensity: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 ] # StdDev, ORDER RPY(rad) - XYZ(meters)