Wait for odometry message before setting manual datum so that the bas…

…e and world frame names can be set. (#836)

* wait for odom msg before setting manual datum

include/robot_localization/navsat_transform.hpp

@@ -177,6 +177,11 @@ class NavSatTransform : public rclcpp::Node
     const tf2::Vector3 & point, double & latitude, double & longitude,
     double & altitude) const;
 
+  /**
+   * @brief Sets the manual datum pose to be used by the transform computation
+   */
+  void setManualDatum();
+
   /**
    * @brief Frame ID of the robot's body frame
    *
@@ -433,6 +438,15 @@ class NavSatTransform : public rclcpp::Node
    * converted GPS odometry message.
    */
   bool zero_altitude_;
+
+  /**
+   * @brief Manual datum pose to be used by the transform computation
+   *
+   * Then manual datum requested by a service request (or configuration) is stored
+   * here until the odom message is received, and the manual datum pose can be
+   * set.
+   */
+  geographic_msgs::msg::GeoPose manual_datum_geopose_;  
 };
 
 }  // namespace robot_localization

src/navsat_transform.cpp

@@ -243,6 +243,13 @@ void NavSatTransform::transformCallback()
 
 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_) {
+    setManualDatum();
+  }
+
   // Only do this if:
   // 1. We haven't computed the odom_frame->cartesian_frame transform before
   // 2. We've received the data we need
@@ -355,18 +362,25 @@ bool NavSatTransform::datumCallback(
   robot_localization::srv::SetDatum::Request::SharedPtr request,
   robot_localization::srv::SetDatum::Response::SharedPtr)
 {
+  // store manual data geopose until the transform can be computed.
+  manual_datum_geopose_ = request->geo_pose;
+
   // If we get a service call with a manual datum, even if we already computed
   // the transform using the robot's initial pose, then we want to assume that
   // 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;
 
   transform_good_ = false;
+  return true;
+}
 
+void NavSatTransform::setManualDatum()
+{
   sensor_msgs::msg::NavSatFix fix;
-  fix.latitude = request->geo_pose.position.latitude;
-  fix.longitude = request->geo_pose.position.longitude;
-  fix.altitude = request->geo_pose.position.altitude;
+  fix.latitude = manual_datum_geopose_.position.latitude;
+  fix.longitude = manual_datum_geopose_.position.longitude;
+  fix.altitude = manual_datum_geopose_.position.altitude;
   fix.header.stamp = this->now();
   fix.position_covariance[0] = 0.1;
   fix.position_covariance[4] = 0.1;
@@ -391,13 +405,11 @@ bool NavSatTransform::datumCallback(
   setTransformOdometry(odom_ptr);
 
   sensor_msgs::msg::Imu imu;
-  imu.orientation = request->geo_pose.orientation;
+  imu.orientation = manual_datum_geopose_.orientation;
   imu.header.frame_id = base_link_frame_id_;
   sensor_msgs::msg::Imu::SharedPtr imu_ptr =
     std::make_shared<sensor_msgs::msg::Imu>(imu);
   imuCallback(imu_ptr);
-
-  return true;
 }
 
 bool NavSatTransform::toLLCallback(
@@ -705,7 +717,7 @@ void NavSatTransform::odomCallback(
   world_frame_id_ = msg->header.frame_id;
   base_link_frame_id_ = msg->child_frame_id;
 
-  if (!transform_good_ && !use_manual_datum_) {
+  if (!transform_good_) {
     setTransformOdometry(msg);
   }