Added Nav2 support (#72)

* Added Nav2 support

* Patched serial implementation and improved RTCM3 stream

* Added missing dependencies

* Updated packages

---------

Co-authored-by: Facundo Garcia <facundo.garcia@fixposition.com>

Author: fgarciacardenas

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__

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

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;

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
 )
 

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
  *

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

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>
 

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"

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"

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>