Merge pull request #54 from fixposition/feature/odomshfacu

Feature/odomshfacu

README.md

@@ -153,6 +153,7 @@ The output is published on the following:
     | Topic                       | Message Type              | Frequency                      | Description                                                                                                                                                   |
     | --------------------------- | ------------------------- | ------------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------- |
     | `/fixposition/odometry`     | `nav_msgs/Odometry`       | as configured on web-interface | Position, Orientation from ECEF to FP_POI, Velocity and Angular Velocity in FP_POI                                                                            |
+    | `/fixposition/odomsh`       | `nav_msgs/Odometry`       | as configured on web-interface | Position, Orientation from ECEF to FP_POI, Velocity and Angular Velocity in FP_POI. Based on smooth odometry output.                                          |
     | `/fixposition/odometry_enu` | `nav_msgs/Odometry`       | as configured on web-interface | Position, Orientation from ENU0 to FP_POI, Velocity and Angular Velocity in FP_POI                                                                            |
     | `/fixposition/vrtk`         | `fixposition_driver/VRTK` | as configured on web-interface | Custom Message containing same Odometry information as well as status flags                                                                                   |
     | `/fixposition/poiimu`       | `sensor_msgs/Imu`         | as configured on web-interface | Bias Corrected acceleration and rotation rate in FP_POI                                                                                                       |

fixposition_driver_lib/src/fixposition_driver.cpp

