Adding publishing of an REP-105 compliant ECEF transform

CMakeLists.txt

@@ -268,6 +268,10 @@ if(BUILD_TESTING)
   target_include_directories(test_navsat_conversions PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>")
   target_link_libraries(test_navsat_conversions ${library_name} ${GeographicLib_LIBRARIES})
 
+  #### EARTH CHAIN TESTS ####
+  ament_add_gtest(test_earth_chain test/test_earth_chain.cpp)
+  target_link_libraries(test_earth_chain ${library_name})
+
   ament_add_gtest_executable(test_ros_robot_localization_listener
                   test/test_ros_robot_localization_listener.cpp)
   target_link_libraries(test_ros_robot_localization_listener ${library_name})
@@ -303,6 +307,7 @@ if(BUILD_TESTING)
     #test_ekf_localization_node_bag3
     test_robot_localization_estimator
     test_navsat_conversions
+    test_earth_chain
     test_ros_robot_localization_listener
     test_ros_robot_localization_listener_publisher
     #test_ukf_localization_node_bag1

doc/integrating_gps.rst

@@ -158,6 +158,6 @@ We now need to convert our latitude and longitude to UTM coordinates. The UTM gr
 
  :math:`\theta = yaw_{imu} + \omega + offset_{yaw}`
 
-We now have a translation :math:`(x_{UTM}, y_{UTM})` and rotation :math:`\theta`, which we can use to create the required *utm* -> *map* transform. We use the transform to convert all future GPS  positions into the robot's local coordinate frame. ``navsat_transform_node`` will also broadcast this transform if the ``broadcast_utm_transform`` parameter is set to *true*.
+We now have a translation :math:`(x_{UTM}, y_{UTM})` and rotation :math:`\theta`, which we can use to create the required *utm* -> *map* transform. We use the transform to convert all future GPS  positions into the robot's local coordinate frame. ``navsat_transform_node`` will also broadcast this transform if the ``broadcast_cartesian_transform`` parameter is set to *true*.
 
 If you have any questions about this tutorial, please feel free to ask questions on `answers.ros.org <http://answers.ros.org>`_.

doc/navsat_transform_node.rst

@@ -28,13 +28,19 @@ Your IMU should read 0 for yaw when facing east. If it doesn't, enter the offset
 ^^^^^^^^^^^^^^
 If this is *true*, the `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message produced by this node has its pose Z value set to 0.
 
+~use_local_cartesian
+^^^^^^^^^^^^^^^^^^^^
+If this is *true*, the node uses a local East-North-Up (ENU) cartesian coordinate system centered at the GPS datum. If *false*, the node uses UTM coordinates. Defaults to *false* (UTM mode). Local cartesian mode is required for earth frame transform support.
+
 ~publish_filtered_gps
 ^^^^^^^^^^^^^^^^^^^^^
 If *true*, ``navsat_transform_node`` will also transform your robot's world frame (e.g., *map*) position back to GPS coordinates, and publish a `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message on the ``/gps/filtered`` topic.
 
-~broadcast_utm_transform
-^^^^^^^^^^^^^^^^^^^^^^^^
-If this is *true*, ``navsat_transform_node`` will broadcast the transform between the UTM grid and the frame of the input odometry data. See Published Transforms below for more information.
+~broadcast_cartesian_transform
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+If this is *true*, ``navsat_transform_node`` will broadcast the transform between the cartesian frame (UTM or local_enu) and the world frame. See Published Transforms below for more information.
+
+.. note:: The legacy parameter name ``broadcast_utm_transform`` is deprecated but still supported for backward compatibility.
 
 ~use_odometry_yaw
 ^^^^^^^^^^^^^^^^^
@@ -47,15 +53,27 @@ If *true*, ``navsat_transform_node`` will wait to get a datum from either:
 * The ``datum`` parameter
 * The ``set_datum`` service
 
-~broadcast_utm_transform_as_parent_frame
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-If *true*, ``navsat_transform_node`` will publish the utm->world_frame transform instead of the world_frame->utm transform. 
-Note that for the transform to be published ``broadcast_utm_transform`` also has to be set to *true*.
+~broadcast_cartesian_transform_as_parent_frame
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+If *true*, ``navsat_transform_node`` will publish the cartesian->world_frame transform instead of the world_frame->cartesian transform.
+Note that for the transform to be published ``broadcast_cartesian_transform`` also has to be set to *true*.
+
+.. note:: The legacy parameter name ``broadcast_utm_transform_as_parent_frame`` is deprecated but still supported for backward compatibility.
 
 ~transform_timeout
 ^^^^^^^^^^^^^^^^^^
 This parameter specifies how long we would like to wait if a transformation is not available yet. Defaults to 0 if not set. The value 0 means we just get us the latest available (see ``tf2`` implementation) transform.
 
+~earth_frame_id
+^^^^^^^^^^^^^^^
+The name of the ECEF earth frame for REP-105 compliant coordinate systems. Defaults to ``earth``. This frame represents the WGS-84 Earth-Centered Earth-Fixed coordinate system using ellipsoidal heights only (no geoid corrections).
+
+~broadcast_earth_transform
+^^^^^^^^^^^^^^^^^^^^^^^^^^
+If *true*, ``navsat_transform_node`` will broadcast transforms that link the earth frame to the existing cartesian/world frames, enabling REP-105 compliant ECEF coordinate system support. The specific transform chain published depends on the ``broadcast_cartesian_transform`` and ``broadcast_cartesian_transform_as_parent_frame`` settings. Defaults to *false*.
+
+.. note:: This parameter is only valid when ``use_local_cartesian`` is *true*. Earth frame transforms are not supported with UTM coordinates. If you attempt to set both ``broadcast_earth_transform: true`` and ``use_local_cartesian: false``, the node will log an error and disable the earth transform functionality.
+
 Subscribed Topics
 =================
 * ``imu/data`` A `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ message with orientation data
@@ -72,4 +90,12 @@ Published Topics
 
 Published Transforms
 ====================
-* ``world_frame->utm`` (optional) - If the ``broadcast_utm_transform`` parameter is set to  *true*, ``navsat_transform_node`` calculates a transform from the  *utm* frame to the ``frame_id`` of the input odometry data. By default, the *utm* frame is published as a child of the odometry frame by using the inverse transform. With use of the ``broadcast_utm_transform_as_parent_frame`` parameter, the *utm* frame will be published as a parent of the odometry frame. This is useful if you have multiple robots within one TF tree.
+* ``world_frame->cartesian`` (optional) - If the ``broadcast_cartesian_transform`` parameter is set to  *true*, ``navsat_transform_node`` calculates a transform from the cartesian frame (UTM or local_enu) to the ``frame_id`` of the input odometry data. By default, the cartesian frame is published as a child of the world frame by using the inverse transform. With use of the ``broadcast_cartesian_transform_as_parent_frame`` parameter, the cartesian frame will be published as a parent of the world frame. This is useful if you have multiple robots within one TF tree.
+
+* ``earth->world_frame/local_enu`` (optional) - If the ``broadcast_earth_transform`` parameter is set to *true*, and ``use_local_cartesian`` is set to *true*, ``navsat_transform_node`` publishes transforms that link the REP-105 compliant earth frame (ECEF WGS-84) to the existing coordinate frames. The child frame can be either the world_frame of local_enu dependent on other configurations. If ``broadcast_cartesian_transform`` is *false* or ``broadcast_cartesian_transform_as_parent_frame`` is *false*, then the child frame is set as the world_frame. Otherwise, the child frame is set to local_enu.
+
+
+Heights
+=======
+
+All ECEF conversions use WGS-84 **ellipsoidal** heights. No geoid corrections are applied.

include/robot_localization/navsat_transform.hpp

@@ -211,6 +211,13 @@ class NavSatTransform : public rclcpp::Node {
    */
   void setManualDatum();
 
+  /**
+   * @brief Computes earth to cartesian transform
+   * @note Only valid when use_local_cartesian_ is true
+   */
+  void computeEarthToCartesian(double lat0_deg, double lon0_deg, double h0_m);
+
+
   /**
    * @brief Frame ID of the robot's body frame
    *
@@ -439,9 +446,9 @@ class NavSatTransform : public rclcpp::Node {
   bool use_odometry_yaw_;
 
   /**
-   * @brief Used for publishing the static world_frame->cartesian transform
+   * @brief Used for publishing static transforms
    */
-  tf2_ros::StaticTransformBroadcaster cartesian_broadcaster_;
+  tf2_ros::StaticTransformBroadcaster tf_broadcaster_;
 
   /**
    * @brief UTM's meridian convergence
@@ -505,6 +512,32 @@ class NavSatTransform : public rclcpp::Node {
    * can be set.
    */
   geographic_msgs::msg::GeoPose manual_datum_geopose_;
+
+  /**
+   * @brief Earth frame ID for ECEF coordinates
+   */
+  std::string earth_frame_id_;
+
+  /**
+   * @brief Whether or not we broadcast the earth transform
+   * @note Only valid when use_local_cartesian_ is true.
+   *       Earth transforms are not supported with UTM coordinates.
+   */
+  bool broadcast_earth_transform_;
+
+
+  /**
+   * @brief Transform from earth to cartesian (ECEF to local/UTM)
+   * Note: This is the mathematical transform E2C, inverted when published to TF
+   */
+  tf2::Transform earth_cartesian_transform_;
+
+  /**
+   * @brief Origin GPS coordinates for earth frame computation
+   */
+  double origin_latitude_;
+  double origin_longitude_;
+  double origin_altitude_;
 };
 
 }  // namespace robot_localization

params/navsat_transform.yaml

@@ -48,3 +48,10 @@ navsat_transform_node:
 # and heading in radians. As navsat_transform_node assumes an ENU standard, a 0 heading corresponds to east.
         datum: [55.944904, -3.186693, 0.0]
 
+# Earth frame ID for ECEF coordinates (REP-105 compliant). Defaults to "earth".
+        earth_frame_id: "earth"
+
+# If this is true, the transform earth->cartesian or earth->world transform is broadcast for use by other nodes.
+# This enables REP-105 compliant ECEF coordinate system support. Defaults to false.
+        broadcast_earth_transform: false
+

src/navsat_transform.cpp

@@ -85,13 +85,18 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options)
   force_user_utm_(false),
   use_manual_datum_(false),
   use_odometry_yaw_(false),
-  cartesian_broadcaster_(*this),
+  tf_broadcaster_(*this),
   utm_meridian_convergence_(0.0),
   utm_zone_(0),
   northp_(true),
   world_frame_id_("odom"),
   yaw_offset_(0.0),
-  zero_altitude_(false)
+  zero_altitude_(false),
+  earth_frame_id_("earth"),
+  broadcast_earth_transform_(false),
+  origin_latitude_(0.0),
+  origin_longitude_(0.0),
+  origin_altitude_(0.0)
 {
   tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
   tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);
@@ -145,6 +150,21 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options)
       broadcast_cartesian_transform_as_parent_frame_);
   }
 
+  earth_frame_id_ = this->declare_parameter("earth_frame_id", earth_frame_id_);
+  broadcast_earth_transform_ = this->declare_parameter("broadcast_earth_transform",
+      broadcast_earth_transform_);
+
+  // Validate parameter combinations
+  if (broadcast_earth_transform_ && !use_local_cartesian_) {
+    RCLCPP_ERROR(
+      this->get_logger(),
+      "Invalid parameter combination: broadcast_earth_transform=true requires "
+      "use_local_cartesian=true. Earth frame transforms are only supported with local cartesian "
+      "coordinates, not UTM. This setting will be ignored!");
+    broadcast_earth_transform_ = false;
+  }
+
+
   if (!this->get_clock()->started()) {
     RCLCPP_INFO(this->get_logger(), "Waiting for clock to start...");
     this->get_clock()->wait_until_started();
@@ -353,22 +373,60 @@ void NavSatTransform::computeTransform()
 
     transform_good_ = true;
 
-    // Send out the (static) UTM transform in case anyone else would like to use
-    // it.
+    // Prepare vector of transforms to publish
+    std::vector<geometry_msgs::msg::TransformStamped> transforms_to_publish;
+
+    // Add cartesian transform if requested
     if (broadcast_cartesian_transform_) {
       geometry_msgs::msg::TransformStamped cartesian_transform_stamped;
       cartesian_transform_stamped.header.stamp = this->now();
-      std::string cartesian_frame_id = (use_local_cartesian_ ? "local_enu" : "utm");
-      cartesian_transform_stamped.header.frame_id =
-        (broadcast_cartesian_transform_as_parent_frame_ ? cartesian_frame_id : world_frame_id_);
-      cartesian_transform_stamped.child_frame_id =
-        (broadcast_cartesian_transform_as_parent_frame_ ? world_frame_id_ : cartesian_frame_id);
-      cartesian_transform_stamped.transform =
-        (broadcast_cartesian_transform_as_parent_frame_ ?
-        tf2::toMsg(cartesian_world_trans_inverse_) : tf2::toMsg(cartesian_world_transform_));
+      std::string const cartesian_frame_id =
+        (use_local_cartesian_ ? "local_enu" : "utm");
+      if (broadcast_cartesian_transform_as_parent_frame_) {
+        cartesian_transform_stamped.header.frame_id = cartesian_frame_id;
+        cartesian_transform_stamped.child_frame_id = world_frame_id_;
+        cartesian_transform_stamped.transform = tf2::toMsg(cartesian_world_trans_inverse_);
+      } else {
+        cartesian_transform_stamped.header.frame_id = world_frame_id_;
+        cartesian_transform_stamped.child_frame_id = cartesian_frame_id;
+        cartesian_transform_stamped.transform = tf2::toMsg(cartesian_world_transform_);
+      }
       cartesian_transform_stamped.transform.translation.z =
         (zero_altitude_ ? 0.0 : cartesian_transform_stamped.transform.translation.z);
-      cartesian_broadcaster_.sendTransform(cartesian_transform_stamped);
+      transforms_to_publish.push_back(cartesian_transform_stamped);
+    }
+
+    // Add earth frame transforms if requested
+    // Note: Only valid when use_local_cartesian_ is true (validated in constructor)
+    if (broadcast_earth_transform_) {
+      computeEarthToCartesian(origin_latitude_, origin_longitude_, origin_altitude_);
+
+      geometry_msgs::msg::TransformStamped earth_transform_stamped;
+
+      earth_transform_stamped.header.stamp = this->now();
+      earth_transform_stamped.header.frame_id = earth_frame_id_;
+
+      // If cartesian -> world is published, then we public earth -> cartesian
+      // Otherwise, we publish earth -> world
+      if (broadcast_cartesian_transform_ &&
+        broadcast_cartesian_transform_as_parent_frame_)
+      {
+        earth_transform_stamped.child_frame_id = "local_enu";
+        earth_transform_stamped.transform = tf2::toMsg(earth_cartesian_transform_.inverse());
+
+      } else {
+        tf2::Transform const C2W = cartesian_world_transform_;
+        tf2::Transform const E2W = C2W * earth_cartesian_transform_;
+
+        earth_transform_stamped.child_frame_id = world_frame_id_;
+        earth_transform_stamped.transform = tf2::toMsg(E2W.inverse());
+      }
+      transforms_to_publish.push_back(earth_transform_stamped);
+    }
+
+    // Publish all transforms in a single call
+    if (!transforms_to_publish.empty()) {
+      tf_broadcaster_.sendTransform(transforms_to_publish);
     }
   }
 }
@@ -948,6 +1006,11 @@ bool NavSatTransform::prepareGpsOdometry(nav_msgs::msg::Odometry * gps_odom)
 void NavSatTransform::setTransformGps(
   const sensor_msgs::msg::NavSatFix::SharedPtr & msg)
 {
+  // Store origin coordinates for earth frame computation
+  origin_latitude_ = msg->latitude;
+  origin_longitude_ = msg->longitude;
+  origin_altitude_ = msg->altitude;
+
   double cartesian_x {};
   double cartesian_y {};
   double cartesian_z {};
@@ -1038,4 +1101,38 @@ rcl_interfaces::msg::SetParametersResult NavSatTransform::parametersCallback(
   return result;
 }
 
+void NavSatTransform::computeEarthToCartesian(double lat0_deg, double lon0_deg, double h0_m)
+{
+  // This function should only be called when use_local_cartesian_ is true
+  // (validated in constructor)
+
+  GeographicLib::Geocentric geo(GeographicLib::Constants::WGS84_a(),
+    GeographicLib::Constants::WGS84_f());
+  double X0, Y0, Z0;
+  geo.Forward(lat0_deg, lon0_deg, h0_m, X0, Y0, Z0);
+
+  double sphi, cphi, slam, clam;
+  GeographicLib::Math::sincosd(lat0_deg, sphi, cphi);
+  GeographicLib::Math::sincosd(lon0_deg, slam, clam);
+
+  // ECEF -> ENU at datum (using GeographicLib's exact matrix layout)
+  tf2::Matrix3x3 R_ecef_to_enu(
+    -slam, clam, 0,                          // East axis
+    -clam * sphi, -slam * sphi, cphi,        // North axis
+    clam * cphi, slam * cphi, sphi           // Up axis
+  );
+
+  // Create transform to match LocalCartesian behavior
+  // LocalCartesian does: R * (input - origin)
+  // TF2 does: R * input + translation
+  // Therefore: translation = -R * origin
+  tf2::Vector3 origin_ecef(X0, Y0, Z0);
+  tf2::Vector3 translated_origin = -(R_ecef_to_enu * origin_ecef);
+  tf2::Transform E2C(R_ecef_to_enu, translated_origin);
+
+  // Store the transform (earth -> local_enu)
+  earth_cartesian_transform_ = E2C;
+}
+
+
 }  // namespace robot_localization