Add merged odometry to firmware message converter

leo_fw/CMakeLists.txt

@@ -7,6 +7,7 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(leo_msgs REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
@@ -26,10 +27,12 @@ ament_target_dependencies(leo_fw
   sensor_msgs
   leo_msgs
   nav_msgs
+  std_srvs
 )
 rclcpp_components_register_node(leo_fw
   PLUGIN "leo_fw::FirmwareMessageConverter"
   EXECUTABLE "firmware_message_converter"
+  EXECUTOR "MultiThreadedExecutor"
 )
 
 ament_python_install_package(${PROJECT_NAME})

leo_fw/package.xml

@@ -36,13 +36,15 @@
   <build_depend>rclcpp</build_depend>
   <build_depend>rclcpp_components</build_depend>
   <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
   <build_depend>yaml-cpp</build_depend>
 
   <exec_depend>leo_msgs</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclcpp_components</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
 
   <!-- Test -->
   <test_depend>ament_lint_auto</test_depend>

leo_fw/src/firmware_message_converter.cpp

@@ -28,6 +28,7 @@
 #include "nav_msgs/msg/odometry.hpp"
 #include "sensor_msgs/msg/imu.hpp"
 #include "sensor_msgs/msg/joint_state.hpp"
+#include "std_srvs/srv/trigger.hpp"
 
 #include "leo_msgs/msg/imu.hpp"
 #include "leo_msgs/msg/wheel_odom.hpp"
@@ -39,6 +40,9 @@
 
 using namespace std::chrono_literals;
 using std::placeholders::_1;
+using std::placeholders::_2;
+
+constexpr double PI = 3.141592653;
 
 namespace leo_fw
 {
@@ -57,6 +61,17 @@ class FirmwareMessageConverter : public rclcpp::Node
         &FirmwareMessageConverter::set_imu_calibration_callback, this,
         std::placeholders::_1, std::placeholders::_2));
 
+    client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    reset_odometry_client_ = create_client<std_srvs::srv::Trigger>(
+      "firmware/reset_odometry",
+      rmw_qos_profile_services_default,
+      client_cb_group_);
+
+    reset_odometry_service_ = create_service<std_srvs::srv::Trigger>(
+      "reset_odometry",
+      std::bind(
+        &FirmwareMessageConverter::reset_odometry_callback, this, std::placeholders::_1,
+        std::placeholders::_2));
 
     robot_frame_id_ = declare_parameter("robot_frame_id", robot_frame_id_);
     odom_frame_id_ = declare_parameter("odom_frame_id", odom_frame_id_);
@@ -94,7 +109,7 @@ class FirmwareMessageConverter : public rclcpp::Node
     joint_states_pub_->publish(joint_states);
   }
 
