Skip to content

Don't process GNSS messages before SetDatum has been called #929

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: jazzy-devel
Choose a base branch
from
Open

Don't process GNSS messages before SetDatum has been called #929

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
319bbdc
Wait for setDatum call before processing incoming messages
karl-schulz May 26, 2025
bfe23c4
Catch and fix race condition with fix arriving before TF is computed
karl-schulz May 27, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions include/robot_localization/navsat_transform.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -421,6 +421,11 @@ class NavSatTransform : public rclcpp::Node {
*/
bool use_manual_datum_;

/**
* @brief Whether we got a datum from the datum service or parameters
*/
bool datum_is_set_;

/**
* @brief Whether we get the transform's yaw from the odometry or IMU source
*/
Expand Down
69 changes: 53 additions & 16 deletions src/navsat_transform.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,7 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options)
use_local_cartesian_(false),
force_user_utm_(false),
use_manual_datum_(false),
datum_is_set_(false),
use_odometry_yaw_(false),
cartesian_broadcaster_(*this),
utm_meridian_convergence_(0.0),
Expand Down Expand Up @@ -168,17 +169,20 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options)
datum_lat = datum_vals[0];
datum_lon = datum_vals[1];
datum_yaw = datum_vals[2];
}

auto request = std::make_shared<robot_localization::srv::SetDatum::Request>();
request->geo_pose.position.latitude = datum_lat;
request->geo_pose.position.longitude = datum_lon;
request->geo_pose.position.altitude = 0.0;
tf2::Quaternion quat;
quat.setRPY(0.0, 0.0, datum_yaw);
request->geo_pose.orientation = tf2::toMsg(quat);
auto response = std::make_shared<robot_localization::srv::SetDatum::Response>();
datumCallback(request, response);
auto request = std::make_shared<robot_localization::srv::SetDatum::Request>();
request->geo_pose.position.latitude = datum_lat;
request->geo_pose.position.longitude = datum_lon;
request->geo_pose.position.altitude = 0.0;
tf2::Quaternion quat;
quat.setRPY(0.0, 0.0, datum_yaw);
request->geo_pose.orientation = tf2::toMsg(quat);
auto response = std::make_shared<robot_localization::srv::SetDatum::Response>();
datumCallback(request, response);
} else {
RCLCPP_INFO(
this->get_logger(), "No datum parameter given, waiting to SetDatum call.");
}
}

auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(1));
Expand Down Expand Up @@ -253,7 +257,7 @@ void NavSatTransform::computeTransform()
// When using manual datum, wait for the receive of odometry message so
// that the base frame and world frame names can be set before
// the manual datum pose is set. This must be done prior to the transform computation.
if (!transform_good_ && has_transform_odom_ && use_manual_datum_) {
if (!transform_good_ && has_transform_odom_ && use_manual_datum_ && datum_is_set_) {
setManualDatum();
}

Expand All @@ -265,6 +269,7 @@ void NavSatTransform::computeTransform()
{
// The UTM pose we have is given at the location of the GPS sensor on the
// robot. We need to get the UTM pose of the robot's origin.
RCLCPP_INFO(this->get_logger(), "Computing utm->odom TF.");
tf2::Transform transform_cartesian_pose_corrected;
if (!use_manual_datum_) {
getRobotOriginCartesianPose(
Expand Down Expand Up @@ -377,13 +382,15 @@ bool NavSatTransform::datumCallback(
// we are using a datum from now on, and we want other methods to not attempt
// to transform the values we are specifying here.
use_manual_datum_ = true;

datum_is_set_ = true;
transform_good_ = false;
RCLCPP_INFO(this->get_logger(), "Datum set.");
return true;
}

void NavSatTransform::setManualDatum()
{
RCLCPP_INFO(this->get_logger(), "Setting map origin fix from manual datum value.");
sensor_msgs::msg::NavSatFix fix;
fix.latitude = manual_datum_geopose_.position.latitude;
fix.longitude = manual_datum_geopose_.position.longitude;
Expand Down Expand Up @@ -423,6 +430,10 @@ bool NavSatTransform::toLLCallback(
const std::shared_ptr<robot_localization::srv::ToLL::Request> request,
std::shared_ptr<robot_localization::srv::ToLL::Response> response)
{
if (use_manual_datum_ && !datum_is_set_){
// We have to wait for a call to SetDatum before computing anything.
return false;
}
if (!transform_good_) {
return false;
}
Expand All @@ -439,6 +450,10 @@ bool NavSatTransform::fromLLCallback(
const std::shared_ptr<robot_localization::srv::FromLL::Request> request,
std::shared_ptr<robot_localization::srv::FromLL::Response> response)
{
if (use_manual_datum_ && !datum_is_set_){
// We have to wait for a call to SetDatum before computing anything.
return false;
}
try {
response->map_point = fromLL(request->ll_point);
} catch(const std::runtime_error & e) {
Expand All @@ -452,6 +467,10 @@ bool NavSatTransform::fromLLArrayCallback(
const std::shared_ptr<robot_localization::srv::FromLLArray::Request> request,
std::shared_ptr<robot_localization::srv::FromLLArray::Response> response)
{
if (use_manual_datum_ && !datum_is_set_){
// We have to wait for a call to SetDatum before computing anything.
return false;
}
decltype(response->map_points) converted_points;
converted_points.reserve(request->ll_points.size());

Expand Down Expand Up @@ -498,7 +517,7 @@ geometry_msgs::msg::Point NavSatTransform::fromLL(
latitude, longitude,
zone_tmp, northp_tmp, cartesian_x, cartesian_y, utm_zone_);
} catch (GeographicLib::GeographicErr const & e) {
RCLCPP_ERROR_STREAM(this->get_logger(), e.what());
RCLCPP_ERROR_STREAM(this->get_logger(), "Error in fromLL: " << e.what());
throw;
}
}
Expand Down Expand Up @@ -592,7 +611,7 @@ void NavSatTransform::mapToLL(
longitude);
altitude = odom_as_cartesian.getOrigin().getZ();
} catch (const GeographicLib::GeographicErr & e) {
RCLCPP_ERROR_STREAM(this->get_logger(), e.what());
RCLCPP_ERROR_STREAM(this->get_logger(), "Error in mapToLL: " << e.what());
latitude = longitude = altitude = std::numeric_limits<double>::quiet_NaN();
}
}
Expand Down Expand Up @@ -692,6 +711,20 @@ void NavSatTransform::getRobotOriginWorldPose(
void NavSatTransform::gpsFixCallback(
const sensor_msgs::msg::NavSatFix::SharedPtr msg)
{
if (use_manual_datum_ && !datum_is_set_){
// We have to wait for a call to SetDatum before computing anything.
return;
}
if (use_manual_datum_ && datum_is_set_ && !transform_good_) {
// This race condition that can happen when the TF is not prepared before the first fix message arrives
RCLCPP_ERROR_THROTTLE(
this->get_logger(),
*this->get_clock(), 5000,
"Cannot compute fix to odom because TF is missing: has_transform_gps: %s, "
"has_transform_odom: %s, has_transform_imu: %s",
has_transform_gps_ ? "true" : "false", has_transform_odom_ ? "true" : "false", has_transform_imu_ ? "true" : "false");
return;
}
gps_frame_id_ = msg->header.frame_id;

if (gps_frame_id_.empty()) {
Expand Down Expand Up @@ -730,7 +763,7 @@ void NavSatTransform::gpsFixCallback(
msg->latitude, msg->longitude, zone_tmp, northp_tmp,
cartesian_x, cartesian_y, utm_zone_);
} catch (GeographicLib::GeographicErr const & e) {
RCLCPP_ERROR_STREAM(this->get_logger(), e.what());
RCLCPP_ERROR_STREAM(this->get_logger(), "Error converting fix message to odometry: " << e.what());
return;
}
}
Expand Down Expand Up @@ -811,6 +844,10 @@ void NavSatTransform::odomCallback(
if (!transform_good_) {
setTransformOdometry(msg);
}
if (use_manual_datum_ && !datum_is_set_) {
// We have to wait for a call to SetDatum before computing anything.
return;
}

tf2::fromMsg(msg->pose.pose, latest_world_pose_);
latest_odom_covariance_.setZero();
Expand Down Expand Up @@ -961,7 +998,7 @@ void NavSatTransform::setTransformGps(
msg->latitude, msg->longitude, utm_zone_, northp_,
cartesian_x, cartesian_y, utm_meridian_convergence_degrees, k_tmp, set_zone);
} catch (const GeographicLib::GeographicErr & e) {
RCLCPP_ERROR_STREAM(this->get_logger(), e.what());
RCLCPP_ERROR_STREAM(this->get_logger(), "Error computing utm->map TF: " << e.what());
return;
}
utm_meridian_convergence_ = utm_meridian_convergence_degrees *
Expand Down