Add odometry covariance to ros node (#283)

* Add fixed covariance to odometry msg * Add default value just in case
PRBonn · Mar 18, 2024 · f1dc4c9 · f1dc4c9
1 parent 6947e8e
commit f1dc4c9
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 0 deletions.
diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py
@@ -49,6 +49,10 @@ class config:
     initial_threshold: float = 2.0
     min_motion_th: float = 0.1
 
+    # Covariance diagonal values
+    position_covariance: float = 0.1
+    orientation_covariance: float = 0.1
+
 
 def generate_launch_description():
     use_sim_time = LaunchConfiguration("use_sim_time", default="true")
@@ -88,6 +92,8 @@ def generate_launch_description():
                 "voxel_size": config.voxel_size,
                 "initial_threshold": config.initial_threshold,
                 "min_motion_th": config.min_motion_th,
+                "position_covariance": config.position_covariance,
+                "orientation_covariance": config.orientation_covariance,
                 # ROS CLI arguments
                 "publish_debug_clouds": visualize,
                 "use_sim_time": use_sim_time,

diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
@@ -57,6 +57,8 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
     odom_frame_ = declare_parameter<std::string>("odom_frame", odom_frame_);
     publish_odom_tf_ = declare_parameter<bool>("publish_odom_tf", publish_odom_tf_);
     publish_debug_clouds_ = declare_parameter<bool>("publish_debug_clouds", publish_debug_clouds_);
+    position_covariance_ = declare_parameter<double>("position_covariance", 0.1);
+    orientation_covariance_ = declare_parameter<double>("orientation_covariance",0.1);
 
     kiss_icp::pipeline::KISSConfig config;
     config.max_range = declare_parameter<double>("max_range", config.max_range);
@@ -163,6 +165,13 @@ void OdometryServer::PublishOdometry(const Sophus::SE3d &pose,
     odom_msg.header.stamp = stamp;
     odom_msg.header.frame_id = odom_frame_;
     odom_msg.pose.pose = tf2::sophusToPose(pose);
+    odom_msg.pose.covariance.fill(0.0);
+    odom_msg.pose.covariance[0] = position_covariance_;
+    odom_msg.pose.covariance[7] = position_covariance_;
+    odom_msg.pose.covariance[14] = position_covariance_;
+    odom_msg.pose.covariance[21] = orientation_covariance_;
+    odom_msg.pose.covariance[28] = orientation_covariance_;
+    odom_msg.pose.covariance[35] = orientation_covariance_;
     odom_publisher_->publish(std::move(odom_msg));
 }
 

diff --git a/ros/src/OdometryServer.hpp b/ros/src/OdometryServer.hpp
@@ -85,6 +85,10 @@ class OdometryServer : public rclcpp::Node {
     /// Global/map coordinate frame.
     std::string odom_frame_{"odom"};
     std::string base_frame_{};
+
+    /// Covariance diagonal
+    double position_covariance_;
+    double orientation_covariance_;
 };
 
 }  // namespace kiss_icp_ros