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/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp index e47f9e0..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, 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 +74,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 6bd03d4..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 @@ -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 @@ -106,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 @@ -132,6 +134,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/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_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index 887de05..07a7e86 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 2f8f94f..626a784 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_); @@ -93,14 +93,30 @@ 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_); 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); + + // 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_); + } + } }); } @@ -115,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); @@ -368,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) { @@ -457,6 +485,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(); @@ -472,6 +501,7 @@ void FixpositionDriverNode::StopNode() { // - Other jump_pub_.shutdown(); raw_pub_.shutdown(); + datum_pub_.shutdown(); // Stop input message subscribers ws_sub_.shutdown(); @@ -482,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); @@ -498,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")) { @@ -512,6 +549,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_); @@ -587,9 +625,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::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 @@ -624,21 +672,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..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, +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 672bb3f..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 @@ -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 @@ -112,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 @@ -143,6 +145,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/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. diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index fce0ac3..ce7297f 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 7a9b3b3..3e127dc 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_); @@ -119,14 +119,30 @@ 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_); 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); + + // 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_); + } + } }); } @@ -141,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); @@ -396,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) { @@ -484,6 +512,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(); @@ -499,6 +528,7 @@ void FixpositionDriverNode::StopNode() { // - Other jump_pub_.reset(); raw_pub_.reset(); + datum_pub_.reset(); // Stop input message subscribers ws_sub_.reset(); @@ -515,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); @@ -534,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")) { @@ -548,6 +586,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_); @@ -624,9 +663,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 @@ -661,21 +710,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_); } /* ****************************************************************************************************************** */