From e907c7ce43e500ac99917e3f3434f8a4e07137aa Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Mon, 28 Oct 2024 13:53:48 +0100 Subject: [PATCH] Patched serial implementation and improved RTCM3 stream --- .../fixposition_driver.hpp | 2 + .../src/fixposition_driver.cpp | 105 +++++++++--------- fixposition_driver_ros1/CMakeLists.txt | 1 + .../fixposition_driver_node.hpp | 2 +- .../fixposition_driver_ros1/ros_msgs.hpp | 1 + fixposition_driver_ros1/msg/RTCM.msg | 19 ++++ .../src/fixposition_driver_node.cpp | 12 +- fixposition_driver_ros2/CMakeLists.txt | 1 + .../fixposition_driver_node.hpp | 4 +- .../fixposition_driver_ros2/ros2_msgs.hpp | 1 + fixposition_driver_ros2/msg/RTCM.msg | 16 +++ .../src/fixposition_driver_node.cpp | 8 +- 12 files changed, 107 insertions(+), 65 deletions(-) create mode 100644 fixposition_driver_ros1/msg/RTCM.msg create mode 100644 fixposition_driver_ros2/msg/RTCM.msg diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp index 88333d5..9d1bc80 100644 --- 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 index 92da33d..39dd7a4 100644 --- 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 index 339805b..be225f3 100644 --- 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 index ee12245..1202295 100644 --- 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 index 57c4a4a..8559665 100644 --- 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 #include #include +#include // Include extras #include diff --git a/fixposition_driver_ros1/msg/RTCM.msg b/fixposition_driver_ros1/msg/RTCM.msg new file mode 100644 index 0000000..1ed9206 --- /dev/null +++ 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 index 706f975..3692ddf 100644 --- 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(params_.customer_input.speed_topic, 10, &FixpositionDriverNode::WsCallback, this, ros::TransportHints().tcpNoDelay()); - rtcm_sub_ = nh_.subscribe(params_.customer_input.rtcm_topic, 10, - &FixpositionDriverNode::RtcmCallback, this, - ros::TransportHints().tcpNoDelay()); + rtcm_sub_ = nh_.subscribe(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 index b8bbc8e..d276498 100644 --- 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 index d80a4c5..41e2042 100644 --- 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::SharedPtr ws_sub_; //!< wheelspeed message subscriber - rclcpp::Subscription::SharedPtr rtcm_sub_; //!< RTCM3 message subscriber + rclcpp::Subscription::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 index b74d829..904a13e 100644 --- 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 #include #include +#include // Include extras #include diff --git a/fixposition_driver_ros2/msg/RTCM.msg b/fixposition_driver_ros2/msg/RTCM.msg new file mode 100644 index 0000000..b63c536 --- /dev/null +++ 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 index 120b510..ac878ce 100644 --- 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 node, ws_sub_ = node_->create_subscription( params_.customer_input.speed_topic, 100, std::bind(&FixpositionDriverNode::WsCallback, this, std::placeholders::_1)); - rtcm_sub_ = node_->create_subscription( + rtcm_sub_ = node_->create_subscription( 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); }