Patched serial implementation and improved RTCM3 stream

fixposition · Oct 28, 2024 · e907c7c · e907c7c
1 parent 56d0412
commit e907c7c
Show file tree

Hide file tree

Showing 12 changed files with 107 additions and 65 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/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
@@ -28,6 +28,7 @@ add_message_files(
   WheelSensor.msg
   GnssSats.msg
   NMEA.msg
+  RTCM.msg
   fpa/eoe.msg
   fpa/gnssant.msg
   fpa/gnsscorr.msg

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
@@ -45,7 +45,7 @@ class FixpositionDriverNode : public FixpositionDriver {
 
     void WsCallback(const fixposition_driver_ros1::SpeedConstPtr& msg);
 
-    void RtcmCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg);
+    void RtcmCallback(const fixposition_driver_ros1::RTCMConstPtr& msg);
 
    private:
     /**

diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp
@@ -29,6 +29,7 @@
 #include <fixposition_driver_ros1/Speed.h>
 #include <fixposition_driver_ros1/GnssSats.h>
 #include <fixposition_driver_ros1/NMEA.h>
+#include <fixposition_driver_ros1/RTCM.h>
 
 // Include extras
 #include <fixposition_driver_ros1/CovWarn.h>

diff --git a/fixposition_driver_ros1/msg/RTCM.msg b/fixposition_driver_ros1/msg/RTCM.msg
@@ -0,0 +1,19 @@
+####################################################################################################
+#
+#    Copyright (c) 2023  ___     ___
+#                       \\  \\  /  /
+#                        \\  \\/  /
+#                         /  /\\  \\
+#                        /__/  \\__\\  Fixposition AG
+#
+####################################################################################################
+#
+# Fixposition RTCM message.
+#
+#
+####################################################################################################
+
+# Format | Field         | Unit    | Description
+# -------|---------------|---------|------------------------------------|
+Header     header        # [-]     | Message header.
+uint8[]    message       # [-]     | RTCM message.
diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
@@ -68,9 +68,9 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para
     ws_sub_ = nh_.subscribe<fixposition_driver_ros1::Speed>(params_.customer_input.speed_topic, 10,
                                                             &FixpositionDriverNode::WsCallback, this,
                                                             ros::TransportHints().tcpNoDelay());
-    rtcm_sub_ = nh_.subscribe<std_msgs::UInt8MultiArray>(params_.customer_input.rtcm_topic, 10,
-                                                         &FixpositionDriverNode::RtcmCallback, this,
-                                                         ros::TransportHints().tcpNoDelay());
+    rtcm_sub_ = nh_.subscribe<fixposition_driver_ros1::RTCM>(params_.customer_input.rtcm_topic, 10,
+                                                             &FixpositionDriverNode::RtcmCallback, this,
+                                                             ros::TransportHints().tcpNoDelay());
 
     // Configure jump warning message
     if (params_.fp_output.cov_warning) {
@@ -415,9 +415,9 @@ void FixpositionDriverNode::WsCallback(const fixposition_driver_ros1::SpeedConst
     FixpositionDriver::WsCallback(measurements);
 }
 
-void FixpositionDriverNode::RtcmCallback(const std_msgs::UInt8MultiArray::ConstPtr& msg) {
-    const void* rtcm_msg = &(msg->data[0]);
-    size_t msg_size = msg->layout.dim[0].size;
+void FixpositionDriverNode::RtcmCallback(const fixposition_driver_ros1::RTCMConstPtr& msg) {
+    const void* rtcm_msg = &(msg->message[0]);
+    size_t msg_size = msg->message.size();
     FixpositionDriver::RtcmCallback(rtcm_msg, msg_size);
 }
 

diff --git a/fixposition_driver_ros2/CMakeLists.txt b/fixposition_driver_ros2/CMakeLists.txt
@@ -27,6 +27,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   msg/Speed.msg
   msg/Gnsssats.msg
   msg/NMEA.msg
+  msg/RTCM.msg
   msg/WheelSensor.msg
   msg/fpa/GNSSANT.msg
   msg/fpa/GNSSCORR.msg

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
@@ -45,7 +45,7 @@ class FixpositionDriverNode : public FixpositionDriver {
 
     void WsCallback(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg);
 
-    void RtcmCallback(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr msg);
+    void RtcmCallback(const fixposition_driver_ros2::msg::RTCM::ConstSharedPtr msg);
 
    private:
     /**
@@ -68,7 +68,7 @@ class FixpositionDriverNode : public FixpositionDriver {
 
     // ROS subscribers
     rclcpp::Subscription<fixposition_driver_ros2::msg::Speed>::SharedPtr ws_sub_;  //!< wheelspeed message subscriber
-    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr rtcm_sub_;  //!< RTCM3 message subscriber
+    rclcpp::Subscription<fixposition_driver_ros2::msg::RTCM>::SharedPtr rtcm_sub_; //!< RTCM3 message subscriber
 
     // ROS publishers
     // FP_A messages

diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp
@@ -30,6 +30,7 @@
 #include <fixposition_driver_ros2/msg/speed.hpp>
 #include <fixposition_driver_ros2/msg/gnsssats.hpp>
 #include <fixposition_driver_ros2/msg/nmea.hpp>
+#include <fixposition_driver_ros2/msg/rtcm.hpp>
 
 // Include extras
 #include <fixposition_driver_ros2/msg/covwarn.hpp>

diff --git a/fixposition_driver_ros2/msg/RTCM.msg b/fixposition_driver_ros2/msg/RTCM.msg
@@ -0,0 +1,16 @@
+####################################################################################################
+#
+#    Copyright (c) 2023
+#    Fixposition AG
+#
+####################################################################################################
+#
+# Fixposition RTCM Message.
+#
+#
+####################################################################################################
+
+# Format        | Field         | Unit    | Description
+# --------------|---------------|---------|------------------------------------|
+std_msgs/Header   header        # [-]     | Message header.
+uint8[]           message       # [-]     | RTCM message.
diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
@@ -73,7 +73,7 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr<rclcpp::Node> node,
     ws_sub_ = node_->create_subscription<fixposition_driver_ros2::msg::Speed>(
         params_.customer_input.speed_topic, 100,
         std::bind(&FixpositionDriverNode::WsCallback, this, std::placeholders::_1));
-    rtcm_sub_ = node_->create_subscription<std_msgs::msg::UInt8MultiArray>(
+    rtcm_sub_ = node_->create_subscription<fixposition_driver_ros2::msg::RTCM>(
         params_.customer_input.rtcm_topic, 10,
         std::bind(&FixpositionDriverNode::RtcmCallback, this, std::placeholders::_1));
 
@@ -423,9 +423,9 @@ void FixpositionDriverNode::WsCallback(const fixposition_driver_ros2::msg::Speed
     FixpositionDriver::WsCallback(measurements);
 }
 
-void FixpositionDriverNode::RtcmCallback(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr msg) {
-    const void* rtcm_msg = &(msg->data[0]);
-    size_t msg_size = msg->layout.dim[0].size;
+void FixpositionDriverNode::RtcmCallback(const fixposition_driver_ros2::msg::RTCM::ConstSharedPtr msg) {
+    const void* rtcm_msg = &(msg->message[0]);
+    size_t msg_size = msg->message.size();
     FixpositionDriver::RtcmCallback(rtcm_msg, msg_size);
 }