Solved issues with ROS1 and ROS2 rtcm3 compatibility

fixposition · Oct 28, 2024 · b603a75 · b603a75
1 parent 2b7301e
commit b603a75
Show file tree

Hide file tree

Showing 15 changed files with 37 additions and 44 deletions.
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 fixposition_driver_ros1::RTCMConstPtr& msg);
+    void RtcmCallbackRos(const rtcm_msgs::MessageConstPtr& 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,7 +29,9 @@
 #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 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
@@ -14,4 +14,4 @@ fp_output:
 
 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
@@ -14,4 +14,4 @@ fp_output:
 
 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,6 +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/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
@@ -66,11 +66,11 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para
 
 {
     ws_sub_ = nh_.subscribe<fixposition_driver_ros1::Speed>(params_.customer_input.speed_topic, 10,
-                                                            &FixpositionDriverNode::WsCallback, this,
+                                                            &FixpositionDriverNode::WsCallbackRos, this,
                                                             ros::TransportHints().tcpNoDelay());
-    rtcm_sub_ = nh_.subscribe<fixposition_driver_ros1::RTCM>(params_.customer_input.rtcm_topic, 10,
-                                                             &FixpositionDriverNode::RtcmCallback, this,
-                                                             ros::TransportHints().tcpNoDelay());
+    rtcm_sub_ = nh_.subscribe<rtcm_msgs::Message>(params_.customer_input.rtcm_topic, 10,
+                                                  &FixpositionDriverNode::RtcmCallbackRos, this,
+                                                  ros::TransportHints().tcpNoDelay());
 
     // Configure jump warning message
     if (params_.fp_output.cov_warning) {
@@ -405,20 +405,20 @@ void FixpositionDriverNode::PublishNmea() {
     }
 }
 
-void FixpositionDriverNode::WsCallback(const fixposition_driver_ros1::SpeedConstPtr& msg) {
+void FixpositionDriverNode::WsCallbackRos(const fixposition_driver_ros1::SpeedConstPtr& msg) {
     std::unordered_map<std::string, std::vector<std::pair<bool, int>>> measurements;
     for (const auto& sensor : msg->sensors) {
         measurements[sensor.location].push_back({sensor.vx_valid, sensor.vx});
         measurements[sensor.location].push_back({sensor.vy_valid, sensor.vy});
         measurements[sensor.location].push_back({sensor.vz_valid, sensor.vz});
     }
-    FixpositionDriver::WsCallback(measurements);
+    WsCallback(measurements);
 }
 
-void FixpositionDriverNode::RtcmCallback(const fixposition_driver_ros1::RTCMConstPtr& msg) {
+void FixpositionDriverNode::RtcmCallbackRos(const rtcm_msgs::MessageConstPtr& msg) {
     const void* rtcm_msg = &(msg->message[0]);
     size_t msg_size = msg->message.size();
-    FixpositionDriver::RtcmCallback(rtcm_msg, msg_size);
+    RtcmCallback(rtcm_msg, msg_size);
 }
 
 void FixpositionDriverNode::BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header,

diff --git a/fixposition_driver_ros2/CMakeLists.txt b/fixposition_driver_ros2/CMakeLists.txt
@@ -23,12 +23,12 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(fixposition_driver_lib REQUIRED)
+find_package(rtcm_msgs REQUIRED)
 
 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
@@ -63,6 +63,7 @@ include_directories(
   include
   ${sensor_msgs_INCLUDE_DIR}
   ${fixposition_driver_lib_INCLUDE_DIR}
+  ${rtcm_msgs_INCLUDE_DIR}
   ${EIGEN3_INCLUDE_DIR}
   ${Boost_INCLUDE_DIR}
 )
@@ -86,6 +87,7 @@ endif()
 target_link_libraries(
   ${PROJECT_NAME}_exec
   ${fixposition_driver_lib_LIBRARIES}
+  ${rtcm_msgs_LIBRARIES}
   ${Boost_LIBRARIES}
   ${EIGEN3_LIBRARIES}
   ${cpp_typesupport_target}
@@ -107,7 +109,7 @@ install(DIRECTORY
   "launch"
   DESTINATION share/${PROJECT_NAME}/
 )
-ament_target_dependencies(${PROJECT_NAME}_exec rclcpp std_msgs nav_msgs geometry_msgs sensor_msgs tf2_ros tf2_eigen fixposition_driver_lib tf2_geometry_msgs)
+ament_target_dependencies(${PROJECT_NAME}_exec rclcpp std_msgs nav_msgs geometry_msgs sensor_msgs tf2_ros tf2_eigen fixposition_driver_lib rtcm_msgs tf2_geometry_msgs)
 
 # define ament package for this project
 ament_package()
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
@@ -43,9 +43,9 @@ class FixpositionDriverNode : public FixpositionDriver {
 
     void RegisterObservers();
 
-    void WsCallback(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg);
+    void WsCallbackRos(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg);
 
-    void RtcmCallback(const fixposition_driver_ros2::msg::RTCM::ConstSharedPtr msg);
+    void RtcmCallbackRos(const rtcm_msgs::msg::Message::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<fixposition_driver_ros2::msg::RTCM>::SharedPtr rtcm_sub_; //!< RTCM3 message subscriber
+    rclcpp::Subscription<rtcm_msgs::msg::Message>::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,7 +30,9 @@
 #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 RTCM
+#include <rtcm_msgs/msg/message.hpp>
 
 // Include extras
 #include <fixposition_driver_ros2/msg/covwarn.hpp>

diff --git a/fixposition_driver_ros2/launch/serial.yaml b/fixposition_driver_ros2/launch/serial.yaml
@@ -16,4 +16,4 @@ fixposition_driver_ros2:
 
     customer_input:
       speed_topic: "/fixposition/speed"
-      rtcm_topic: "/fixposition/rtcm"
+      rtcm_topic: "/rtcm"
diff --git a/fixposition_driver_ros2/launch/tcp.yaml b/fixposition_driver_ros2/launch/tcp.yaml
@@ -17,4 +17,4 @@ fixposition_driver_ros2:
 
     customer_input:
       speed_topic: "/fixposition/speed"
-      rtcm_topic: "/fixposition/rtcm"
+      rtcm_topic: "/rtcm"
diff --git a/fixposition_driver_ros2/msg/RTCM.msg b/fixposition_driver_ros2/msg/RTCM.msg
diff --git a/fixposition_driver_ros2/package.xml b/fixposition_driver_ros2/package.xml
@@ -24,6 +24,7 @@
     <depend>tf2_eigen</depend>
     <depend>tf2_ros</depend>
     <depend>tf2_geometry_msgs</depend>
+    <depend>rtcm_msgs</depend>
 
     <exec_depend>rclcpp</exec_depend>
     <exec_depend>rosidl_default_runtime</exec_depend>

diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
@@ -72,10 +72,10 @@ 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<fixposition_driver_ros2::msg::RTCM>(
+        std::bind(&FixpositionDriverNode::WsCallbackRos, this, std::placeholders::_1));
+    rtcm_sub_ = node_->create_subscription<rtcm_msgs::msg::Message>(
         params_.customer_input.rtcm_topic, 10,
-        std::bind(&FixpositionDriverNode::RtcmCallback, this, std::placeholders::_1));
+        std::bind(&FixpositionDriverNode::RtcmCallbackRos, this, std::placeholders::_1));
 
     // Configure jump warning message
     if (params_.fp_output.cov_warning) {
@@ -413,20 +413,20 @@ void FixpositionDriverNode::PublishNmea() {
     }
 }
 
-void FixpositionDriverNode::WsCallback(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg) {
+void FixpositionDriverNode::WsCallbackRos(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg) {
     std::unordered_map<std::string, std::vector<std::pair<bool, int>>> measurements;
     for (const auto& sensor : msg->sensors) {
         measurements[sensor.location].push_back({sensor.vx_valid, sensor.vx});
         measurements[sensor.location].push_back({sensor.vy_valid, sensor.vy});
         measurements[sensor.location].push_back({sensor.vz_valid, sensor.vz});
     }
-    FixpositionDriver::WsCallback(measurements);
+    WsCallback(measurements);
 }
 
-void FixpositionDriverNode::RtcmCallback(const fixposition_driver_ros2::msg::RTCM::ConstSharedPtr msg) {
+void FixpositionDriverNode::RtcmCallbackRos(const rtcm_msgs::msg::Message::ConstSharedPtr msg) {
     const void* rtcm_msg = &(msg->message[0]);
     size_t msg_size = msg->message.size();
-    FixpositionDriver::RtcmCallback(rtcm_msg, msg_size);
+    RtcmCallback(rtcm_msg, msg_size);
 }
 
 void FixpositionDriverNode::BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header,

diff --git a/rtcm_msgs b/rtcm_msgs