From 477c0deb4891dd95363dd73ea9e5008d0c6db57d Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Tue, 30 Apr 2024 15:13:25 +0200 Subject: [PATCH 01/11] support for odomsh --- fixposition_driver_lib/src/fixposition_driver.cpp | 5 ++++- .../fixposition_driver_node.hpp | 1 + .../src/fixposition_driver_node.cpp | 10 ++++++++++ 3 files changed, 15 insertions(+), 1 deletion(-) diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 50f0155..1277399 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -185,6 +185,8 @@ bool FixpositionDriver::InitializeConverters() { if (format == "ODOMETRY") { a_converters_["ODOMETRY"] = std::unique_ptr(new OdometryConverter()); a_converters_["TF"] = std::unique_ptr(new TfConverter()); + } else if (format == "ODOMSH") { + a_converters_["ODOMSH"] = std::unique_ptr(new OdometryConverter()); } else if (format == "LLH") { a_converters_["LLH"] = std::unique_ptr(new LlhConverter()); } else if (format == "RAWIMU") { @@ -307,7 +309,8 @@ 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" if (a_converters_[header] != nullptr) { a_converters_[header]->ConvertTokens(tokens); } 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 1df89e5..7978964 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 @@ -102,6 +102,7 @@ class FixpositionDriverNode : public FixpositionDriver { rclcpp::Publisher::SharedPtr navsatfix_gnss2_pub_; rclcpp::Publisher::SharedPtr nmea_pub_; rclcpp::Publisher::SharedPtr odometry_pub_; + rclcpp::Publisher::SharedPtr odometry_smooth_pub_; rclcpp::Publisher::SharedPtr poiimu_pub_; //!< Bias corrected IMU from ODOMETRY rclcpp::Publisher::SharedPtr vrtk_pub_; //!< VRTK message rclcpp::Publisher::SharedPtr odometry_enu0_pub_; //!< ENU0 Odometry diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index c8b7469..dc1b629 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -48,6 +48,7 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr node, navsatfix_gnss2_pub_(node_->create_publisher("/fixposition/gnss2", 100)), nmea_pub_(node_->create_publisher("/fixposition/nmea", 100)), odometry_pub_(node_->create_publisher("/fixposition/odometry", 100)), + odometry_smooth_pub_(node_->create_publisher("/fixposition/odomsh", 100)), poiimu_pub_(node_->create_publisher("/fixposition/poiimu", 100)), vrtk_pub_(node_->create_publisher("/fixposition/vrtk", 100)), odometry_enu0_pub_(node_->create_publisher("/fixposition/odometry_enu", 100)), @@ -146,6 +147,15 @@ void FixpositionDriverNode::RegisterObservers() { static_br_->sendTransform(tf_ecef_enu0); } }); + } else if (format == "ODOMSH") { + dynamic_cast(a_converters_["ODOMSH"].get()) + ->AddObserver([this](const OdometryConverter::Msgs& data) { + if (odometry_smooth_pub_->get_subscription_count() > 0) { + nav_msgs::msg::Odometry odometry; + OdometryDataToMsg(data.odometry, odometry); + odometry_pub_->publish(odometry); + } + }); } else if (format == "LLH" && a_converters_["LLH"]) { dynamic_cast(a_converters_["LLH"].get())->AddObserver([this](const NavSatFixData& data) { // LLH Observer Lambda From 7e3a180561c22dc27e250dbb6afce9af89a9e017 Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Tue, 30 Apr 2024 15:33:20 +0200 Subject: [PATCH 02/11] debug logs --- fixposition_driver_lib/src/fixposition_driver.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 1277399..1918e7d 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -311,7 +311,9 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { // If we have a converter available, convert to ros. // Currently supported are "FP", "LLH", "ODOMETRY", "ODOMSH", "TF", "RAWIMU", "CORRIMU" + std::cout << "Got message: " << header << "\n"; if (a_converters_[header] != nullptr) { + std::cout << "Converting message \n"; a_converters_[header]->ConvertTokens(tokens); } } From d6dd4ff040d5d24ce595ed9d48ed1d6d864e957f Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Tue, 30 Apr 2024 15:37:26 +0200 Subject: [PATCH 03/11] debug print --- fixposition_driver_lib/src/fixposition_driver.cpp | 3 +-- fixposition_driver_lib/src/odometry.cpp | 1 + 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 1918e7d..01ed77b 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -287,6 +287,7 @@ bool FixpositionDriver::ReadAndPublish() { void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { // split the msg into tokens, removing the *XX checksum + std::cout << "Converting msg: " << msg << std::endl; std::vector tokens; std::size_t star_pos = msg.find_last_of("*"); SplitMessage(tokens, msg.substr(1, star_pos - 1), ","); @@ -311,9 +312,7 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { // If we have a converter available, convert to ros. // Currently supported are "FP", "LLH", "ODOMETRY", "ODOMSH", "TF", "RAWIMU", "CORRIMU" - std::cout << "Got message: " << header << "\n"; if (a_converters_[header] != nullptr) { - std::cout << "Converting message \n"; a_converters_[header]->ConvertTokens(tokens); } } diff --git a/fixposition_driver_lib/src/odometry.cpp b/fixposition_driver_lib/src/odometry.cpp index adcf846..db8c983 100644 --- a/fixposition_driver_lib/src/odometry.cpp +++ b/fixposition_driver_lib/src/odometry.cpp @@ -92,6 +92,7 @@ void OdometryConverter::ConvertTokens(const std::vector& 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 From 211d7827772836cb3de17b3ab881c3aab210d746 Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Tue, 30 Apr 2024 15:43:23 +0200 Subject: [PATCH 04/11] token missing at the end of odomsh --- fixposition_driver_lib/src/fixposition_driver.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 01ed77b..bc11329 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -312,6 +312,9 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { // If we have a converter available, convert to ros. // Currently supported are "FP", "LLH", "ODOMETRY", "ODOMSH", "TF", "RAWIMU", "CORRIMU" + if (header == "ODOMSH") { + tokens.push_back(""); + } if (a_converters_[header] != nullptr) { a_converters_[header]->ConvertTokens(tokens); } From 75b17fd61d434e98b55da832b3fc7dbbadf5571d Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Tue, 30 Apr 2024 15:52:25 +0200 Subject: [PATCH 05/11] hacking the verion --- fixposition_driver_lib/src/fixposition_driver.cpp | 4 +++- fixposition_driver_lib/src/odometry.cpp | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index bc11329..d07aede 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -312,8 +312,10 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { // If we have a converter available, convert to ros. // Currently supported are "FP", "LLH", "ODOMETRY", "ODOMSH", "TF", "RAWIMU", "CORRIMU" + + // The odomsh does not seem to contain the firmware version if (header == "ODOMSH") { - tokens.push_back(""); + tokens.push_back("2"); } if (a_converters_[header] != nullptr) { a_converters_[header]->ConvertTokens(tokens); diff --git a/fixposition_driver_lib/src/odometry.cpp b/fixposition_driver_lib/src/odometry.cpp index db8c983..e52b2ca 100644 --- a/fixposition_driver_lib/src/odometry.cpp +++ b/fixposition_driver_lib/src/odometry.cpp @@ -101,7 +101,7 @@ void OdometryConverter::ConvertTokens(const std::vector& tokens) { 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"; } } From d0b8e49cdd5a29973d34cfa30af9181acc2b48ed Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Tue, 30 Apr 2024 15:57:04 +0200 Subject: [PATCH 06/11] forcing version on odomsh --- fixposition_driver_lib/src/fixposition_driver.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index d07aede..0e06a63 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -315,7 +315,8 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { // The odomsh does not seem to contain the firmware version if (header == "ODOMSH") { - tokens.push_back("2"); + tokens[2] = "2"; + tokens.push_back(""); } if (a_converters_[header] != nullptr) { a_converters_[header]->ConvertTokens(tokens); From 17c2117835fb8ef61376479c5d56eec82a68eeb7 Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Tue, 30 Apr 2024 16:00:46 +0200 Subject: [PATCH 07/11] typo --- fixposition_driver_ros2/src/fixposition_driver_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index dc1b629..5d4dbca 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -153,7 +153,7 @@ void FixpositionDriverNode::RegisterObservers() { if (odometry_smooth_pub_->get_subscription_count() > 0) { nav_msgs::msg::Odometry odometry; OdometryDataToMsg(data.odometry, odometry); - odometry_pub_->publish(odometry); + odometry_smooth_pub_->publish(odometry); } }); } else if (format == "LLH" && a_converters_["LLH"]) { From e9bb94c2efc18192eaab238f83da4655f9b8906d Mon Sep 17 00:00:00 2001 From: Giuseppe Rizzi Date: Tue, 30 Apr 2024 16:03:38 +0200 Subject: [PATCH 08/11] more comments and cleanup --- fixposition_driver_lib/src/fixposition_driver.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 0e06a63..49df3ff 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -287,7 +287,6 @@ bool FixpositionDriver::ReadAndPublish() { void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { // split the msg into tokens, removing the *XX checksum - std::cout << "Converting msg: " << msg << std::endl; std::vector tokens; std::size_t star_pos = msg.find_last_of("*"); SplitMessage(tokens, msg.substr(1, star_pos - 1), ","); @@ -313,7 +312,7 @@ void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) { // If we have a converter available, convert to ros. // Currently supported are "FP", "LLH", "ODOMETRY", "ODOMSH", "TF", "RAWIMU", "CORRIMU" - // The odomsh does not seem to contain the firmware version + // The odomsh does not seem to contain the firmware version, and has the wrong number of fields. if (header == "ODOMSH") { tokens[2] = "2"; tokens.push_back(""); From 84d7f3e0a3472c36277cc1990587a5a22cedc11f Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 2 May 2024 16:37:09 +0200 Subject: [PATCH 09/11] Added support for ROS1 --- README.md | 1 + fixposition_driver_lib/src/fixposition_driver.cpp | 6 ++++-- fixposition_driver_lib/src/odometry.cpp | 2 -- fixposition_driver_ros1/README.md | 2 ++ .../fixposition_driver_node.hpp | 13 +++++++------ .../src/fixposition_driver_node.cpp | 13 +++++++++++-- fixposition_driver_ros2/README.md | 2 ++ .../fixposition_driver_node.hpp | 2 +- .../src/fixposition_driver_node.cpp | 1 + 9 files changed, 29 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index 36edfca..465fb39 100644 --- a/README.md +++ b/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 | diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 49df3ff..3a40f7d 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -310,11 +310,13 @@ 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", "ODOMETRY", "ODOMSH", "TF", "RAWIMU", "CORRIMU" + // Currently supported are "FP", "LLH", "ODOMETRY", "ODOMSH", "TF", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC" - // The odomsh does not seem to contain the firmware version, and has the wrong number of fields. + // 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) { diff --git a/fixposition_driver_lib/src/odometry.cpp b/fixposition_driver_lib/src/odometry.cpp index e52b2ca..b10a8d3 100644 --- a/fixposition_driver_lib/src/odometry.cpp +++ b/fixposition_driver_lib/src/odometry.cpp @@ -92,8 +92,6 @@ void OdometryConverter::ConvertTokens(const std::vector& 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)); diff --git a/fixposition_driver_ros1/README.md b/fixposition_driver_ros1/README.md index c416da3..e231362 100644 --- a/fixposition_driver_ros1/README.md +++ b/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 | 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 f7e6ace..a267204 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 @@ -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_; diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 0609519..b6abb87 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -47,6 +47,7 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para nmea_pub_(nh_.advertise("/fixposition/nmea", 100)), // ODOMETRY odometry_pub_(nh_.advertise("/fixposition/odometry", 100)), + odometry_smooth_pub_(nh_.advertise("/fixposition/odomsh", 100)), poiimu_pub_(nh_.advertise("/fixposition/poiimu", 100)), vrtk_pub_(nh_.advertise("/fixposition/vrtk", 100)), odometry_enu0_pub_(nh_.advertise("/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(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(a_converters_["LLH"].get())->AddObserver([this](const NavSatFixData& data) { // LLH Observer Lambda diff --git a/fixposition_driver_ros2/README.md b/fixposition_driver_ros2/README.md index 551ffd1..ab7e949 100644 --- a/fixposition_driver_ros2/README.md +++ b/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 | 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 7978964..a32ad6c 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 @@ -106,7 +106,7 @@ class FixpositionDriverNode : public FixpositionDriver { rclcpp::Publisher::SharedPtr poiimu_pub_; //!< Bias corrected IMU from ODOMETRY rclcpp::Publisher::SharedPtr vrtk_pub_; //!< VRTK message rclcpp::Publisher::SharedPtr odometry_enu0_pub_; //!< ENU0 Odometry - rclcpp::Publisher::SharedPtr eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU + rclcpp::Publisher::SharedPtr eul_pub_; //!< Euler angles Yaw-Pitch-Roll in local ENU rclcpp::Publisher::SharedPtr eul_imu_pub_; //!< Euler angles Pitch-Roll as estimated from the IMU in // local horizontal diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 5d4dbca..88f31a7 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -150,6 +150,7 @@ void FixpositionDriverNode::RegisterObservers() { } else if (format == "ODOMSH") { dynamic_cast(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); From d943b013cafcb3f5b04e13e5c6a387bfac301995 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 2 May 2024 16:53:24 +0200 Subject: [PATCH 10/11] Updates formats in the TCP.yaml --- fixposition_driver_ros1/launch/tcp.yaml | 4 ++-- fixposition_driver_ros2/launch/tcp.yaml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml index bb8add1..fbfe18f 100644 --- a/fixposition_driver_ros1/launch/tcp.yaml +++ b/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 diff --git a/fixposition_driver_ros2/launch/tcp.yaml b/fixposition_driver_ros2/launch/tcp.yaml index 09d7079..22edddc 100644 --- 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", "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 From 3b305f970c97f3ae6baa37be5bb6ee4c63f7a719 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 2 May 2024 16:54:19 +0200 Subject: [PATCH 11/11] Updates serial.yaml files for ROS1 and ROS2 --- fixposition_driver_ros1/launch/serial.yaml | 2 +- fixposition_driver_ros2/launch/serial.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/fixposition_driver_ros1/launch/serial.yaml b/fixposition_driver_ros1/launch/serial.yaml index 81ab81e..60cf256 100644 --- a/fixposition_driver_ros1/launch/serial.yaml +++ b/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 diff --git a/fixposition_driver_ros2/launch/serial.yaml b/fixposition_driver_ros2/launch/serial.yaml index 85d07f9..65f2ac6 100644 --- a/fixposition_driver_ros2/launch/serial.yaml +++ b/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