-  void wheel_odom_callback(const leo_msgs::msg::WheelOdom::SharedPtr msg) const
+  void wheel_odom_callback(const leo_msgs::msg::WheelOdom::SharedPtr msg)
   {
     nav_msgs::msg::Odometry wheel_odom;
     wheel_odom.header.frame_id = odom_frame_id_;
@@ -107,6 +122,8 @@ class FirmwareMessageConverter : public rclcpp::Node
     wheel_odom.pose.pose.orientation.z = std::sin(msg->pose_yaw * 0.5F);
     wheel_odom.pose.pose.orientation.w = std::cos(msg->pose_yaw * 0.5F);
 
+    velocity_linear_x = msg->velocity_lin;
+
     for (int i = 0; i < 6; i++) {
       wheel_odom.twist.covariance[i * 7] =
         wheel_odom_twist_covariance_diagonal_[i];
@@ -115,7 +132,7 @@ class FirmwareMessageConverter : public rclcpp::Node
     wheel_odom_pub_->publish(wheel_odom);
   }
 
-  void mecanum_odom_callback(const leo_msgs::msg::WheelOdomMecanum::SharedPtr msg) const
+  void mecanum_odom_callback(const leo_msgs::msg::WheelOdomMecanum::SharedPtr msg)
   {
     nav_msgs::msg::Odometry wheel_odom;
     wheel_odom.header.frame_id = odom_frame_id_;
@@ -129,6 +146,9 @@ class FirmwareMessageConverter : public rclcpp::Node
     wheel_odom.pose.pose.orientation.z = std::sin(msg->pose_yaw * 0.5F);
     wheel_odom.pose.pose.orientation.w = std::cos(msg->pose_yaw * 0.5F);
 
+    velocity_linear_x = msg->velocity_lin_x;
+    velocity_linear_y = msg->velocity_lin_y;
+
     for (int i = 0; i < 6; i++) {
       wheel_odom.twist.covariance[i * 7] =
         wheel_odom_mecanum_twist_covariance_diagonal_[i];
@@ -137,7 +157,7 @@ class FirmwareMessageConverter : public rclcpp::Node
     wheel_odom_mecanum_pub_->publish(wheel_odom);
   }
 
-  void imu_callback(const leo_msgs::msg::Imu::SharedPtr msg) const
+  void imu_callback(const leo_msgs::msg::Imu::SharedPtr msg)
   {
     sensor_msgs::msg::Imu imu;
     imu.header.frame_id = tf_frame_prefix_ + imu_frame_id_;
@@ -149,6 +169,8 @@ class FirmwareMessageConverter : public rclcpp::Node
     imu.linear_acceleration.y = msg->accel_y;
     imu.linear_acceleration.z = msg->accel_z;
 
+    velocity_angular_z = imu.angular_velocity.z;
+
     for (int i = 0; i < 3; i++) {
       imu.angular_velocity_covariance[i * 4] =
         imu_angular_velocity_covariance_diagonal_[i];
@@ -159,6 +181,52 @@ class FirmwareMessageConverter : public rclcpp::Node
     imu_pub_->publish(imu);
   }
 
+  void merged_odometry_callback()
+  {
+    nav_msgs::msg::Odometry merged_odom;
+    merged_odom.header.frame_id = odom_frame_id_;
+    merged_odom.child_frame_id = tf_frame_prefix_ + robot_frame_id_;
+    merged_odom.header.stamp = now();
+    merged_odom.twist.twist.linear.x = velocity_linear_x;
+    merged_odom.twist.twist.linear.y = velocity_linear_y;
+    merged_odom.twist.twist.angular.z = velocity_angular_z;
+
+    double move_x = velocity_linear_x * std::cos(odom_merged_yaw) - velocity_linear_y * std::sin(
+      odom_merged_yaw);
+    double move_y = velocity_linear_x * std::sin(odom_merged_yaw) + velocity_linear_y * std::cos(
+      odom_merged_yaw);
+
+    odom_merged_position.x += move_x * 0.01;
+    odom_merged_position.y += move_y * 0.01;
+
+    odom_merged_yaw += velocity_angular_z * 0.01;
+
+    if (odom_merged_yaw > 2.0 * PI) {
+      odom_merged_yaw -= 2.0 * PI;
+    } else if (odom_merged_yaw < 0.0) {
+      odom_merged_yaw += 2.0 * PI;
+    }
+
+    merged_odom.pose.pose.position.x = odom_merged_position.x;
+    merged_odom.pose.pose.position.y = odom_merged_position.y;
+    merged_odom.pose.pose.orientation.z = std::sin(odom_merged_yaw * 0.5F);
+    merged_odom.pose.pose.orientation.w = std::cos(odom_merged_yaw * 0.5F);
+
+    const std::vector<double> * merged_covariance;
+    if (wheel_odom_mecanum_pub_) {
+      merged_covariance = &wheel_odom_mecanum_twist_covariance_diagonal_;
+    } else {
+      merged_covariance = &wheel_odom_twist_covariance_diagonal_;
+    }
+
+    for (int i = 0; i < 5; i++) {
+      merged_odom.twist.covariance[i * 7] = (*merged_covariance)[i];
+    }
+    merged_odom.twist.covariance[35] = imu_angular_velocity_covariance_diagonal_[2];
+
+    odom_merged_pub_->publish(merged_odom);
+  }
+
   void set_imu_calibration_callback(
     const std::shared_ptr<leo_msgs::srv::SetImuCalibration::Request> request,
     std::shared_ptr<leo_msgs::srv::SetImuCalibration::Response> response)
@@ -177,6 +245,37 @@ class FirmwareMessageConverter : public rclcpp::Node
     response->success = true;
   }
 
+  void reset_odometry_callback(
+    const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
+    std::shared_ptr<std_srvs::srv::Trigger::Response> res)
+  {
+    constexpr std::chrono::seconds callback_timeout = std::chrono::seconds(3);
+
+    odom_merged_position.x = 0.0;
+    odom_merged_position.y = 0.0;
+    odom_merged_yaw = 0.0;
+
+    auto reset_request = std::make_shared<std_srvs::srv::Trigger_Request>();
+    auto future = reset_odometry_client_->async_send_request(reset_request);
+    auto result_status = future.wait_for(callback_timeout);
+
+    if (result_status == std::future_status::ready) {
+      if (future.get()->success) {
+        res->success = true;
+        res->message = "Odometry reset successful.";
+      } else {
+        res->success = false;
+        res->message = "Failed to reset odometry.";
+      }
+    } else if (result_status == std::future_status::timeout) {
+      res->success = false;
+      res->message = "Firmware service timeout.";
+    } else {
+      res->success = false;
+      res->message = "Firmware service call deffered.";
+    }
+  }
+
   void load_yaml_bias()
   {
     YAML::Node node;
@@ -249,9 +348,11 @@ class FirmwareMessageConverter : public rclcpp::Node
     if (wheel_odom_pub_ && wheel_odom_publishers == 0) {
       RCLCPP_INFO(
         get_logger(), "firmware/wheel_odom topic no longer has any publishers. "
-        "Shutting down wheel_odom_with_covariance publisher.");
+        "Shutting down wheel_odom_with_covariance and odometry_merged publishers.");
       wheel_odom_sub_.reset();
       wheel_odom_pub_.reset();
+      odom_merged_pub_.reset();
+      odom_merged_timer_->cancel();
     }
 
     if (!wheel_odom_pub_ && wheel_odom_publishers > 0) {
@@ -270,9 +371,11 @@ class FirmwareMessageConverter : public rclcpp::Node
       RCLCPP_INFO(
         get_logger(),
         "firmware/wheel_odom_mecanum topic no longer has any publishers. "
-        "Shutting down wheel_odom_with_covariance publisher.");
+        "Shutting down wheel_odom_with_covariance and odometry_merged publishers.");
       wheel_odom_mecanum_sub_.reset();
       wheel_odom_mecanum_pub_.reset();
+      odom_merged_pub_.reset();
+      odom_merged_timer_->cancel();
     }
 
     if (!wheel_odom_mecanum_pub_ && wheel_odom_mecanum_publishers > 0) {
@@ -295,9 +398,11 @@ class FirmwareMessageConverter : public rclcpp::Node
     if (imu_pub_ && imu_publishers == 0) {
       RCLCPP_INFO(
         get_logger(), "firmware/imu topic no longer has any publishers. "
-        "Shutting down imu/data_raw publisher.");
+        "Shutting down imu/data_raw and odometry_merged publishers.");
       imu_sub_.reset();
       imu_pub_.reset();
+      odom_merged_pub_.reset();
+      odom_merged_timer_->cancel();
     }
 
     if (!imu_pub_ && imu_publishers > 0) {
@@ -309,8 +414,27 @@ class FirmwareMessageConverter : public rclcpp::Node
         imu_topic_, rclcpp::QoS(5).best_effort(),
         std::bind(&FirmwareMessageConverter::imu_callback, this, _1));
     }
