From 773e31882862f30a96ca26f70f1948f35c987311 Mon Sep 17 00:00:00 2001
From: Juli <nubertj@ethz.ch>
Date: Tue, 2 Jul 2024 20:59:43 +0200
Subject: [PATCH 1/5] added elevation mapping config

---
 .../config/elevation_mapping/default.yaml     | 22 +++++++++++++++++++
 .../config/elevation_mapping/vlp16.yaml       | 12 ++++++++++
 .../launch/smb_msf_graph_o3d_replay.launch    | 10 +++++++++
 3 files changed, 44 insertions(+)
 create mode 100644 smb_msf_graph/config/elevation_mapping/default.yaml
 create mode 100644 smb_msf_graph/config/elevation_mapping/vlp16.yaml

diff --git a/smb_msf_graph/config/elevation_mapping/default.yaml b/smb_msf_graph/config/elevation_mapping/default.yaml
new file mode 100644
index 0000000..3807ff4
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/smb_msf_graph/config/elevation_mapping/vlp16.yaml b/smb_msf_graph/config/elevation_mapping/vlp16.yaml
new file mode 100644
index 0000000..aba2d97
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch b/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
index 3814af6..2db6346 100644
--- a/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
+++ b/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
@@ -34,4 +34,14 @@
         <arg name="cloud_topic" value="$(arg cloud_topic)" />
     </include>
 
+    <!-- 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>
+
 </launch>
\ No newline at end of file

From 06b5f4814980f9fec9ab772f5bc78eea21fbfa55 Mon Sep 17 00:00:00 2001
From: Julian Nubert <nubertj@ethz.ch>
Date: Wed, 24 Jul 2024 19:21:36 +0200
Subject: [PATCH 2/5] update to latest version

---
 smb_msf_graph/launch/smb_msf_graph.launch     |  1 +
 .../launch/smb_msf_graph_o3d_replay.launch    | 23 +++++++++----------
 .../tutorial/config/1_smb_graph_params.yaml   | 11 +++++----
 3 files changed, 18 insertions(+), 17 deletions(-)

diff --git a/smb_msf_graph/launch/smb_msf_graph.launch b/smb_msf_graph/launch/smb_msf_graph.launch
index 8677771..4f30724 100644
--- a/smb_msf_graph/launch/smb_msf_graph.launch
+++ b/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)">
diff --git a/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch b/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
index 2db6346..8457464 100644
--- a/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
+++ b/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
@@ -1,14 +1,13 @@
-<?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="cloud_topic" default="/rslidar/points"/>
+    <arg name="imu_topic_name" default="/imu"/>
+    <arg name="lidar_odometry_topic_name" default="/mapping_node/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"/>
@@ -16,13 +15,13 @@
 
     <!-- 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 -->
@@ -44,4 +43,4 @@
         <rosparam command="load" file="$(arg elevation_mapping_parameter_file)"/>
     </node>
 
-</launch>
\ No newline at end of file
+</launch>
diff --git a/smb_msf_graph/tutorial/config/1_smb_graph_params.yaml b/smb_msf_graph/tutorial/config/1_smb_graph_params.yaml
index a252ad0..71d92f5 100644
--- a/smb_msf_graph/tutorial/config/1_smb_graph_params.yaml
+++ b/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)

From 409b6d7ba37772c63ce6620028de70b81f6f827f Mon Sep 17 00:00:00 2001
From: Julian Nubert <nubertj@ethz.ch>
Date: Wed, 24 Jul 2024 19:22:57 +0200
Subject: [PATCH 3/5] add elevation_mapping as run dependency

---
 smb_msf_graph/package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/smb_msf_graph/package.xml b/smb_msf_graph/package.xml
index 0692566..ddae527 100644
--- a/smb_msf_graph/package.xml
+++ b/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>

From 5728142153acb72d16282c48b6486d3970fdecc0 Mon Sep 17 00:00:00 2001
From: Julian Nubert <nubertj@ethz.ch>
Date: Thu, 25 Jul 2024 00:47:21 +0200
Subject: [PATCH 4/5] template for potential tf publisher

---
 smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch b/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
index 8457464..62bd564 100644
--- a/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
+++ b/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
@@ -29,8 +29,8 @@
 
     <!-- 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>
 
     <!-- Elevation mapping node -->
@@ -43,4 +43,7 @@
         <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>

From d128d120b05ff5c10f9c9b88c4fecf2453b29287 Mon Sep 17 00:00:00 2001
From: Juli <nubertj@ethz.ch>
Date: Tue, 12 Nov 2024 14:32:27 +0100
Subject: [PATCH 5/5] change of o3d slam topic

---
 smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch b/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
index 62bd564..1cf4eb3 100644
--- a/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
+++ b/smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
@@ -6,7 +6,8 @@
 
     <!-- Parameters -->
     <arg name="imu_topic_name" default="/imu"/>
-    <arg name="lidar_odometry_topic_name" default="/mapping_node/scan2map_odometry"/>
+    <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 -->