@@ -185,6 +185,8 @@ bool FixpositionDriver::InitializeConverters() {
         if (format == "ODOMETRY") {
             a_converters_["ODOMETRY"] = std::unique_ptr<OdometryConverter>(new OdometryConverter());
             a_converters_["TF"] = std::unique_ptr<TfConverter>(new TfConverter());
+        } else if (format == "ODOMSH") {
+            a_converters_["ODOMSH"] = std::unique_ptr<OdometryConverter>(new OdometryConverter());
         } else if (format == "LLH") {
             a_converters_["LLH"] = std::unique_ptr<LlhConverter>(new LlhConverter());
         } else if (format == "RAWIMU") {
@@ -307,7 +309,16 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) {
     }
     const std::string header = _header;
 
-    // If we have a converter available, convert to ros. Currently supported are "FP", "LLH", "TF", "RAWIMU", "CORRIMU"
+    // If we have a converter available, convert to ros.
+    // Currently supported are "FP", "LLH", "ODOMETRY", "ODOMSH", "TF", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC"
+
+    // Adapt ODOMSH message to be compatible with the ODOMETRY message
+    if (header == "ODOMSH") {
+        // Change software version
+        tokens[2] = "2";
+        // Add missing software field
+        tokens.push_back("");
+    }
     if (a_converters_[header] != nullptr) {
         a_converters_[header]->ConvertTokens(tokens);
     }

fixposition_driver_lib/src/odometry.cpp

@@ -92,15 +92,14 @@ void OdometryConverter::ConvertTokens(const std::vector<std::string>& tokens) {
         // Size is wrong
         std::cout << "Error in parsing Odometry string with " << tokens.size()
                   << " fields! Odometry and status messages will be empty.\n";
-
     } else {
         // If size is ok, check version
         const int version = std::stoi(tokens.at(msg_version_idx));
 
         ok = version == kVersion_;
         if (!ok) {
             // Version is wrong
-            std::cout << "Error in parsing Odometry string with verion " << version
+            std::cout << "Error in parsing Odometry string with version " << version
                       << " ! Odometry and status messages will be empty.\n";
         }
     }

fixposition_driver_ros1/README.md

@@ -85,6 +85,7 @@ To change the settings of TCP (IP, Port) or Serial (Baudrate, Port) connections,
    /fixposition/corrimu
    /fixposition/navsatfix
    /fixposition/odometry
+   /fixposition/odomsh
    /fixposition/odometry_enu
    /fixposition/poiimu
    /fixposition/rawimu
@@ -110,6 +111,7 @@ The output is published on the following:
     | Topic                       | Message Type              | Frequency                      | Description                                                                                                                                                   |
     | --------------------------- | ------------------------- | ------------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------- |
     | `/fixposition/odometry`     | `nav_msgs/Odometry`       | as configured on web-interface | Position, Orientation from ECEF to FP_POI, Velocity and Angular Velocity in FP_POI                                                                            |
+    | `/fixposition/odomsh`       | `nav_msgs/Odometry`       | as configured on web-interface | Position, Orientation from ECEF to FP_POI, Velocity and Angular Velocity in FP_POI. Based on smooth odometry output.                                          |
     | `/fixposition/odometry_enu` | `nav_msgs/Odometry`       | as configured on web-interface | Position, Orientation from ENU0 to FP_POI, Velocity and Angular Velocity in FP_POI                                                                            |
     | `/fixposition/vrtk`         | `fixposition_driver/VRTK` | as configured on web-interface | Custom Message containing same Odometry information as well as status flags                                                                                   |
     | `/fixposition/poiimu`       | `sensor_msgs/Imu`         | as configured on web-interface | Bias Corrected acceleration and rotation rate in FP_POI                                                                                                       |

fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp

@@ -98,12 +98,13 @@ class FixpositionDriverNode : public FixpositionDriver {
     ros::Publisher navsatfix_gnss1_pub_;
     ros::Publisher navsatfix_gnss2_pub_;
     ros::Publisher nmea_pub_;
-    ros::Publisher odometry_pub_;       //!< ECEF Odometry
-    ros::Publisher poiimu_pub_;         //!< Bias corrected IMU
-    ros::Publisher vrtk_pub_;           //!< VRTK message
-    ros::Publisher odometry_enu0_pub_;  //!< ENU0 Odometry
-    ros::Publisher eul_pub_;            //!< Euler angles Yaw-Pitch-Roll in local ENU
-    ros::Publisher eul_imu_pub_;        //!< Euler angles Pitch-Roll as estimated from the IMU in local horizontal
+    ros::Publisher odometry_pub_;        //!< ECEF Odometry
+    ros::Publisher odometry_smooth_pub_; //!< ECEF Smooth Odometry
+    ros::Publisher poiimu_pub_;          //!< Bias corrected IMU
+    ros::Publisher vrtk_pub_;            //!< VRTK message
+    ros::Publisher odometry_enu0_pub_;   //!< ENU0 Odometry
+    ros::Publisher eul_pub_;             //!< Euler angles Yaw-Pitch-Roll in local ENU
+    ros::Publisher eul_imu_pub_;         //!< Euler angles Pitch-Roll as estimated from the IMU in local horizontal
 
     NmeaMessage nmea_message_;
 

fixposition_driver_ros1/launch/serial.yaml

@@ -1,5 +1,5 @@
 fp_output:
-    formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "TF"]
+    formats: ["ODOMETRY", "ODOMSH", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"]
     type: serial
     port: "/dev/ttyUSB0"
     serial_baudrate: 115200

fixposition_driver_ros1/launch/tcp.yaml

@@ -1,8 +1,8 @@
 fp_output:
-    formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"]
+    formats: ["ODOMETRY", "ODOMSH", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"]
     type: tcp
     port: "21000"
-    ip: "172.22.1.46" # 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
 

fixposition_driver_ros1/src/fixposition_driver_node.cpp

@@ -47,6 +47,7 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para
       nmea_pub_(nh_.advertise<fixposition_driver_ros1::NMEA>("/fixposition/nmea", 100)),
       //   ODOMETRY
       odometry_pub_(nh_.advertise<nav_msgs::Odometry>("/fixposition/odometry", 100)),
+      odometry_smooth_pub_(nh_.advertise<nav_msgs::Odometry>("/fixposition/odomsh", 100)),
       poiimu_pub_(nh_.advertise<sensor_msgs::Imu>("/fixposition/poiimu", 100)),
       vrtk_pub_(nh_.advertise<fixposition_driver_ros1::VRTK>("/fixposition/vrtk", 100)),
       odometry_enu0_pub_(nh_.advertise<nav_msgs::Odometry>("/fixposition/odometry_enu", 100)),
@@ -57,8 +58,6 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para
                                                             &FixpositionDriverNode::WsCallback, this,
                                                             ros::TransportHints().tcpNoDelay());
 
-
-
     RegisterObservers();
 }
 
@@ -123,6 +122,16 @@ void FixpositionDriverNode::RegisterObservers() {
                         static_br_.sendTransform(tf_ecef_enu0);
                     }
                 });
