From fdd2df3a586839e29d6c53f8887db24771f8ac2a Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 10 Jul 2026 08:51:48 +0200 Subject: [PATCH] fix: GNSS ROS publisher never set NavSatFix.status.status Found via end-to-end MOLA + MVSIM simulation testing: the simulated GNSS observation was converted to sensor_msgs::NavSatFix by hand, field by field (latitude/longitude/altitude/covariance only), and never touched msg.status.status. ROS leaves that field at NavSatStatus::STATUS_UNKNOWN (-2) by IDL default -- not STATUS_FIX (0) as a naive zero-init might suggest. Consumers that treat anything other than a definite fix as invalid (e.g. mola_state_estimation_smoother's GNSS fusion, which checks CObservationGPS's GGA fix_quality field, itself derived from status.status by the MRPT ROS bridge) then silently reject *every* GPS reading -- even though the simulated fix quality (mrpt::obs::gnss::Message_NMEA_GGA::fields::fix_quality, set in Sensors/GNSS.cpp) was perfectly valid the whole time. Fixed by delegating to the existing, already-correct mrpt2ros::toROS(CObservationGPS, Header, NavSatFix&) bridge function, matching the pattern the adjacent IMU handler already used, instead of reimplementing the conversion. The GPS bridge header ( / ) wasn't even included before. --- mvsim_node_src/mvsim_node.cpp | 40 ++++++++++++++--------------------- 1 file changed, 16 insertions(+), 24 deletions(-) diff --git a/mvsim_node_src/mvsim_node.cpp b/mvsim_node_src/mvsim_node.cpp index 82272f46..b5d30ce8 100644 --- a/mvsim_node_src/mvsim_node.cpp +++ b/mvsim_node_src/mvsim_node.cpp @@ -28,6 +28,7 @@ // =========================================== // ROS 1 // =========================================== +#include #include #include #include @@ -60,6 +61,7 @@ using Msg_Marker = visualization_msgs::Marker; // =========================================== // ROS 2 // =========================================== +#include #include #include #include @@ -1291,31 +1293,21 @@ void MVSimNode::internalOn(const mvsim::VehicleBase& veh, const mrpt::obs::CObse // Send observation: { - // Convert observation MRPT -> ROS - auto msg = mvsim_node::make_shared(); - msg->header.stamp = myNow(); - msg->header.frame_id = obs.sensorLabel; - - const auto& o = obs.getMsgByClass(); - - msg->latitude = o.fields.latitude_degrees; - msg->longitude = o.fields.longitude_degrees; - msg->altitude = o.fields.altitude_meters; - - if (auto& c = obs.covariance_enu; c.has_value()) - { -#if PACKAGE_ROS_VERSION == 1 - msg->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; -#else - msg->position_covariance_type = - sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; -#endif + // Convert observation MRPT -> ROS. Delegate to the library conversion + // (as the IMU handler above already does) instead of reimplementing it + // field-by-field: the hand-rolled version here never set + // msg->status.status, which ROS leaves at NavSatStatus::STATUS_UNKNOWN + // (-2) by IDL default; consumers that treat anything other than a + // definite fix as invalid (e.g. mola_state_estimation_smoother's GNSS + // fusion) would then silently reject every single reading, even + // though the simulated fix quality (mrpt::obs::gnss::Message_NMEA_GGA + // ::fields::fix_quality) was perfectly valid. + Msg_Header msg_header; + msg_header.stamp = myNow(); + msg_header.frame_id = obs.sensorLabel; - msg->position_covariance.fill(0.0); - msg->position_covariance[0] = (*c)(0, 0); - msg->position_covariance[4] = (*c)(1, 1); - msg->position_covariance[8] = (*c)(2, 2); - } + auto msg = mvsim_node::make_shared(); + mrpt2ros::toROS(obs, msg_header, *msg); pub->publish(msg); }