Add merged odometry to firmware_message_converter

leo_fw/src/firmware_message_converter.cpp

@@ -9,13 +9,16 @@
 #include "nav_msgs/Odometry.h"
 #include "sensor_msgs/Imu.h"
 #include "sensor_msgs/JointState.h"
+#include "std_srvs/Trigger.h"
 
 #include "leo_msgs/Imu.h"
 #include "leo_msgs/WheelOdom.h"
 #include "leo_msgs/WheelStates.h"
 
 #include "leo_msgs/SetImuCalibration.h"
 
+constexpr double PI = 3.141592653;
+
 static ros::Subscriber wheel_states_sub;
 static ros::Publisher joint_states_pub;
 static bool joint_states_advertised = false;
@@ -28,6 +31,16 @@ static ros::Subscriber imu_sub;
 static ros::Publisher imu_pub;
 static bool imu_advertised = false;
 
+static ros::Publisher odom_merged_pub;
+static ros::Timer odom_merged_timer;
+static geometry_msgs::Point odom_merged_position;
+static double odom_merged_yaw;
+static bool odom_merged_advertised = false;
+static double velocity_linear_x = 0;
+static double velocity_angular_z = 0;
+static ros::ServiceClient odom_reset_client;
+static ros::ServiceServer reset_odometry_service;
+
 ros::ServiceServer set_imu_calibration_service;
 
 std::string robot_frame_id = "base_link";
@@ -37,6 +50,7 @@ std::vector<std::string> wheel_joint_names = {
     "wheel_FL_joint", "wheel_RL_joint", "wheel_FR_joint", "wheel_RR_joint"};
 std::vector<double> wheel_odom_twist_covariance_diagonal = {0.0001, 0.0, 0.0,
                                                             0.0,    0.0, 0.001};
+
 std::vector<double> imu_angular_velocity_covariance_diagonal = {
     0.000001, 0.000001, 0.00001};
 std::vector<double> imu_linear_acceleration_covariance_diagonal = {0.001, 0.001,
@@ -90,12 +104,15 @@ void wheel_odom_callback(const leo_msgs::WheelOdomPtr& msg) {
   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];
 
   wheel_odom_pub.publish(wheel_odom);
 }
+
 void imu_callback(const leo_msgs::ImuPtr& msg) {
   sensor_msgs::Imu imu;
   imu.header.frame_id = imu_frame_id;
@@ -107,6 +124,8 @@ void imu_callback(const leo_msgs::ImuPtr& msg) {
   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];
@@ -117,6 +136,35 @@ void imu_callback(const leo_msgs::ImuPtr& msg) {
   imu_pub.publish(imu);
 }
 
+void merge_odometry_callback(const ros::TimerEvent& events) {
+  nav_msgs::Odometry merged_odom;
+  merged_odom.header.frame_id = odom_frame_id;
+  merged_odom.child_frame_id = robot_frame_id;
+  merged_odom.header.stamp = ros::Time::now();
+  merged_odom.twist.twist.linear.x = velocity_linear_x;
+  merged_odom.twist.twist.angular.z = velocity_angular_z;
+
+  const float move_x = velocity_linear_x * std::cos(odom_merged_yaw);
+  const float move_y = velocity_linear_x * std::sin(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);
+
+  odom_merged_pub.publish(merged_odom);
+}
+
 void load_yaml_bias() {
   YAML::Node node;
   try {
@@ -173,6 +221,20 @@ bool set_imu_calibration_callback(leo_msgs::SetImuCalibrationRequest& req,
   return true;
 }
 
+bool reset_odometry_callback(std_srvs::TriggerRequest& req,
+                             std_srvs::TriggerResponse& res) {
+  odom_merged_position.x = 0.0F;
+  odom_merged_position.y = 0.0F;
+  odom_merged_yaw = 0.0F;
+  if (odom_reset_client.call(req, res)) {
+    res.success = true;
+    return true;
+  } else {
+    res.success = false;
+    return false;
+  }
+}
+
 int main(int argc, char** argv) {
   ros::init(argc, argv, "firmware_message_converter");
 
@@ -190,10 +252,16 @@ int main(int argc, char** argv) {
       ros::names::resolve("firmware/wheel_states");
   const std::string wheel_odom_topic =
       ros::names::resolve("firmware/wheel_odom");
+  const std::string wheel_odom_mecanum_topic =
+      ros::names::resolve("firmware/wheel_odom_mecanum");
   const std::string imu_topic = ros::names::resolve("firmware/imu");
 
   set_imu_calibration_service =
       nh.advertiseService("set_imu_calibration", set_imu_calibration_callback);
+  odom_reset_client =
+      nh.serviceClient<std_srvs::Trigger>("firmware/reset_odometry");
+  reset_odometry_service =
+      nh.advertiseService("reset_odometry", &reset_odometry_callback);
 
   ros::Rate rate(2);
   while (ros::ok()) {
@@ -209,18 +277,25 @@ int main(int argc, char** argv) {
     if (wheel_odom_advertised && wheel_odom_sub.getNumPublishers() == 0) {
       ROS_INFO(
           "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.shutdown();
       wheel_odom_pub.shutdown();
+      odom_merged_pub.shutdown();
+      odom_merged_timer.stop();
       wheel_odom_advertised = false;
+      odom_merged_advertised = false;
     }
     if (imu_advertised && imu_sub.getNumPublishers() == 0) {
       ROS_INFO(
           "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.shutdown();
       imu_pub.shutdown();
+      odom_merged_pub.shutdown();
+      odom_merged_timer.stop();
       imu_advertised = false;
+      odom_merged_advertised = false;
     }
 
     rate.sleep();
@@ -258,6 +333,16 @@ int main(int argc, char** argv) {
         imu_sub = nh.subscribe("firmware/imu", 5, imu_callback);
         imu_advertised = true;
       }
+      if (!odom_merged_advertised && imu_advertised && wheel_odom_advertised) {
+        ROS_INFO(
+            "Both firmware/imu and firmware/wheel_odom topics are advertised. "
+            "Advertising odometry_merged topic.");
+        odom_merged_pub =
+            nh.advertise<nav_msgs::Odometry>("odometry_merged", 10);
+        odom_merged_timer =
+            nh.createTimer(ros::Duration(0.01), merge_odometry_callback);
+        odom_merged_advertised = true;
+      }
     }
 
     rate.sleep();