+        } else if (format == "ODOMSH") {
+            dynamic_cast<OdometryConverter*>(a_converters_["ODOMSH"].get())
+                ->AddObserver([this](const OdometryConverter::Msgs& data) {
+                    // ODOMSH Observer Lambda
+                    if (odometry_smooth_pub_.getNumSubscribers() > 0) {
+                        nav_msgs::Odometry odometry;
+                        OdometryDataToMsg(data.odometry, odometry);
+                        odometry_smooth_pub_.publish(odometry);
+                    }
+                });
         } else if (format == "LLH" && a_converters_["LLH"]) {
             dynamic_cast<LlhConverter*>(a_converters_["LLH"].get())->AddObserver([this](const NavSatFixData& data) {
                 // LLH Observer Lambda

fixposition_driver_ros2/README.md

@@ -85,6 +85,7 @@ To change the settings of TCP (IP, Port) or Serial (Baudrate, Port) connections,
    /fixposition/corrimu
    /fixposition/navsatfix
    /fixposition/odometry
+   /fixposition/odomsh
    /fixposition/odometry_enu
    /fixposition/poiimu
    /fixposition/rawimu
@@ -110,6 +111,7 @@ The output is published on the following:
     | Topic                       | Message Type              | Frequency                      | Description                                                                                                                                                   |
     | --------------------------- | ------------------------- | ------------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------- |
     | `/fixposition/odometry`     | `nav_msgs/Odometry`       | as configured on web-interface | Position, Orientation from ECEF to FP_POI, Velocity and Angular Velocity in FP_POI                                                                            |
+    | `/fixposition/odomsh`       | `nav_msgs/Odometry`       | as configured on web-interface | Position, Orientation from ECEF to FP_POI, Velocity and Angular Velocity in FP_POI. Based on smooth odometry output.                                          |
     | `/fixposition/odometry_enu` | `nav_msgs/Odometry`       | as configured on web-interface | Position, Orientation from ENU0 to FP_POI, Velocity and Angular Velocity in FP_POI                                                                            |
     | `/fixposition/vrtk`         | `fixposition_driver/VRTK` | as configured on web-interface | Custom Message containing same Odometry information as well as status flags                                                                                   |
     | `/fixposition/poiimu`       | `sensor_msgs/Imu`         | as configured on web-interface | Bias Corrected acceleration and rotation rate in FP_POI                                                                                                       |

fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp

@@ -102,10 +102,11 @@ class FixpositionDriverNode : public FixpositionDriver {
     rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_gnss2_pub_;
     rclcpp::Publisher<fixposition_driver_ros2::msg::NMEA>::SharedPtr nmea_pub_;
     rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_pub_;
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_smooth_pub_;
     rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr poiimu_pub_;             //!< Bias corrected IMU from ODOMETRY
     rclcpp::Publisher<fixposition_driver_ros2::msg::VRTK>::SharedPtr vrtk_pub_;  //!< VRTK message
     rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_enu0_pub_;    //!< ENU0 Odometry
-    rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr eul_pub_;  //!< Euler angles Yaw-Pitch-Roll in local ENU
+    rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr eul_pub_;   //!< Euler angles Yaw-Pitch-Roll in local ENU
     rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr
         eul_imu_pub_;  //!< Euler angles Pitch-Roll as estimated from the IMU in
                        // local horizontal

fixposition_driver_ros2/launch/serial.yaml

@@ -1,7 +1,7 @@
 fixposition_driver_ros2:
   ros__parameters:
     fp_output:
-      formats: ["FP", "LLH"]
+      formats: ["ODOMETRY", "ODOMSH", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"]
       type: "serial"
       port: "/dev/ttyUSB0"
       baudrate: 115200

fixposition_driver_ros2/launch/tcp.yaml

@@ -1,7 +1,7 @@
 fixposition_driver_ros2:
   ros__parameters:
     fp_output:
-      formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"]
+      formats: ["ODOMETRY", "ODOMSH", "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

fixposition_driver_ros2/src/fixposition_driver_node.cpp

@@ -48,6 +48,7 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr<rclcpp::Node> node,
       navsatfix_gnss2_pub_(node_->create_publisher<sensor_msgs::msg::NavSatFix>("/fixposition/gnss2", 100)),
       nmea_pub_(node_->create_publisher<fixposition_driver_ros2::msg::NMEA>("/fixposition/nmea", 100)),
       odometry_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("/fixposition/odometry", 100)),
+      odometry_smooth_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("/fixposition/odomsh", 100)),
       poiimu_pub_(node_->create_publisher<sensor_msgs::msg::Imu>("/fixposition/poiimu", 100)),
       vrtk_pub_(node_->create_publisher<fixposition_driver_ros2::msg::VRTK>("/fixposition/vrtk", 100)),
       odometry_enu0_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("/fixposition/odometry_enu", 100)),
@@ -146,6 +147,16 @@ void FixpositionDriverNode::RegisterObservers() {
                         static_br_->sendTransform(tf_ecef_enu0);
                     }
                 });
+        } else if (format == "ODOMSH") {
+            dynamic_cast<OdometryConverter*>(a_converters_["ODOMSH"].get())
+                ->AddObserver([this](const OdometryConverter::Msgs& data) {
+                    // ODOMSH Observer Lambda
+                    if (odometry_smooth_pub_->get_subscription_count() > 0) {
+                        nav_msgs::msg::Odometry odometry;
+                        OdometryDataToMsg(data.odometry, odometry);
+                        odometry_smooth_pub_->publish(odometry);
+                    }
+                });
         } else if (format == "LLH" && a_converters_["LLH"]) {
             dynamic_cast<LlhConverter*>(a_converters_["LLH"].get())->AddObserver([this](const NavSatFixData& data) {
                 // LLH Observer Lambda