changed some SBG translation stuff. changed topic, added new msgs for…

… visuals. add wait for reset

src/hardware/sbg_translator/include/node_sbg_translator.hpp

@@ -9,11 +9,12 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include "driverless_common/common.hpp"
-#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/vector3.hpp"
 #include "nav_msgs/msg/odometry.hpp"
+#include "nav_msgs/msg/path.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "sbg_driver/msg/sbg_ekf_euler.hpp"
 #include "sbg_driver/msg/sbg_ekf_nav.hpp"
@@ -39,7 +40,10 @@ class SBGTranslate : public rclcpp::Node {
 
     // publishers
     rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_;
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr raw_pose_pub_;
     rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
+    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
 
     // initial values
     float init_yaw_;
@@ -53,6 +57,10 @@ class SBGTranslate : public rclcpp::Node {
     sbg_driver::msg::SbgEkfEuler last_euler_msg_;
     std::vector<float> state_;
 
+    // store path
+    nav_msgs::msg::Path path_msg_;
+    rclcpp::Time last_pub_time_;
+
     bool received_imu_ = false;
     bool received_nav_ = false;
     bool received_euler_ = false;
@@ -63,13 +71,23 @@ class SBGTranslate : public rclcpp::Node {
     sensor_msgs::msg::Imu make_imu_msg(nav_msgs::msg::Odometry odom_msg) {
         sensor_msgs::msg::Imu imu_msg;
         imu_msg.header.stamp = odom_msg.header.stamp;
-        imu_msg.header.frame_id = "base_footprint";
+        imu_msg.header.frame_id = "chassis";
 
         imu_msg.orientation = odom_msg.pose.pose.orientation;
 
         return imu_msg;
     }
 
+    geometry_msgs::msg::PoseStamped make_pose_msg(nav_msgs::msg::Odometry odom_msg) {
+        geometry_msgs::msg::PoseStamped pose_msg;
+        pose_msg.header.stamp = odom_msg.header.stamp;
+        pose_msg.header.frame_id = "track";
+
+        pose_msg.pose = odom_msg.pose.pose;
+
+        return pose_msg;
+    }
+
     void initUTM(double Lat, double Long, double altitude);
     void LLtoUTM(double Lat, double Long, int zoneNumber, double &UTMNorthing, double &UTMEasting);
 

src/hardware/sbg_translator/src/node_sbg_translator.cpp

@@ -14,10 +14,17 @@ SBGTranslate::SBGTranslate() : Node("sbg_translator_node") {
         "/imu/odometry", 1, std::bind(&SBGTranslate::ekf_odom_callback, this, _1));
 
     // Odometry
-    this->odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("/odometry/sbg_ekf", 1);
+    this->odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("/sbg_translated/odometry", 1);
+
+    // Pose (for visuals)
+    this->pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/sbg_translated/pose", 1);
+    this->raw_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/imu/pose", 1);
+
+    // Pose path (for visuals)
+    this->path_pub_ = this->create_publisher<nav_msgs::msg::Path>("/sbg_translated/path_odom", 1);
 
     // IMU for GPS init heading
-    this->imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("/imu/sbg_ekf_heading", 1);
+    this->imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("/sbg_translated/imu", 1);
 
     m_utm0_.easting = 0.0;
     m_utm0_.northing = 0.0;
@@ -26,6 +33,8 @@ SBGTranslate::SBGTranslate() : Node("sbg_translator_node") {
 
     state_ = {0.0, 0.0, 0.0};
 
+    last_pub_time_ = this->now();
+
     RCLCPP_INFO(this->get_logger(), "---SBG Odom Converter Node Initialised---");
 }
 
@@ -234,9 +243,13 @@ void SBGTranslate::ekf_odom_callback(const nav_msgs::msg::Odometry::SharedPtr ms
         return;
     }
 
+    // create a new pose msg with the same header
+    raw_pose_pub_->publish(make_pose_msg(*msg));
+
     // create a new odom msg with the same header
     nav_msgs::msg::Odometry odom_msg;
     odom_msg.header = msg->header;
+    odom_msg.child_frame_id = "chassis";
 
     // orient the position delta by the last yaw
     double yaw = quat_to_euler(msg->pose.pose.orientation);
@@ -283,6 +296,25 @@ void SBGTranslate::ekf_odom_callback(const nav_msgs::msg::Odometry::SharedPtr ms
 
     // publish imu message
     imu_pub_->publish(make_imu_msg(odom_msg));
+
+    // publish pose message
+    pose_pub_->publish(make_pose_msg(odom_msg));
+
+    // throttle path pub
+    // rclcpp time
+    if (this->now() - last_pub_time_ < rclcpp::Duration(std::chrono::seconds(1) / 10)) {
+        return;
+    }
+
+    // keep 50 points in the path
+    if (path_msg_.poses.size() > 50) {
+        path_msg_.poses.erase(path_msg_.poses.begin());
+    }
+    // update the path
+    path_msg_.header = odom_msg.header;
+    path_msg_.header.frame_id = "track";
+    path_msg_.poses.push_back(make_pose_msg(odom_msg));
+    path_pub_->publish(path_msg_);
 }
 
 int main(int argc, char *argv[]) {

src/navigation/nav_bringup/config/localisation_params.yaml

@@ -29,7 +29,7 @@ odom_filter_node:
     #                false, false, false,  # roll_dot, pitch_dot, yaw_dot
     #                false, false, false]  # x_ddot, y_ddot, z_ddot
 
-    odom1: odometry/sbg_ekf
+    odom1: sbg_translated/odometry
     odom1_config: [true, true, false,  # x, y, z
                   false, false, true,  # roll, pitch, yaw
                   true, true, false,  # x_dot, y_dot, z_dot

src/operations/mission_controller/mission_controller/node_trackdrive_handler_nav.py

@@ -89,6 +89,9 @@ def state_callback(self, msg: State):
                 # reset odom and pose from camera
                 self.reset_odom_client.call_async(Trigger.Request())
                 self.reset_pose_client.call_async(Trigger.Request())
+
+            print("Waiting for pose to reset")
+            time.sleep(2)
 
             command = ["stdbuf", "-o", "L", "ros2", "launch", "mission_controller", "trackdrive.launch.py"]
             self.get_logger().info(f"Command: {' '.join(command)}")