+
+    if (imu_pub_ && (wheel_odom_mecanum_pub_ || wheel_odom_pub_) && !odom_merged_pub_) {
+      RCLCPP_INFO(
+        get_logger(), "Both firmware/imu and (firmware/wheel_odom or "
+        "firmware/wheel_odom_mecanum) topics are advertised. "
+        "Advertising odometry_merged topic.");
+      odom_merged_pub_ = create_publisher<nav_msgs::msg::Odometry>("odometry_merged", 10);
+      odom_merged_timer_ =
+        create_wall_timer(
+        10ms,
+        std::bind(&FirmwareMessageConverter::merged_odometry_callback, this));
+    }
   }
 
+  // Merged Odom variables
+  double velocity_linear_x;
+  double velocity_linear_y;
+  double velocity_angular_z;
+  double odom_merged_yaw;
+  geometry_msgs::msg::Point odom_merged_position;
+
   // Parameters
   std::string robot_frame_id_ = "base_link";
   std::string odom_frame_id_ = "odom";
@@ -335,13 +459,15 @@ class FirmwareMessageConverter : public rclcpp::Node
   std::string wheel_odom_mecanum_topic_;
   std::string imu_topic_;
 
-  // Timer
+  // Timers
   rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::TimerBase::SharedPtr odom_merged_timer_;
 
   // Publishers
   rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_states_pub_;
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr wheel_odom_pub_;
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr wheel_odom_mecanum_pub_;
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_merged_pub_;
   rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
 
   // Subscriptions
@@ -350,8 +476,15 @@ class FirmwareMessageConverter : public rclcpp::Node
   rclcpp::Subscription<leo_msgs::msg::WheelOdomMecanum>::SharedPtr wheel_odom_mecanum_sub_;
   rclcpp::Subscription<leo_msgs::msg::Imu>::SharedPtr imu_sub_;
 
-  // Service
+  // Services
   rclcpp::Service<leo_msgs::srv::SetImuCalibration>::SharedPtr set_imu_calibration_service;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reset_odometry_service_;
+
+  // Clients
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr reset_odometry_client_;
+
+  // Callback Groups
+  rclcpp::CallbackGroup::SharedPtr client_cb_group_;
 };
 
 }  // namespace leo_fw