From 696e4bf2f1cfcd3ca96296a8c0fd1a543856f522 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 13 Feb 2025 14:24:16 +0000 Subject: [PATCH 1/8] Added odometry_enu_smooth topic --- .../include/fixposition_driver_lib/helper.hpp | 1 + fixposition_driver_lib/src/helper.cpp | 37 +++++++++++++++++++ .../fixposition_driver_node.hpp | 10 +++-- .../src/fixposition_driver_node.cpp | 11 ++++++ .../fixposition_driver_node.hpp | 10 +++-- .../src/fixposition_driver_node.cpp | 11 ++++++ 6 files changed, 72 insertions(+), 8 deletions(-) diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp index 696afdb..787c3ae 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp @@ -103,6 +103,7 @@ struct OdometryData { PoseWithCovData pose; TwistWithCovData twist; bool SetFromFpaOdomPayload(const fpsdk::common::parser::fpa::FpaOdomPayload& payload); + bool ConvertToEnu(const TfData& tf_ecef_enu0); }; /** diff --git a/fixposition_driver_lib/src/helper.cpp b/fixposition_driver_lib/src/helper.cpp index e667b07..781b772 100644 --- a/fixposition_driver_lib/src/helper.cpp +++ b/fixposition_driver_lib/src/helper.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -148,6 +149,42 @@ bool OdometryData::SetFromFpaOdomPayload(const fpa::FpaOdomPayload& payload) { // --------------------------------------------------------------------------------------------------------------------- +bool OdometryData::ConvertToEnu(const TfData& tf_ecef_enu0) { + // Check that TF data is valid + if (!tf_ecef_enu0.valid) { + return false; + } + + // Extract data from the odometry message + const Eigen::Vector3d t_ecef_body = pose.position; + const Eigen::Quaterniond q_ecef_body = pose.orientation; + const Eigen::Matrix cov_ecef = pose.cov; + + // Extract data from the TF message (using the arrow operator) + const Eigen::Vector3d t_ecef_enu0 = tf_ecef_enu0.translation; + const Eigen::Quaterniond q_ecef_enu0 = tf_ecef_enu0.rotation; + const Eigen::Matrix3d rot_ecef_enu0 = q_ecef_enu0.toRotationMatrix(); + + // Convert position in ECEF into position in ENU + const Eigen::Vector3d t_enu_body = + fpsdk::common::trafo::TfEnuEcef(t_ecef_body, fpsdk::common::trafo::TfWgs84LlhEcef(t_ecef_enu0)); + const Eigen::Quaterniond q_enu_body = q_ecef_enu0.inverse() * q_ecef_body; + + // Convert covariance matrix to ENU + Eigen::Matrix cov_enu = Eigen::Matrix::Zero(); + cov_enu.topLeftCorner(3, 3) = rot_ecef_enu0 * cov_ecef.topLeftCorner(3, 3) * rot_ecef_enu0.transpose(); + cov_enu.bottomRightCorner(3, 3) = rot_ecef_enu0 * cov_ecef.bottomRightCorner(3, 3) * rot_ecef_enu0.transpose(); + + // Repopulate odometry data + pose.position = t_enu_body; + pose.orientation = q_enu_body; + pose.cov = cov_enu; + + return true; +} + +// --------------------------------------------------------------------------------------------------------------------- + bool TfData::SetFromFpaTfPayload(const fpa::FpaTfPayload& payload) { bool ok = true; if (payload.gps_time.week.valid && payload.gps_time.tow.valid) { 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 6bd03d4..1a9d828 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 @@ -88,10 +88,11 @@ class FixpositionDriverNode { // - NOV_B messages ros::Publisher novb_inspvax_pub_; //!< NOV_B-INSPVAX message // - Odometry - ros::Publisher odometry_ecef_pub_; //!< ECEF odometry - ros::Publisher odometry_enu_pub_; //!< ENU odometry - ros::Publisher odometry_llh_pub_; //!< LLH odometry - ros::Publisher odometry_smooth_pub_; //!< Smooth Odometry (ECEF) + ros::Publisher odometry_ecef_pub_; //!< ECEF odometry + ros::Publisher odometry_enu_pub_; //!< ENU odometry + ros::Publisher odometry_smooth_pub_; //!< Smooth Odometry (ECEF) + ros::Publisher odometry_enu_smooth_pub_; //!< Smooth Odometry (ENU) + ros::Publisher odometry_llh_pub_; //!< LLH odometry // - Fusion ros::Publisher fusion_epoch_pub_; //!< Fusion epoch data // - Orientation @@ -132,6 +133,7 @@ class FixpositionDriverNode { std::unique_ptr enu0_poi_; }; Tfs tfs_; + std::unique_ptr ecef_enu0_tf_; void ProcessTfData(const TfData& tf_data); void ProcessOdometryData(const OdometryData& odometry_data); diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 2f8f94f..4d65649 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -93,6 +93,7 @@ bool FixpositionDriverNode::StartNode() { if (params_.MessageEnabled(fpa::FpaOdomshPayload::MSG_NAME)) { _PUB(fpa_odomsh_pub_, fixposition_driver_msgs::FpaOdomsh, output_ns + "/fpa/odomsh", 5); _PUB(odometry_smooth_pub_, nav_msgs::Odometry, output_ns + "/odometry_smooth", 5); + _PUB(odometry_enu_smooth_pub_, nav_msgs::Odometry, output_ns + "/odometry_enu_smooth", 5); driver_.AddFpaObserver(fpa::FpaOdomshPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { auto odomsh_payload = dynamic_cast(payload); PublishFpaOdomsh(odomsh_payload, fpa_odomsh_pub_); @@ -101,6 +102,14 @@ bool FixpositionDriverNode::StartNode() { PublishOdometryData(odometry_data, odometry_smooth_pub_); ProcessOdometryData(odometry_data); fusion_epoch_data_.CollectFpaOdomsh(odomsh_payload); + + // Convert message to ENU + if (ecef_enu0_tf_) { + bool enu_valid = odometry_data.ConvertToEnu(*ecef_enu0_tf_); + if (enu_valid) { + PublishOdometryData(odometry_data, odometry_enu_smooth_pub_); + } + } }); } @@ -457,6 +466,7 @@ void FixpositionDriverNode::StopNode() { odometry_enu_pub_.shutdown(); odometry_llh_pub_.shutdown(); odometry_smooth_pub_.shutdown(); + odometry_enu_smooth_pub_.shutdown(); // - Orientation eul_pub_.shutdown(); eul_imu_pub_.shutdown(); @@ -512,6 +522,7 @@ void FixpositionDriverNode::ProcessTfData(const TfData& tf_data) { // FP_ECEF -> FP_ENU0 else if ((tf.child_frame_id == "FP_ENU0") && (tf.header.frame_id == "FP_ECEF")) { static_br_.sendTransform(tf); + ecef_enu0_tf_ = std::make_unique(tf_data); // Store TF if Nav2 mode is enabled if (params_.nav2_mode_) { std::unique_lock lock(tfs_.mutex_); 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 672bb3f..ee71278 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 @@ -92,10 +92,11 @@ class FixpositionDriverNode { // - NOV_B messages rclcpp::Publisher::SharedPtr novb_inspvax_pub_; //!< NOV_B-INSPVAX message // - Odometry - rclcpp::Publisher::SharedPtr odometry_ecef_pub_; //!< ECEF odometry - rclcpp::Publisher::SharedPtr odometry_enu_pub_; //!< ENU odometry - rclcpp::Publisher::SharedPtr odometry_smooth_pub_; //!< Smooth odometry (ECEF) - rclcpp::Publisher::SharedPtr odometry_llh_pub_; //!< LLH odometry + rclcpp::Publisher::SharedPtr odometry_ecef_pub_; //!< ECEF odometry + rclcpp::Publisher::SharedPtr odometry_enu_pub_; //!< ENU odometry + rclcpp::Publisher::SharedPtr odometry_smooth_pub_; //!< Smooth odometry (ECEF) + rclcpp::Publisher::SharedPtr odometry_enu_smooth_pub_; //!< Smooth odometry (ENU) + rclcpp::Publisher::SharedPtr odometry_llh_pub_; //!< LLH odometry // - Fusion rclcpp::Publisher::SharedPtr fusion_epoch_pub_; //!< Fusion epoch data // - Orientation @@ -143,6 +144,7 @@ class FixpositionDriverNode { std::unique_ptr enu0_poi_; }; Tfs tfs_; + std::unique_ptr ecef_enu0_tf_; void ProcessTfData(const TfData& tf_data); void ProcessOdometryData(const OdometryData& odometry_data); diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 7a9b3b3..c818fbb 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -119,6 +119,7 @@ bool FixpositionDriverNode::StartNode() { if (params_.MessageEnabled(fpa::FpaOdomshPayload::MSG_NAME)) { _PUB(fpa_odomsh_pub_, fpmsgs::FpaOdomsh, output_ns + "/fpa/odomsh", qos_settings_); _PUB(odometry_smooth_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_smooth", qos_settings_); + _PUB(odometry_enu_smooth_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_enu_smooth", qos_settings_); driver_.AddFpaObserver(fpa::FpaOdomshPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { auto odomsh_payload = dynamic_cast(payload); PublishFpaOdomsh(odomsh_payload, fpa_odomsh_pub_); @@ -127,6 +128,14 @@ bool FixpositionDriverNode::StartNode() { PublishOdometryData(odometry_data, odometry_smooth_pub_); ProcessOdometryData(odometry_data); fusion_epoch_data_.CollectFpaOdomsh(odomsh_payload); + + // Convert message to ENU + if (ecef_enu0_tf_) { + bool enu_valid = odometry_data.ConvertToEnu(*ecef_enu0_tf_); + if (enu_valid) { + PublishOdometryData(odometry_data, odometry_enu_smooth_pub_); + } + } }); } @@ -484,6 +493,7 @@ void FixpositionDriverNode::StopNode() { odometry_enu_pub_.reset(); odometry_llh_pub_.reset(); odometry_smooth_pub_.reset(); + odometry_enu_smooth_pub_.reset(); // - Orientation eul_pub_.reset(); eul_imu_pub_.reset(); @@ -548,6 +558,7 @@ void FixpositionDriverNode::ProcessTfData(const TfData& tf_data) { // FP_ECEF -> FP_ENU0 else if ((tf.child_frame_id == "FP_ENU0") && (tf.header.frame_id == "FP_ECEF")) { static_br_->sendTransform(tf); + ecef_enu0_tf_ = std::make_unique(tf_data); // Store TF if Nav2 mode is enabled if (params_.nav2_mode_) { std::unique_lock lock(tfs_.mutex_); From c09a836a4d2565620c24ca2af3d0e322f0e6af03 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Thu, 13 Feb 2025 15:01:52 +0000 Subject: [PATCH 2/8] Changed frame_id of ODOMSH-ENU --- fixposition_driver_lib/src/helper.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/fixposition_driver_lib/src/helper.cpp b/fixposition_driver_lib/src/helper.cpp index 781b772..5057e92 100644 --- a/fixposition_driver_lib/src/helper.cpp +++ b/fixposition_driver_lib/src/helper.cpp @@ -179,6 +179,7 @@ bool OdometryData::ConvertToEnu(const TfData& tf_ecef_enu0) { pose.position = t_enu_body; pose.orientation = q_enu_body; pose.cov = cov_enu; + frame_id = ODOMENU_FRAME_ID; return true; } From d7aeaa1f837744c4bab65730ab34ef972bcaaf28 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 7 Mar 2025 16:58:34 +0000 Subject: [PATCH 3/8] Nav2 update --- fixposition_driver_lib/src/helper.cpp | 1 - .../fixposition_driver_ros1/data_to_ros1.hpp | 5 +- .../fixposition_driver_node.hpp | 5 +- fixposition_driver_ros1/src/data_to_ros1.cpp | 56 +++++++++++++++--- .../src/fixposition_driver_node.cpp | 51 ++++++++++++++--- .../fixposition_driver_ros2/data_to_ros2.hpp | 6 +- .../fixposition_driver_node.hpp | 5 +- fixposition_driver_ros2/src/data_to_ros2.cpp | 57 ++++++++++++++++--- .../src/fixposition_driver_node.cpp | 53 +++++++++++++---- 9 files changed, 192 insertions(+), 47 deletions(-) diff --git a/fixposition_driver_lib/src/helper.cpp b/fixposition_driver_lib/src/helper.cpp index 5057e92..781b772 100644 --- a/fixposition_driver_lib/src/helper.cpp +++ b/fixposition_driver_lib/src/helper.cpp @@ -179,7 +179,6 @@ bool OdometryData::ConvertToEnu(const TfData& tf_ecef_enu0) { pose.position = t_enu_body; pose.orientation = q_enu_body; pose.cov = cov_enu; - frame_id = ODOMENU_FRAME_ID; return true; } diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp index e47f9e0..fcf4620 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp @@ -35,8 +35,8 @@ void TfDataToTransformStamped(const TfData& data, geometry_msgs::TransformStampe void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::TransformStamped& msg); void PublishFpaOdometry(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, ros::Publisher& pub); -void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, ros::Publisher& pub); -void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, +void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, ros::Publisher& pub); +void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, ros::Publisher& pub); void PublishFpaOdomenu(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload, ros::Publisher& pub); void PublishFpaOdomenuVector3Stamped(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload, ros::Publisher& pub); @@ -73,6 +73,7 @@ void PublishParserMsg(const fpsdk::common::parser::ParserMsg& msg, ros::Publishe void PublishNmeaEpochData(const NmeaEpochData& data, ros::Publisher& pub); void PublishOdometryData(const OdometryData& data, ros::Publisher& pub); void PublishJumpWarning(const JumpDetector& jump_detector, ros::Publisher& pub); +void PublishDatum(const geometry_msgs::Vector3& payload, const ros::Time& stamp, ros::Publisher& pub); void PublishFusionEpochData(const FusionEpochData& data, ros::Publisher& pub); /* ****************************************************************************************************************** */ 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 1a9d828..d3c5c4b 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 @@ -107,8 +107,9 @@ class FixpositionDriverNode { ros::Publisher navsatfix_gnss2_pub_; //!< GNSS2 position and status ros::Publisher nmea_epoch_pub_; //!< NMEA epoch data // - Other - ros::Publisher jump_pub_; //!< Jump warning topic - ros::Publisher raw_pub_; //!< Raw messages topic + ros::Publisher jump_pub_; //!< Jump warning topic + ros::Publisher raw_pub_; //!< Raw messages topic + ros::Publisher datum_pub_; //!< WGS84 datum topic // ROS subscribers ros::Subscriber ws_sub_; //!< Wheelspeed input subscriber diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 887de05..8820d9e 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -135,11 +135,15 @@ void PublishFpaOdomsh(const fpa::FpaOdomshPayload& payload, ros::Publisher& pub) // --------------------------------------------------------------------------------------------------------------------- -void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, ros::Publisher& pub) { +void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, bool nav2_mode_, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { sensor_msgs::Imu msg; msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); - msg.header.frame_id = ODOMETRY_FRAME_ID; + if (nav2_mode_) { + msg.header.frame_id = "vrtk_link"; + } else { + msg.header.frame_id = ODOMETRY_FRAME_ID; + } FpaFloat3ToVector3(payload.acc, msg.linear_acceleration); FpaFloat3ToVector3(payload.rot, msg.angular_velocity); pub.publish(msg); @@ -148,11 +152,15 @@ void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, ros::Publ // --------------------------------------------------------------------------------------------------------------------- -void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, ros::Publisher& pub) { +void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, bool nav2_mode_, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { sensor_msgs::NavSatFix msg; msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); - msg.header.frame_id = ODOMETRY_CHILD_FRAME_ID; + if (nav2_mode_) { + msg.header.frame_id = "vrtk_link"; + } else { + msg.header.frame_id = ODOMETRY_CHILD_FRAME_ID; + } // Populate LLH position PoseWithCovData pose; @@ -178,11 +186,16 @@ void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, ros } // Populate LLH status - const fpa::FpaGnssFix fix = (payload.gnss1_fix > payload.gnss2_fix ? payload.gnss1_fix : payload.gnss2_fix); - msg.status.status = FpaGnssFixToNavSatStatusStatus(msg.status, fix); - if (msg.status.status != msg.status.STATUS_NO_FIX) { - msg.status.service = (msg.status.SERVICE_GPS | msg.status.SERVICE_GLONASS | msg.status.SERVICE_COMPASS | - msg.status.SERVICE_GALILEO); + if (nav2_mode_) { + msg.status.status = 2; + msg.status.service = 15; + } else { + const fpa::FpaGnssFix fix = (payload.gnss1_fix > payload.gnss2_fix ? payload.gnss1_fix : payload.gnss2_fix); + msg.status.status = FpaGnssFixToNavSatStatusStatus(msg.status, fix); + if (msg.status.status != msg.status.STATUS_NO_FIX) { + msg.status.service = (msg.status.SERVICE_GPS | msg.status.SERVICE_GLONASS | msg.status.SERVICE_COMPASS | + msg.status.SERVICE_GALILEO); + } } // Publish message @@ -800,6 +813,31 @@ void PublishJumpWarning(const JumpDetector& jump_detector, ros::Publisher& pub) // --------------------------------------------------------------------------------------------------------------------- +void PublishDatum(const geometry_msgs::Vector3& payload, const ros::Time& stamp, ros::Publisher& pub) { + if (pub.getNumSubscribers() > 0) { + sensor_msgs::NavSatFix msg; + msg.header.stamp = stamp; + msg.header.frame_id = "vrtk_link"; + + // Populate LLH position + const Eigen::Vector3d position = {payload.x, payload.y, payload.z}; + const Eigen::Vector3d llh_pos = trafo::TfWgs84LlhEcef(position); + msg.latitude = math::RadToDeg(llh_pos(0)); + msg.longitude = math::RadToDeg(llh_pos(1)); + msg.altitude = llh_pos(2); + + // Populate status + msg.status.status = 2; + msg.status.service = 15; + msg.position_covariance_type = 3; + + // Publish message + pub.publish(msg); + } +} + +// --------------------------------------------------------------------------------------------------------------------- + void PublishFusionEpochData(const FusionEpochData& data, ros::Publisher& pub) { if (pub.getNumSubscribers() > 0) { fixposition_driver_msgs::FusionEpoch msg; diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 4d65649..8e95472 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -79,8 +79,8 @@ bool FixpositionDriverNode::StartNode() { driver_.AddFpaObserver(fpa::FpaOdometryPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { auto odometry_payload = dynamic_cast(payload); PublishFpaOdometry(odometry_payload, fpa_odometry_pub_); - PublishFpaOdometryDataImu(odometry_payload, poiimu_pub_); - PublishFpaOdometryDataNavSatFix(odometry_payload, odometry_llh_pub_); + PublishFpaOdometryDataImu(odometry_payload, params_.nav2_mode_, poiimu_pub_); + PublishFpaOdometryDataNavSatFix(odometry_payload, params_.nav2_mode_, odometry_llh_pub_); OdometryData odometry_data; odometry_data.SetFromFpaOdomPayload(odometry_payload); PublishOdometryData(odometry_data, odometry_ecef_pub_); @@ -99,6 +99,13 @@ bool FixpositionDriverNode::StartNode() { PublishFpaOdomsh(odomsh_payload, fpa_odomsh_pub_); OdometryData odometry_data; odometry_data.SetFromFpaOdomPayload(odomsh_payload); + + // Update frames for Nav2 + if (params_.nav2_mode_) { + odometry_data.frame_id = "odom"; + odometry_data.child_frame_id = "vrtk_link"; + } + PublishOdometryData(odometry_data, odometry_smooth_pub_); ProcessOdometryData(odometry_data); fusion_epoch_data_.CollectFpaOdomsh(odomsh_payload); @@ -124,6 +131,13 @@ bool FixpositionDriverNode::StartNode() { PublishFpaOdomenuVector3Stamped(odomenu_payload, eul_pub_); OdometryData odometry_data; odometry_data.SetFromFpaOdomPayload(odomenu_payload); + + // Update frames for Nav2 + if (params_.nav2_mode_) { + odometry_data.frame_id = "map"; + odometry_data.child_frame_id = "vrtk_link"; + } + PublishOdometryData(odometry_data, odometry_enu_pub_); ProcessOdometryData(odometry_data); fusion_epoch_data_.CollectFpaOdomenu(odomenu_payload); @@ -377,6 +391,11 @@ bool FixpositionDriverNode::StartNode() { _PUB(jump_pub_, fixposition_driver_msgs::CovWarn, output_ns + "/extras/jump", 5); } + // WGS84 datum message + if (params_.nav2_mode_) { + _PUB(datum_pub_, sensor_msgs::NavSatFix, output_ns + "/datum", 5); + } + // Subscribe to correction data input if (!params_.corr_topic_.empty()) { _SUB(corr_sub_, rtcm_msgs::Message, params_.corr_topic_, 100, [this](const rtcm_msgs::MessageConstPtr& msg) { @@ -482,6 +501,7 @@ void FixpositionDriverNode::StopNode() { // - Other jump_pub_.shutdown(); raw_pub_.shutdown(); + datum_pub_.shutdown(); // Stop input message subscribers ws_sub_.shutdown(); @@ -598,9 +618,19 @@ void FixpositionDriverNode::PublishNav2Tf() { return; } - // Publish FP_ECEF -> map - tfs_.ecef_enu0_->child_frame_id = "map"; - static_br_.sendTransform(*tfs_.ecef_enu0_); + // Publish a static identity transform from FP_ENU0 to map + geometry_msgs::msg::TransformStamped static_transform; + static_transform.header.stamp = tfs_.ecef_enu0_->header.stamp; + static_transform.header.frame_id = "FP_ENU0"; + static_transform.child_frame_id = "map"; + static_transform.transform.translation.x = 0.0; + static_transform.transform.translation.y = 0.0; + static_transform.transform.translation.z = 0.0; + static_transform.transform.rotation.w = 1.0; + static_transform.transform.rotation.x = 0.0; + static_transform.transform.rotation.y = 0.0; + static_transform.transform.rotation.z = 0.0; + static_br_.sendTransform(static_transform); // Compute FP_ENU0 -> FP_POISH // Extract translation and rotation from ECEFENU0 @@ -635,21 +665,24 @@ void FixpositionDriverNode::PublishNav2Tf() { // Create a new TransformStamped message geometry_msgs::TransformStamped tfs_odom; - tfs_odom.header.stamp = ros::Time::now(); + tfs_odom.header.stamp = tfs_.enu0_poi_->header.stamp; tfs_odom.header.frame_id = "map"; tfs_odom.child_frame_id = "odom"; tfs_odom.transform = tf2::toMsg(tf_combined); tf_br_.sendTransform(tfs_odom); - // Publish odom -> base_link + // Publish odom -> vrtk_link geometry_msgs::TransformStamped tf_odom_base; - tf_odom_base.header.stamp = ros::Time::now(); + tf_odom_base.header.stamp = tfs_.enu0_poi_->header.stamp; tf_odom_base.header.frame_id = "odom"; - tf_odom_base.child_frame_id = "base_link"; + tf_odom_base.child_frame_id = "vrtk_link"; tf_odom_base.transform = tf2::toMsg(tf_ENU0POISH); // Send the transform tf_br_.sendTransform(tf_odom_base); + + // Publish WGS84 datum + PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, datum_pub_); } /* ****************************************************************************************************************** */ diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp index 087266b..46c8176 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp @@ -36,9 +36,9 @@ void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::msg void PublishFpaOdometry(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, rclcpp::Publisher::SharedPtr& pub); -void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, +void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, rclcpp::Publisher::SharedPtr& pub); -void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, +void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, rclcpp::Publisher::SharedPtr& pub); void PublishFpaOdomenu(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload, rclcpp::Publisher::SharedPtr& pub); @@ -100,6 +100,8 @@ void PublishParserMsg(const fpsdk::common::parser::ParserMsg& msg, void PublishNmeaEpochData(const NmeaEpochData& data, rclcpp::Publisher::SharedPtr& pub); void PublishOdometryData(const OdometryData& data, rclcpp::Publisher::SharedPtr& pub); void PublishJumpWarning(const JumpDetector& jump_detector, rclcpp::Publisher::SharedPtr& pub); +void PublishDatum(const geometry_msgs::msg::Vector3& payload, const builtin_interfaces::msg::Time& stamp, + rclcpp::Publisher::SharedPtr& pub); void PublishFusionEpochData(const FusionEpochData& data, rclcpp::Publisher::SharedPtr& pub); /* ****************************************************************************************************************** */ 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 ee71278..682ba22 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 @@ -113,8 +113,9 @@ class FixpositionDriverNode { rclcpp::Publisher::SharedPtr navsatfix_gnss1_pub_; //!< GNSS1 position and status rclcpp::Publisher::SharedPtr navsatfix_gnss2_pub_; //!< GNSS2 position and status // - Other - rclcpp::Publisher::SharedPtr jump_pub_; //!< Jump warning topic - rclcpp::Publisher::SharedPtr raw_pub_; //!< Raw messages topic + rclcpp::Publisher::SharedPtr jump_pub_; //!< Jump warning topic + rclcpp::Publisher::SharedPtr raw_pub_; //!< Raw messages topic + rclcpp::Publisher::SharedPtr datum_pub_; //!< WGS84 datum topic // ROS subscribers rclcpp::Subscription::SharedPtr ws_sub_; //!< Wheelspeed input subscriber diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index fce0ac3..d271614 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -135,12 +135,16 @@ void PublishFpaOdomsh(const fpa::FpaOdomshPayload& payload, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { sensor_msgs::msg::Imu msg; msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); - msg.header.frame_id = ODOMETRY_FRAME_ID; + if (nav2_mode_) { + msg.header.frame_id = "vrtk_link"; + } else { + msg.header.frame_id = ODOMETRY_FRAME_ID; + } FpaFloat3ToVector3(payload.acc, msg.linear_acceleration); FpaFloat3ToVector3(payload.rot, msg.angular_velocity); pub->publish(msg); @@ -149,12 +153,16 @@ void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, // --------------------------------------------------------------------------------------------------------------------- -void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, +void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, bool nav2_mode_, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { sensor_msgs::msg::NavSatFix msg; msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time)); - msg.header.frame_id = ODOMETRY_CHILD_FRAME_ID; + if (nav2_mode_) { + msg.header.frame_id = "vrtk_link"; + } else { + msg.header.frame_id = ODOMETRY_CHILD_FRAME_ID; + } // Populate LLH position PoseWithCovData pose; @@ -180,11 +188,16 @@ void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, } // Populate LLH status - const fpa::FpaGnssFix fix = (payload.gnss1_fix > payload.gnss2_fix ? payload.gnss1_fix : payload.gnss2_fix); - msg.status.status = FpaGnssFixToNavSatStatusStatus(msg.status, fix); - if (msg.status.status != msg.status.STATUS_NO_FIX) { - msg.status.service = (msg.status.SERVICE_GPS | msg.status.SERVICE_GLONASS | msg.status.SERVICE_COMPASS | - msg.status.SERVICE_GALILEO); + if (nav2_mode_) { + msg.status.status = 2; + msg.status.service = 15; + } else { + const fpa::FpaGnssFix fix = (payload.gnss1_fix > payload.gnss2_fix ? payload.gnss1_fix : payload.gnss2_fix); + msg.status.status = FpaGnssFixToNavSatStatusStatus(msg.status, fix); + if (msg.status.status != msg.status.STATUS_NO_FIX) { + msg.status.service = (msg.status.SERVICE_GPS | msg.status.SERVICE_GLONASS | msg.status.SERVICE_COMPASS | + msg.status.SERVICE_GALILEO); + } } // Publish message @@ -821,6 +834,32 @@ void PublishJumpWarning(const JumpDetector& jump_detector, rclcpp::Publisher::SharedPtr& pub) { + if (pub->get_subscription_count() > 0) { + sensor_msgs::msg::NavSatFix msg; + msg.header.stamp = stamp; + msg.header.frame_id = "vrtk_link"; + + // Populate LLH position + const Eigen::Vector3d position = {payload.x, payload.y, payload.z}; + const Eigen::Vector3d llh_pos = trafo::TfWgs84LlhEcef(position); + msg.latitude = math::RadToDeg(llh_pos(0)); + msg.longitude = math::RadToDeg(llh_pos(1)); + msg.altitude = llh_pos(2); + + // Populate status + msg.status.status = 2; + msg.status.service = 15; + msg.position_covariance_type = 3; + + // Publish message + pub->publish(msg); + } +} + +// --------------------------------------------------------------------------------------------------------------------- + void PublishFusionEpochData(const FusionEpochData& data, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { fpmsgs::FusionEpoch msg; diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index c818fbb..9e4a432 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -105,8 +105,8 @@ bool FixpositionDriverNode::StartNode() { driver_.AddFpaObserver(fpa::FpaOdometryPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) { auto odometry_payload = dynamic_cast(payload); PublishFpaOdometry(odometry_payload, fpa_odometry_pub_); - PublishFpaOdometryDataImu(odometry_payload, poiimu_pub_); - PublishFpaOdometryDataNavSatFix(odometry_payload, odometry_llh_pub_); + PublishFpaOdometryDataImu(odometry_payload, params_.nav2_mode_, poiimu_pub_); + PublishFpaOdometryDataNavSatFix(odometry_payload, params_.nav2_mode_, odometry_llh_pub_); OdometryData odometry_data; odometry_data.SetFromFpaOdomPayload(odometry_payload); PublishOdometryData(odometry_data, odometry_ecef_pub_); @@ -125,6 +125,13 @@ bool FixpositionDriverNode::StartNode() { PublishFpaOdomsh(odomsh_payload, fpa_odomsh_pub_); OdometryData odometry_data; odometry_data.SetFromFpaOdomPayload(odomsh_payload); + + // Update frames for Nav2 + if (params_.nav2_mode_) { + odometry_data.frame_id = "odom"; + odometry_data.child_frame_id = "vrtk_link"; + } + PublishOdometryData(odometry_data, odometry_smooth_pub_); ProcessOdometryData(odometry_data); fusion_epoch_data_.CollectFpaOdomsh(odomsh_payload); @@ -150,6 +157,13 @@ bool FixpositionDriverNode::StartNode() { PublishFpaOdomenuVector3Stamped(odomenu_payload, eul_pub_); OdometryData odometry_data; odometry_data.SetFromFpaOdomPayload(odomenu_payload); + + // Update frames for Nav2 + if (params_.nav2_mode_) { + odometry_data.frame_id = "map"; + odometry_data.child_frame_id = "vrtk_link"; + } + PublishOdometryData(odometry_data, odometry_enu_pub_); ProcessOdometryData(odometry_data); fusion_epoch_data_.CollectFpaOdomenu(odomenu_payload); @@ -405,6 +419,11 @@ bool FixpositionDriverNode::StartNode() { _PUB(jump_pub_, fpmsgs::CovWarn, output_ns + "/extras/jump", qos_settings_); } + // WGS84 datum message + if (params_.nav2_mode_) { + _PUB(datum_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/datum", qos_settings_); + } + // Subscribe to correction data input if (!params_.corr_topic_.empty()) { _SUB(corr_sub_, rtcm_msgs::msg::Message, params_.corr_topic_, 100, [this](const rtcm_msgs::msg::Message& msg) { @@ -509,6 +528,7 @@ void FixpositionDriverNode::StopNode() { // - Other jump_pub_.reset(); raw_pub_.reset(); + datum_pub_.reset(); // Stop input message subscribers ws_sub_.reset(); @@ -635,9 +655,19 @@ void FixpositionDriverNode::PublishNav2Tf() { return; } - // Publish FP_ECEF -> map - tfs_.ecef_enu0_->child_frame_id = "map"; - static_br_->sendTransform(*tfs_.ecef_enu0_); + // Publish a static identity transform from FP_ENU0 to map + geometry_msgs::msg::TransformStamped static_transform; + static_transform.header.stamp = tfs_.ecef_enu0_->header.stamp; + static_transform.header.frame_id = "FP_ENU0"; + static_transform.child_frame_id = "map"; + static_transform.transform.translation.x = 0.0; + static_transform.transform.translation.y = 0.0; + static_transform.transform.translation.z = 0.0; + static_transform.transform.rotation.w = 1.0; + static_transform.transform.rotation.x = 0.0; + static_transform.transform.rotation.y = 0.0; + static_transform.transform.rotation.z = 0.0; + static_br_->sendTransform(static_transform); // Compute FP_ENU0 -> FP_POISH // Extract translation and rotation from ECEFENU0 @@ -672,21 +702,22 @@ void FixpositionDriverNode::PublishNav2Tf() { // Create a new TransformStamped message geometry_msgs::msg::TransformStamped tfs_odom; - tfs_odom.header.stamp = nh_->now(); + tfs_odom.header.stamp = tfs_.enu0_poi_->header.stamp; tfs_odom.header.frame_id = "map"; tfs_odom.child_frame_id = "odom"; tfs_odom.transform = tf2::toMsg(tf_combined); tf_br_->sendTransform(tfs_odom); - // Publish odom -> base_link + // Publish odom -> vrtk_link geometry_msgs::msg::TransformStamped tf_odom_base; - tf_odom_base.header.stamp = nh_->now(); + tf_odom_base.header.stamp = tfs_.enu0_poi_->header.stamp; tf_odom_base.header.frame_id = "odom"; - tf_odom_base.child_frame_id = "base_link"; + tf_odom_base.child_frame_id = "vrtk_link"; tf_odom_base.transform = tf2::toMsg(tf_ENU0POISH); - - // Send the transform tf_br_->sendTransform(tf_odom_base); + + // Publish WGS84 datum + PublishDatum(trans_ecef_enu0, tfs_.enu0_poi_->header.stamp, datum_pub_); } /* ****************************************************************************************************************** */ From 7290c3c5e83d5e7d458561ea20e2ba6b7467ce0a Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 7 Mar 2025 17:06:22 +0000 Subject: [PATCH 4/8] Precommit changes --- .../include/fixposition_driver_ros1/data_to_ros1.hpp | 5 +++-- fixposition_driver_ros1/src/data_to_ros1.cpp | 2 +- .../include/fixposition_driver_ros2/data_to_ros2.hpp | 6 +++--- fixposition_driver_ros2/src/data_to_ros2.cpp | 6 +++--- fixposition_driver_ros2/src/fixposition_driver_node.cpp | 2 +- 5 files changed, 11 insertions(+), 10 deletions(-) diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp index fcf4620..5d40ae7 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp @@ -35,8 +35,9 @@ void TfDataToTransformStamped(const TfData& data, geometry_msgs::TransformStampe void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::TransformStamped& msg); void PublishFpaOdometry(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, ros::Publisher& pub); -void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, ros::Publisher& pub); -void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, +void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, + ros::Publisher& pub); +void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, ros::Publisher& pub); void PublishFpaOdomenu(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload, ros::Publisher& pub); void PublishFpaOdomenuVector3Stamped(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload, ros::Publisher& pub); diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 8820d9e..07a7e86 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -194,7 +194,7 @@ void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, boo msg.status.status = FpaGnssFixToNavSatStatusStatus(msg.status, fix); if (msg.status.status != msg.status.STATUS_NO_FIX) { msg.status.service = (msg.status.SERVICE_GPS | msg.status.SERVICE_GLONASS | msg.status.SERVICE_COMPASS | - msg.status.SERVICE_GALILEO); + msg.status.SERVICE_GALILEO); } } diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp index 46c8176..d57a0d0 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp @@ -36,9 +36,9 @@ void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::msg void PublishFpaOdometry(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, rclcpp::Publisher::SharedPtr& pub); -void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, +void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, rclcpp::Publisher::SharedPtr& pub); -void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, +void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, bool nav2_mode_, rclcpp::Publisher::SharedPtr& pub); void PublishFpaOdomenu(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload, rclcpp::Publisher::SharedPtr& pub); @@ -100,7 +100,7 @@ void PublishParserMsg(const fpsdk::common::parser::ParserMsg& msg, void PublishNmeaEpochData(const NmeaEpochData& data, rclcpp::Publisher::SharedPtr& pub); void PublishOdometryData(const OdometryData& data, rclcpp::Publisher::SharedPtr& pub); void PublishJumpWarning(const JumpDetector& jump_detector, rclcpp::Publisher::SharedPtr& pub); -void PublishDatum(const geometry_msgs::msg::Vector3& payload, const builtin_interfaces::msg::Time& stamp, +void PublishDatum(const geometry_msgs::msg::Vector3& payload, const builtin_interfaces::msg::Time& stamp, rclcpp::Publisher::SharedPtr& pub); void PublishFusionEpochData(const FusionEpochData& data, rclcpp::Publisher::SharedPtr& pub); diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index d271614..ce7297f 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -135,7 +135,7 @@ void PublishFpaOdomsh(const fpa::FpaOdomshPayload& payload, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { sensor_msgs::msg::Imu msg; @@ -153,7 +153,7 @@ void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, bool nav2 // --------------------------------------------------------------------------------------------------------------------- -void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, bool nav2_mode_, +void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, bool nav2_mode_, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { sensor_msgs::msg::NavSatFix msg; @@ -834,7 +834,7 @@ void PublishJumpWarning(const JumpDetector& jump_detector, rclcpp::Publisher::SharedPtr& pub) { if (pub->get_subscription_count() > 0) { sensor_msgs::msg::NavSatFix msg; diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 9e4a432..54df510 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -157,7 +157,7 @@ bool FixpositionDriverNode::StartNode() { PublishFpaOdomenuVector3Stamped(odomenu_payload, eul_pub_); OdometryData odometry_data; odometry_data.SetFromFpaOdomPayload(odomenu_payload); - + // Update frames for Nav2 if (params_.nav2_mode_) { odometry_data.frame_id = "map"; From e5dd646752dc3548e46661ccbf82a78a1377c650 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 7 Mar 2025 17:07:21 +0000 Subject: [PATCH 5/8] Minor bug fix --- fixposition_driver_ros1/src/fixposition_driver_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 8e95472..d65e548 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -619,7 +619,7 @@ void FixpositionDriverNode::PublishNav2Tf() { } // Publish a static identity transform from FP_ENU0 to map - geometry_msgs::msg::TransformStamped static_transform; + geometry_msgs::TransformStamped static_transform; static_transform.header.stamp = tfs_.ecef_enu0_->header.stamp; static_transform.header.frame_id = "FP_ENU0"; static_transform.child_frame_id = "map"; From 0360f18ebda2cfd35ec40b83effd5cfe9f9b5ae4 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 7 Mar 2025 17:37:16 +0000 Subject: [PATCH 6/8] Increased delay warning --- fixposition_driver_ros1/launch/config.yaml | 2 +- fixposition_driver_ros2/launch/config.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/fixposition_driver_ros1/launch/config.yaml b/fixposition_driver_ros1/launch/config.yaml index 544e29a..fb65e61 100644 --- a/fixposition_driver_ros1/launch/config.yaml +++ b/fixposition_driver_ros1/launch/config.yaml @@ -6,7 +6,7 @@ stream: tcpcli://10.0.2.1:21000 # Wait time in [s] to attempt reconnecting after connection lost reconnect_delay: 5.0 # Delay warning threshold [s]. Note that this only works if your system time is synced to the VRTK2. -delay_warning: 0.01 +delay_warning: 0.03 # Messages that should be used by the driver. Note that the sensor must be configured accordingly for the correct # port used in the "connection" above. diff --git a/fixposition_driver_ros2/launch/config.yaml b/fixposition_driver_ros2/launch/config.yaml index 0109eb8..6b95110 100644 --- a/fixposition_driver_ros2/launch/config.yaml +++ b/fixposition_driver_ros2/launch/config.yaml @@ -8,7 +8,7 @@ # Wait time in [s] to attempt reconnecting after connection lost reconnect_delay: 5.0 # Delay warning threshold [s]. Note that this only works if your system time is synced to the VRTK2. This must be a float. - delay_warning: 0.01 + delay_warning: 0.03 # Messages that should be used by the driver. Note that the sensor must be configured accordingly for the correct # port used in the "connection" above. From 53fbda5b530e7656fa551011d01bfe18ebaeffe4 Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 7 Mar 2025 17:57:29 +0000 Subject: [PATCH 7/8] Added TF warning --- fixposition_driver_ros1/src/fixposition_driver_node.cpp | 8 ++++++++ fixposition_driver_ros2/src/fixposition_driver_node.cpp | 9 +++++++++ 2 files changed, 17 insertions(+) diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index d65e548..4a9edd5 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -512,6 +512,14 @@ void FixpositionDriverNode::StopNode() { // --------------------------------------------------------------------------------------------------------------------- void FixpositionDriverNode::ProcessTfData(const TfData& tf_data) { + // Check if TF is valid + if (tf_data.rotation.w() == 0 && tf_data.rotation.vec().isZero()) { + ROS_WARN_THROTTLE(10.0, "Invalid TF was found! Is the fusion engine initialized? Source: %s, target: %s", + tf_data.frame_id.c_str(), tf_data.child_frame_id.c_str()); + return; + } + + // Generate TF message geometry_msgs::TransformStamped tf; TfDataToTransformStamped(tf_data, tf); diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 54df510..e7d64c8 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -545,6 +545,15 @@ void FixpositionDriverNode::StopNode() { // --------------------------------------------------------------------------------------------------------------------- void FixpositionDriverNode::ProcessTfData(const TfData& tf_data) { + // Check if TF is valid + if (tf_data.rotation.w() == 0 && tf_data.rotation.vec().isZero()) { + RCLCPP_WARN_THROTTLE(logger_, *nh_->get_clock(), 1e4, + "Invalid TF was found! Is the fusion engine initialized? Source: %s, target: %s", + tf_data.frame_id.c_str(), tf_data.child_frame_id.c_str()); + return; + } + + // Generate TF message geometry_msgs::msg::TransformStamped tf; TfDataToTransformStamped(tf_data, tf); From 97b46bdc7772dac5d21f846e122a08fd884b81cc Mon Sep 17 00:00:00 2001 From: Facundo Garcia Date: Fri, 7 Mar 2025 18:06:02 +0000 Subject: [PATCH 8/8] Precommit changes --- fixposition_driver_ros1/src/fixposition_driver_node.cpp | 5 ++--- fixposition_driver_ros2/src/fixposition_driver_node.cpp | 7 +++---- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 4a9edd5..626a784 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -514,11 +514,11 @@ void FixpositionDriverNode::StopNode() { void FixpositionDriverNode::ProcessTfData(const TfData& tf_data) { // Check if TF is valid if (tf_data.rotation.w() == 0 && tf_data.rotation.vec().isZero()) { - ROS_WARN_THROTTLE(10.0, "Invalid TF was found! Is the fusion engine initialized? Source: %s, target: %s", + ROS_WARN_THROTTLE(10.0, "Invalid TF was found! Is the fusion engine initialized? Source: %s, target: %s", tf_data.frame_id.c_str(), tf_data.child_frame_id.c_str()); return; } - + // Generate TF message geometry_msgs::TransformStamped tf; TfDataToTransformStamped(tf_data, tf); @@ -536,7 +536,6 @@ void FixpositionDriverNode::ProcessTfData(const TfData& tf_data) { imu_ypr.header.frame_id = "FP_POI"; tf::vectorEigenToMsg(imu_ypr_eigen, imu_ypr.vector); eul_imu_pub_.publish(imu_ypr); - } // FP_POI -> FP_POISH else if ((tf.child_frame_id == "FP_POISH") && (tf.header.frame_id == "FP_POI")) { diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index e7d64c8..3e127dc 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -547,12 +547,12 @@ void FixpositionDriverNode::StopNode() { void FixpositionDriverNode::ProcessTfData(const TfData& tf_data) { // Check if TF is valid if (tf_data.rotation.w() == 0 && tf_data.rotation.vec().isZero()) { - RCLCPP_WARN_THROTTLE(logger_, *nh_->get_clock(), 1e4, - "Invalid TF was found! Is the fusion engine initialized? Source: %s, target: %s", + RCLCPP_WARN_THROTTLE(logger_, *nh_->get_clock(), 1e4, + "Invalid TF was found! Is the fusion engine initialized? Source: %s, target: %s", tf_data.frame_id.c_str(), tf_data.child_frame_id.c_str()); return; } - + // Generate TF message geometry_msgs::msg::TransformStamped tf; TfDataToTransformStamped(tf_data, tf); @@ -573,7 +573,6 @@ void FixpositionDriverNode::ProcessTfData(const TfData& tf_data) { imu_ypr.vector.set__y(imu_ypr_eigen.y()); imu_ypr.vector.set__z(imu_ypr_eigen.z()); eul_imu_pub_->publish(imu_ypr); - } // FP_POI -> FP_POISH else if ((tf.child_frame_id == "FP_POISH") && (tf.header.frame_id == "FP_POI")) {