Added Nav2 support (fixposition#72)

* Added Nav2 support * Patched serial implementation and improved RTCM3 stream * Added missing dependencies * Updated packages --------- Co-authored-by: Facundo Garcia <[email protected]>
Greenzie · Oct 28, 2024 · 7afd4bc · 7afd4bc
1 parent 21b772e
commit 7afd4bc
Show file tree

Hide file tree

Showing 27 changed files with 507 additions and 115 deletions.
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp
@@ -149,6 +149,8 @@ class FixpositionDriver {
     int client_fd_ = -1;  //!< TCP or Serial file descriptor
     int connection_status_ = -1;
     struct termios options_save_;
+    uint8_t readBuf[8192];
+    int buf_size = 0;
 };
 }  // namespace fixposition
 #endif  //__FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER__
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_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp
@@ -257,66 +257,67 @@ bool FixpositionDriver::RunOnce() {
 }
 
 bool FixpositionDriver::ReadAndPublish() {
-    char readBuf[8192];
-
-    ssize_t rv;
-    if (params_.fp_output.type == INPUT_TYPE::TCP) {
-        rv = recv(client_fd_, (void*)&readBuf, sizeof(readBuf), MSG_DONTWAIT);
-    } else if (params_.fp_output.type == INPUT_TYPE::SERIAL) {
-        rv = read(client_fd_, (void*)&readBuf, sizeof(readBuf));
-    } else {
-        rv = 0;
-    }
-
-    if (rv == 0) {
-        std::cerr << "Connection closed.\n";
-        return false;
-    }
-
-    if (rv < 0 && errno == EAGAIN) {
-        /* no data for now, call back when the socket is readable */
+    int msg_size = 0;
+    // Nov B
+    msg_size = IsNovMessage(readBuf, buf_size);
+    if (msg_size > 0) {
+        NovConvertAndPublish(readBuf);
+        buf_size -= msg_size;
+        if (buf_size > 0) {
+            memmove(readBuf, &readBuf[msg_size], buf_size);
+        }
         return true;
-    }
-    if (rv < 0) {
-        std::cerr << "Connection error.\n";
-        return false;
-    }
-
-    ssize_t start_id = 0;
-    while (start_id < rv) {
-        int msg_size = 0;
-        // Nov B
-        msg_size = IsNovMessage((uint8_t*)&readBuf[start_id], rv - start_id);
+    } else if (msg_size == 0) {
+        // Nmea (incl. FP_A)
+        msg_size = IsNmeaMessage((char*)readBuf, buf_size);
         if (msg_size > 0) {
-            NovConvertAndPublish((uint8_t*)&readBuf[start_id]);
-            start_id += msg_size;
-            continue;
-        }
-        if (msg_size == 0) {
-            // do nothing
-        }
-        if (msg_size < 0) {
-            break;
+            NmeaConvertAndPublish({(const char*)readBuf, (const char*)readBuf + msg_size});
+            buf_size -= msg_size;
+            if (buf_size > 0) {
+                memmove(readBuf, &readBuf[msg_size], buf_size);
+            }
+            return true;
+        } else if (msg_size == 0) {
+            // If not NOV_B nor NMEA, remove 1 char
+            if (buf_size > 0) {
+                buf_size -= 1;
+                memmove(readBuf, &readBuf[1], buf_size);
+            }
+        } else {
+            // Wait for more data
         }
+    } else {
+        // wait for more data
+    }
 
-        // Nmea (incl. FP_A)
-        msg_size = IsNmeaMessage(&readBuf[start_id], rv - start_id);
-        if (msg_size > 0) {
-            // NovConvertAndPublish(start, msg_size);
-            std::string msg(&readBuf[start_id], msg_size);
-            NmeaConvertAndPublish(msg);
-            start_id += msg_size;
-            continue;
+    // Read more data from the TCP/Serial port
+    int rem_size = sizeof(readBuf) - buf_size;
+    if (rem_size > 0) {
+        ssize_t rv;
+        if (params_.fp_output.type == INPUT_TYPE::TCP) {
+            rv = recv(client_fd_, (void*)&readBuf[buf_size], sizeof(readBuf) - buf_size, MSG_DONTWAIT);
+        } else if (params_.fp_output.type == INPUT_TYPE::SERIAL) {
+            rv = read(client_fd_, (void*)&readBuf[buf_size], sizeof(readBuf) - buf_size);
+        } else {
+            rv = 0;
         }
-        if (msg_size == 0) {
-            // do nothing
+
+        if (rv == 0) {
+            std::cerr << "Connection closed.\n";
+            return false;
         }
-        if (msg_size < 0) {
-            break;
+
+        if (rv < 0 && errno == EAGAIN) {
+            /* no data for now, call back when the socket is readable */
+            return true;
+        } 
+
+        if (rv < 0) {
+            std::cerr << "Connection error.\n";
+            return false;
         }
 
-        // No Match, increment by 1
-        ++start_id;
+        buf_size += rv;
     }
 
     return true;

diff --git a/fixposition_driver_ros1/CMakeLists.txt b/fixposition_driver_ros1/CMakeLists.txt
@@ -21,6 +21,8 @@ find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   message_generation
   eigen_conversions
+  tf2_ros
+  tf2_geometry_msgs
 )
 add_message_files(
   FILES
@@ -61,8 +63,8 @@ generate_messages(
 catkin_package(
   INCLUDE_DIRS include
   CATKIN_DEPENDS
-  tf
-  roscpp geometry_msgs
+  tf tf2_ros
+  roscpp geometry_msgs tf2_geometry_msgs
   nav_msgs std_msgs message_runtime
 )
 

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
@@ -43,9 +43,9 @@ class FixpositionDriverNode : public FixpositionDriver {
 
     void RegisterObservers();
 
-    void WsCallback(const fixposition_driver_ros1::SpeedConstPtr& msg);
+    void WsCallbackRos(const fixposition_driver_ros1::SpeedConstPtr& msg);
 
-    void RtcmCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg);
+    void RtcmCallbackRos(const rtcm_msgs::MessageConstPtr& msg);
 
    private:
     /**
@@ -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,12 +22,17 @@
 #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>
 #include <fixposition_driver_ros1/GnssSats.h>
 #include <fixposition_driver_ros1/NMEA.h>
 
+// Include RTCM
+#include <rtcm_msgs/Message.h>
+
 // Include extras
 #include <fixposition_driver_ros1/CovWarn.h>
 

diff --git a/fixposition_driver_ros1/launch/serial.yaml b/fixposition_driver_ros1/launch/serial.yaml
@@ -10,7 +10,8 @@ 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"
-    rtcm_topic: "/fixposition/rtcm"
+    rtcm_topic: "/rtcm"
diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml
@@ -10,7 +10,8 @@ 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"
-    rtcm_topic: "/fixposition/rtcm"
+    rtcm_topic: "/rtcm"
diff --git a/fixposition_driver_ros1/package.xml b/fixposition_driver_ros1/package.xml
@@ -18,4 +18,7 @@
     <depend>message_generation</depend>
     <depend>message_runtime</depend>
     <depend>fixposition_driver_lib</depend>
+    <depend>rtcm_msgs</depend>
+    <depend>tf2_ros</depend>
+    <depend>tf2_geometry_msgs</depend>
 </package>
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