diff --git a/include/robot_localization/navsat_transform.hpp b/include/robot_localization/navsat_transform.hpp index 69bcdd75..46f9ad87 100644 --- a/include/robot_localization/navsat_transform.hpp +++ b/include/robot_localization/navsat_transform.hpp @@ -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 */ diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp index aa70be86..a1553b5c 100644 --- a/src/navsat_transform.cpp +++ b/src/navsat_transform.cpp @@ -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), @@ -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(); - 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(); - datumCallback(request, response); + auto request = std::make_shared(); + 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(); + 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)); @@ -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(); } @@ -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( @@ -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; @@ -423,6 +430,10 @@ bool NavSatTransform::toLLCallback( const std::shared_ptr request, std::shared_ptr 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; } @@ -439,6 +450,10 @@ bool NavSatTransform::fromLLCallback( const std::shared_ptr request, std::shared_ptr 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) { @@ -452,6 +467,10 @@ bool NavSatTransform::fromLLArrayCallback( const std::shared_ptr request, std::shared_ptr 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()); @@ -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; } } @@ -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::quiet_NaN(); } } @@ -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()) { @@ -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; } } @@ -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(); @@ -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 *