Added Nav2 support

fixposition · Oct 14, 2024 · 56d0412 · 56d0412
1 parent 21b772e
commit 56d0412
Show file tree

Hide file tree

Showing 17 changed files with 297 additions and 32 deletions.
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp
@@ -30,6 +30,7 @@ struct FpOutputParams {
     std::vector<std::string> formats;  //!< data formats to convert, supports "FP" for now
     std::string qos_type;              //!< ROS QoS type, supports "sensor_<short/long>" and "default_<short/long>"
     bool cov_warning;                  //!< enable/disable covariance warning
+    bool nav2_mode;                    //!< enable/disable nav2 mode
 
     std::string ip;    //!< IP address for TCP connection
     std::string port;  //!< Port for TCP connection

diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp
@@ -94,6 +94,23 @@ void TwistWithCovDataToMsg(const TwistWithCovData& data, geometry_msgs::TwistWit
  */
 void OdometryDataToTf(const FP_ODOMETRY& data, tf2_ros::TransformBroadcaster& pub);
 
+/**
+ * @brief
+ *
+ * @param[in] data
+ * @param[out] tf
+ */
+void OdomToTf(const OdometryData& data, geometry_msgs::TransformStamped& tf);
+
+/**
+ * @brief
+ *
+ * @param[in] tf_map
+ * @param[out] static_br_
+ * @param[out] br_
+ */
+void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::TransformStamped>>& tf_map, tf2_ros::StaticTransformBroadcaster& static_br_, tf2_ros::TransformBroadcaster& br_);
+
 /**
  * @brief
  *

diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp
@@ -126,6 +126,14 @@ class FixpositionDriverNode : public FixpositionDriver {
     // Previous state
     Eigen::Vector3d prev_pos;
     Eigen::MatrixXd prev_cov;
+
+    // Nav2 TF map
+    std::map<std::string, std::shared_ptr<geometry_msgs::TransformStamped>> tf_map = {
+        {"ECEFENU0", nullptr},
+        {"POIPOISH", nullptr},
+        {"ECEFPOISH", nullptr},
+        {"ENU0POI", nullptr}
+    };
 };
 
 }  // namespace fixposition

diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp
@@ -22,6 +22,8 @@
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 // Include generic
 #include <fixposition_driver_ros1/Speed.h>

diff --git a/fixposition_driver_ros1/launch/serial.yaml b/fixposition_driver_ros1/launch/serial.yaml
@@ -10,6 +10,7 @@ fp_output:
     rate: 200
     reconnect_delay: 5.0 # wait time in [s] until retry connection
     cov_warning: false
+    nav2_mode: false
 
 customer_input:
     speed_topic: "/fixposition/speed"

diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml
@@ -10,6 +10,7 @@ fp_output:
     rate: 200
     reconnect_delay: 5.0 # wait time in [s] until retry connection
     cov_warning: false
+    nav2_mode: false
 
 customer_input:
     speed_topic: "/fixposition/speed"

diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp
@@ -596,27 +596,87 @@ void OdometryDataToTf(const FP_ODOMETRY& data, tf2_ros::TransformBroadcaster& pu
             return;
         }
 
-        // Create message
-        geometry_msgs::TransformStamped msg;
-
         // Populate message
-        msg.header.frame_id = data.odom.frame_id;
-        msg.child_frame_id = data.odom.child_frame_id;
-
-        if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp));
-        }
-
-        tf::quaternionEigenToMsg(data.odom.pose.orientation, msg.transform.rotation);
-        tf::vectorEigenToMsg(data.odom.pose.position, msg.transform.translation);
+        geometry_msgs::TransformStamped msg;
+        OdomToTf(data.odom, msg);
 
         // Publish message
         pub.sendTransform(msg);
     }
 }
 
+void OdomToTf(const OdometryData& data, geometry_msgs::TransformStamped& tf) {
+    // Populate message
+    tf.header.frame_id = data.frame_id;
+    tf.child_frame_id = data.child_frame_id;
+
+    if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
+        tf.header.stamp = ros::Time::now();
+    } else {
+        tf.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
+    }
+
+    tf::quaternionEigenToMsg(data.pose.orientation, tf.transform.rotation);
+    tf::vectorEigenToMsg(data.pose.position, tf.transform.translation);
+}
+
+void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::TransformStamped>>& tf_map, tf2_ros::StaticTransformBroadcaster& static_br_, tf2_ros::TransformBroadcaster& br_) {
+    if (tf_map.at("ECEFENU0") && tf_map.at("POIPOISH") && tf_map.at("ECEFPOISH") && tf_map.at("ENU0POI")) {
+        // Publish FP_ECEF -> map
+        tf_map.at("ECEFENU0")->child_frame_id = "map";
+        static_br_.sendTransform(*tf_map.at("ECEFENU0"));
+
+        // Compute FP_ENU0 -> FP_POISH
+        // Extract translation and rotation from ECEFENU0
+        geometry_msgs::Vector3 trans_ecef_enu0 = tf_map.at("ECEFENU0")->transform.translation;
+        geometry_msgs::Quaternion rot_ecef_enu0 = tf_map.at("ECEFENU0")->transform.rotation;
+        Eigen::Vector3d t_ecef_enu0_;
+        t_ecef_enu0_ << trans_ecef_enu0.x, trans_ecef_enu0.y, trans_ecef_enu0.z;
+        Eigen::Quaterniond q_ecef_enu0_(rot_ecef_enu0.w, rot_ecef_enu0.x, rot_ecef_enu0.y, rot_ecef_enu0.z);
+
+        // Extract translation and rotation from ECEFPOISH
+        geometry_msgs::Vector3 trans_ecef_poish = tf_map.at("ECEFPOISH")->transform.translation;
+        geometry_msgs::Quaternion rot_ecef_poish = tf_map.at("ECEFPOISH")->transform.rotation;
+        Eigen::Vector3d t_ecef_poish;
+        t_ecef_poish << trans_ecef_poish.x, trans_ecef_poish.y, trans_ecef_poish.z;
+        Eigen::Quaterniond q_ecef_poish(rot_ecef_poish.w, rot_ecef_poish.x, rot_ecef_poish.y, rot_ecef_poish.z);
+
+        // Compute the ENU transformation
+        const Eigen::Vector3d t_enu0_poish = fixposition::TfEnuEcef(t_ecef_poish, fixposition::TfWgs84LlhEcef(t_ecef_enu0_));
+        const Eigen::Quaterniond q_enu0_poish = q_ecef_enu0_.inverse() * q_ecef_poish;
+
+        // Create tf2::Transform tf_ENU0POISH
+        tf2::Transform tf_ENU0POISH;
+        tf_ENU0POISH.setOrigin(tf2::Vector3(t_enu0_poish.x(), t_enu0_poish.y(), t_enu0_poish.z()));
+        tf2::Quaternion tf_q_enu0_poish(q_enu0_poish.x(), q_enu0_poish.y(), q_enu0_poish.z(), q_enu0_poish.w());
+        tf_ENU0POISH.setRotation(tf_q_enu0_poish);
+
+        // Publish map -> odom
+        // Multiply the transforms
+        tf2::Transform tf_ENU0POI;
+        tf2::fromMsg(tf_map.at("ENU0POI")->transform, tf_ENU0POI);
+        tf2::Transform tf_combined = tf_ENU0POI * tf_ENU0POISH.inverse();
+
+        // Create a new TransformStamped message
+        geometry_msgs::TransformStamped tf_map_odom;
+        tf_map_odom.header.stamp = ros::Time::now();
+        tf_map_odom.header.frame_id = "map";
+        tf_map_odom.child_frame_id = "odom";
+        tf_map_odom.transform = tf2::toMsg(tf_combined);
+        br_.sendTransform(tf_map_odom);
+
+        // Publish odom -> base_link
+        geometry_msgs::TransformStamped tf_odom_base;
+        tf_odom_base.header.stamp = ros::Time::now();
+        tf_odom_base.header.frame_id = "odom";
+        tf_odom_base.child_frame_id = "base_link";
+        tf_odom_base.transform = tf2::toMsg(tf_ENU0POISH);
+
+        // Send the transform
+        br_.sendTransform(tf_odom_base);
+    }
+}
+
 void OdomToNavSatFix(const FP_ODOMETRY& data, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
         // Create message

diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
@@ -121,7 +121,7 @@ void FixpositionDriverNode::RegisterObservers() {
                         if (!prev_pos.isZero() && !prev_cov.isZero()) {
                             Eigen::Vector3d pos_diff = (prev_pos - data.odom.pose.position).cwiseAbs();
 
-                            if ((pos_diff[0] > 0) || (pos_diff[1] > prev_cov(1,1)) || (pos_diff[2] > prev_cov(2,2))) {
+                            if ((pos_diff[0] > prev_cov(0,0)) || (pos_diff[1] > prev_cov(1,1)) || (pos_diff[2] > prev_cov(2,2))) {
                                 JumpWarningMsg(data.odom.stamp, pos_diff, prev_cov, extras_jump_pub_);
                             }
                         }
@@ -135,12 +135,28 @@ void FixpositionDriverNode::RegisterObservers() {
                     FpToRosMsg(data, fpa_odomenu_pub_);
                     FpToRosMsg(data.odom, odometry_enu_pub_);
                     OdomToYprMsg(data.odom, eul_pub_);
+
+                    // Append TF if Nav2 mode is selected
+                    if (params_.fp_output.nav2_mode) {
+                        // Get FP_ENU0 -> FP_POI
+                        geometry_msgs::TransformStamped tf;
+                        OdomToTf(data.odom, tf);
+                        tf_map["ENU0POI"] = std::make_shared<geometry_msgs::TransformStamped>(tf);
+                    }
                 });
         } else if (format == "ODOMSH") {
             dynamic_cast<NmeaConverter<FP_ODOMSH>*>(a_converters_["ODOMSH"].get())
                 ->AddObserver([this](const FP_ODOMSH& data) {
                     FpToRosMsg(data, fpa_odomsh_pub_);
                     FpToRosMsg(data.odom, odometry_smooth_pub_);
+
+                    // Append TF if Nav2 mode is selected
+                    if (params_.fp_output.nav2_mode) {
+                        // Get FP_ECEF -> FP_POISH
+                        geometry_msgs::TransformStamped tf;
+                        OdomToTf(data.odom, tf);
+                        tf_map["ECEFPOISH"] = std::make_shared<geometry_msgs::TransformStamped>(tf);
+                    }
                 });
         } else if (format == "ODOMSTATUS") {
             dynamic_cast<NmeaConverter<FP_ODOMSTATUS>*>(a_converters_["ODOMSTATUS"].get())
@@ -150,7 +166,14 @@ void FixpositionDriverNode::RegisterObservers() {
                 ->AddObserver([this](const FP_IMUBIAS& data) { FpToRosMsg(data, fpa_imubias_pub_); });
         } else if (format == "EOE") {
             dynamic_cast<NmeaConverter<FP_EOE>*>(a_converters_["EOE"].get())
-                ->AddObserver([this](const FP_EOE& data) { FpToRosMsg(data, fpa_eoe_pub_); });
+                ->AddObserver([this](const FP_EOE& data) {
+                    FpToRosMsg(data, fpa_eoe_pub_);
+
+                    // Generate Nav2 TF tree
+                    if (data.epoch == "FUSION" && params_.fp_output.nav2_mode) {
+                        PublishNav2Tf(tf_map, static_br_, br_);
+                    }
+                });
         } else if (format == "LLH") {
             dynamic_cast<NmeaConverter<FP_LLH>*>(a_converters_["LLH"].get())
                 ->AddObserver([this](const FP_LLH& data) { FpToRosMsg(data, fpa_llh_pub_); });
@@ -189,6 +212,20 @@ void FixpositionDriverNode::RegisterObservers() {
 
                     } else if (tf.child_frame_id == "FP_POISH" && tf.header.frame_id == "FP_POI") {
                         br_.sendTransform(tf);
+
+                        // Append TF if Nav2 mode is selected
+                        if (params_.fp_output.nav2_mode) {
+                            // Get FP_POI -> FP_POISH
+                            tf_map["POIPOISH"] = std::make_shared<geometry_msgs::TransformStamped>(tf);
+                        }
+                    } else if (tf.child_frame_id == "FP_ENU0" && tf.header.frame_id == "FP_ECEF") {
+                        static_br_.sendTransform(tf);
+
+                        // Append TF if Nav2 mode is selected
+                        if (params_.fp_output.nav2_mode) {
+                            // Get FP_ECEF -> FP_ENU0
+                            tf_map["ECEFENU0"] = std::make_shared<geometry_msgs::TransformStamped>(tf);
+                        }
                     } else {
                         static_br_.sendTransform(tf);
                     }

diff --git a/fixposition_driver_ros1/src/params.cpp b/fixposition_driver_ros1/src/params.cpp
@@ -26,6 +26,7 @@ bool LoadParamsFromRos1(const std::string& ns, FpOutputParams& params) {
     const std::string PORT = ns + "/port";
     const std::string BAUDRATE = ns + "/baudrate";
     const std::string COV_WARNING = ns + "/cov_warning";
+    const std::string NAV2_MODE = ns + "/nav2_mode";
 
     // read parameters
     if (!ros::param::get(RATE, params.rate)) {
@@ -40,6 +41,10 @@ bool LoadParamsFromRos1(const std::string& ns, FpOutputParams& params) {
         params.cov_warning = false;
         ROS_WARN("Using Default Covariance Warning option : %i", params.cov_warning);
     }
+    if (!ros::param::get(NAV2_MODE, params.nav2_mode)) {
+        params.nav2_mode = false;
+        ROS_WARN("Using Default Nav2 mode option : %i", params.nav2_mode);
+    }
 
     std::string type_str;
     if (!ros::param::get(TYPE, type_str)) {

diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp
@@ -115,6 +115,23 @@ void TwistWithCovDataToMsg(const TwistWithCovData& data, geometry_msgs::msg::Twi
  */
 void OdometryDataToTf(const FP_ODOMETRY& data, std::shared_ptr<tf2_ros::TransformBroadcaster> pub);
 
+/**
+ * @brief
+ *
+ * @param[in] data
+ * @param[out] tf
+ */
+void OdomToTf(const OdometryData& data, geometry_msgs::msg::TransformStamped& tf);
+
+/**
+ * @brief
+ *
+ * @param[in] tf_map
+ * @param[out] static_br_
+ * @param[out] br_
+ */
+void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::TransformStamped>>& tf_map, std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_br_, std::shared_ptr<tf2_ros::TransformBroadcaster> br_);
+
 /**
  * @brief
  *

diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp
@@ -126,6 +126,14 @@ class FixpositionDriverNode : public FixpositionDriver {
     // Previous state
     Eigen::Vector3d prev_pos;
     Eigen::MatrixXd prev_cov;
+
+    // Nav2 TF map
+    std::map<std::string, std::shared_ptr<geometry_msgs::msg::TransformStamped>> tf_map = {
+        {"ECEFENU0", nullptr},
+        {"POIPOISH", nullptr},
+        {"ECEFPOISH", nullptr},
+        {"ENU0POI", nullptr}
+    };
 };
 
 }  // namespace fixposition

diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp
@@ -23,6 +23,8 @@
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 // Include generic
 #include <fixposition_driver_ros2/msg/speed.hpp>

diff --git a/fixposition_driver_ros2/launch/serial.yaml b/fixposition_driver_ros2/launch/serial.yaml
@@ -12,6 +12,7 @@ fixposition_driver_ros2:
       rate: 200
       reconnect_delay: 5.0 # wait time in [s] until retry connection
       cov_warning: false
+      nav2_mode: false
 
     customer_input:
       speed_topic: "/fixposition/speed"

diff --git a/fixposition_driver_ros2/launch/tcp.yaml b/fixposition_driver_ros2/launch/tcp.yaml
@@ -13,6 +13,7 @@ fixposition_driver_ros2:
       reconnect_delay: 5.0 # wait time in [s] until retry connection
       qos_type: "sensor_short" # Supported types: "sensor_short", "sensor_long", "default_short", "default_long"
       cov_warning: false
+      nav2_mode: false
 
     customer_input:
       speed_topic: "/fixposition/speed"