ROS2 patch fix

fixposition · Jan 15, 2024 · 6dd89eb · 6dd89eb
1 parent 4ece541
commit 6dd89eb
Show file tree

Hide file tree

Showing 6 changed files with 14 additions and 15 deletions.
diff --git a/fixposition_driver_lib/src/gprmc.cpp b/fixposition_driver_lib/src/gprmc.cpp
@@ -20,7 +20,7 @@
 
 namespace fixposition {
 
-// unit transformation constant
+// unit conversion constant
 static constexpr double knots_to_ms = 1852.0 / 3600.0; //!< convert knots to m/s
 
 /// msg field indices

diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml
@@ -2,7 +2,7 @@ fp_output:
     formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"]
     type: tcp
     port: "21000"
-    ip: "127.0.0.1" # change to VRTK2's IP address in the network
+    ip: "10.0.2.1" # change to VRTK2's IP address in the network
     rate: 200
     reconnect_delay: 5.0 # wait time in [s] until retry connection
 

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
@@ -77,6 +77,8 @@ class FixpositionDriverNode : public FixpositionDriver {
      * @param[in] payload
      */
     void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload);
+
+    void PublishNmea(NmeaMessage data);
 
     std::shared_ptr<rclcpp::Node> node_;
     rclcpp::Subscription<fixposition_driver_ros2::msg::Speed>::SharedPtr ws_sub_;  //!< wheelspeed message subscriber

diff --git a/fixposition_driver_ros2/launch/tcp.yaml b/fixposition_driver_ros2/launch/tcp.yaml
@@ -1,7 +1,7 @@
 fixposition_driver_ros2:
   ros__parameters:
     fp_output:
-      formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "TF"]
+      formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"]
       type: "tcp"
       port: "21000"
       ip: "10.0.2.1" # change to VRTK2's IP address in the network

diff --git a/fixposition_driver_ros2/msg/NMEA.msg b/fixposition_driver_ros2/msg/NMEA.msg
@@ -1,10 +1,7 @@
 ####################################################################################################
 #
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
+#    Copyright (c) 2023
+#    Fixposition AG
 #
 ####################################################################################################
 #
@@ -25,7 +22,7 @@
 #        receiver, usually the location of the antenna.  This is a
 #        Euclidean frame relative to the vehicle, not a reference
 #        ellipsoid.
-Header header
+std_msgs/Header header
 
 # Latitude [degrees]. Positive is north of equator; negative is south.
 float64 latitude

diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
@@ -190,23 +190,23 @@ void FixpositionDriverNode::RegisterObservers() {
         } else if (format == "GPGGA") {
             dynamic_cast<GpggaConverter*>(a_converters_["GPGGA"].get())->AddObserver([this](const GpggaData& data) {
                 // GPGGA Observer Lambda
-                if (nmea_pub_.getNumSubscribers() > 0) {
+                if (nmea_pub_->get_subscription_count() > 0) {
                     nmea_message_.gpgga = data;
                     PublishNmea(nmea_message_);
                 }
             });
         } else if (format == "GPZDA") {
             dynamic_cast<GpzdaConverter*>(a_converters_["GPZDA"].get())->AddObserver([this](const GpzdaData& data) {
                 // GPZDA Observer Lambda
-                if (nmea_pub_.getNumSubscribers() > 0) {
+                if (nmea_pub_->get_subscription_count() > 0) {
                     nmea_message_.gpzda = data;
                     PublishNmea(nmea_message_);
                 }
             });
         } else if (format == "GPRMC") {
             dynamic_cast<GprmcConverter*>(a_converters_["GPRMC"].get())->AddObserver([this](const GprmcData& data) {
                 // GPRMC Observer Lambda
-                if (nmea_pub_.getNumSubscribers() > 0) {
+                if (nmea_pub_->get_subscription_count() > 0) {
                     nmea_message_.gprmc = data;
                     PublishNmea(nmea_message_);
                 }
@@ -219,10 +219,10 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) {
     // If epoch message is complete, generate NMEA output
     if (data.checkEpoch()) {
         // Generate new message
-        fixposition_driver_ros1::NMEA msg;
+        fixposition_driver_ros2::msg::NMEA msg;
 
         // ROS Header
-        msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp));
+        msg.header.stamp = GpsTimeToMsgTime(data.gpzda.stamp);
         msg.header.frame_id = "LLH";
 
         // Latitude [degrees]. Positive is north of equator; negative is south
@@ -251,7 +251,7 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) {
         msg.mode = data.gprmc.mode;
 
         // Publish message
-        nmea_pub_.publish(msg);
+        nmea_pub_->publish(msg);
     }
 }