Port to Dashing: melodic-devel 2.6.3 to current (cra-ros-pkg#530)

* Fixing Euler body-to-world transformations

(cherry picked from commit 58bef05)

* fix RLE/RLL tests

* Enabling the user to override the output child_frame_id

(cherry picked from commit f642190)

* Add const& to catch values to prevent the error:  catching polymorphic type ‘class tf2::TransformException’ by value
And ZoneNumber by 0x3fU to prevent error: directive output may be truncated writing between 1 and 11 bytes into a region of size 4

(cherry picked from commit c1963c3)

* Rename odomBaseLinkTrans to baseLinkOdomTrans

Adhere to the naming convention <fromFrame><toFrame>Trans used for worldBaseLinkTrans and mapOdomTrans.

(cherry picked from commit 17d4f10)

* Enable build optimisations if no build type configured.

The selected build type can have a significant influence on processor usage (tenths of %). As setting a build type is often forgotten by users, from-source builds of robot_localization on resource constraint platforms can run into issues of low update rates, starvation and issues with stability.

If a build type is configured (on the ROS buildfarm fi), the added stanzas respect it and do not overwrite it. If nothing is configured, a warning is printed and the CMake-standard `RelWithDebInfo` type is configured.

(cherry picked from commit cc93cef)

* fix differential init typo, compiler warning

* Add broadcast_utm_transform_as_parent_frame

(cherry picked from commit e8deb5d)

* Fixing title underline

(cherry picked from commit da80adf)

* Change note about transform publishing

(cherry picked from commit 0eecd5f)

* Meridian convergence adjustment added to navsat_transform.

(cherry picked from commit 2b57df6)

* Add missing undocumented params

(cherry picked from commit 2585e9f)

* Fix bug with tf_prefix

(cherry picked from commit 492c494)

* Created service for converting to / from lat long

(cherry picked from commit fbd3a72)

* Fixed linting errors.

(cherry picked from commit 46396c0)

* Refactored common conversion methods out

(cherry picked from commit 1efe3a3)

* Remove unused variable utm_odom_tf_yaw_ for clang compatibility

(cherry picked from commit 2f0289d)

* Mask IMU update vector to ignore position and linear velocity

* They are both not contained in sensor_msgs/Imu but so far
  were still introduced if the param in the config file was
  set to true

(cherry picked from commit 0d32daf)

* Warning log output if for invalid IMU config settings

(cherry picked from commit e2f75c3)

* Set the filtered gps frame_id to base_link
This is actually the frame_id of the information, not the 'gps' frame which might not even exist

(cherry picked from commit 791953a)

* Log initial pose once and continue with debug streams

(cherry picked from commit a59e1e5)

* Skip the debug log

(cherry picked from commit 0e6cf56)

* linter

* alphabetize cmake srv list

Co-authored-by: Tom Moore <[email protected]>
Co-authored-by: Matthew Jones <[email protected]>
Co-authored-by: thallerod <[email protected]>
Co-authored-by: G.A. vd. Hoorn <[email protected]>
Co-authored-by: diasdm <[email protected]>
Co-authored-by: Pavlo Kolomiiets <[email protected]>
Co-authored-by: Charles Brian Quinn <[email protected]>
Co-authored-by: oswinso <[email protected]>
Co-authored-by: Timon Engelke <[email protected]>
Co-authored-by: Jonathan Huber <[email protected]>
Co-authored-by: Tim Clephas <[email protected]>

CMakeLists.txt

@@ -9,12 +9,17 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
+  message("${PROJECT_NAME}: You did not request a specific build type: selecting 'RelWithDebInfo'.")
+  set(CMAKE_BUILD_TYPE RelWithDebInfo)
+endif()
+
 find_package(ament_cmake REQUIRED)
 find_package(rcl REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(geographic_msgs REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
 find_package(diagnostic_updater REQUIRED)
+find_package(geographic_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
@@ -32,10 +37,12 @@ pkg_check_modules(YAML_CPP yaml-cpp)
 set(library_name rl_lib)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/FromLL.srv"
   "srv/GetState.srv"
   "srv/SetDatum.srv"
   "srv/SetPose.srv"
   "srv/ToggleFilterProcessing.srv"
+  "srv/ToLL.srv"
   DEPENDENCIES
     builtin_interfaces
     geometry_msgs

doc/navsat_transform_node.rst

@@ -47,6 +47,15 @@ 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*.
+
+~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.
+
 Subscribed Topics
 =================
 * ``imu/data`` A `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ message with orientation data
@@ -63,4 +72,4 @@ 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_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->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.

doc/state_estimation_nodes.rst

@@ -43,21 +43,23 @@ Specific parameters:
 * ``~map_frame``
 * ``~odom_frame``
 * ``~base_link_frame``
+* ``~base_link_output_frame``
 * ``~world_frame``
 
 These parameters define the operating "mode" for ``robot_localization``. `REP-105 <http://www.ros.org/reps/rep-0105.html>`_ specifies three principal coordinate frames: *map*, *odom*, and *base_link*. *base_link* is the coordinate frame that is affixed to the robot. The robot's position in the *odom* frame will drift over time, but is accurate in the short term and should be continuous. The *map* frame, like the *odom* frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data. Here is how to use these parameters:
 
 1. Set the ``map_frame``, ``odom_frame``, and ``base_link_frame`` parameters to the appropriate frame names for your system.
 
  .. note:: If your system does not have a ``map_frame``, just remove it, and make sure ``world_frame`` is set to the value of ``odom_frame``.
+ .. note:: If you are running multiple EKF instances and would like to "override" the output transform and message to have this frame for its ``child_frame_id``, you may set this.  The ``base_link_output_frame`` is optional and will default to the ``base_link_frame``. This helps to enable disconnected TF trees when multiple EKF instances are being run. When the final state is computed, we "override" the output transform and message to have this frame for its ``child_frame_id``.
 
 2. If you are only fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set ``world_frame`` to your ``odom_frame`` value. This is the default behavior for the state estimation nodes in ``robot_localization``, and the most common use for it.
 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark observations) then:
 
  i. Set your ``world_frame`` to your ``map_frame`` value
  ii. **Make sure** something else is generating the *odom->base_link* transform. This can even be another instance of a ``robot_localization`` state estimation node. However, that instance should *not* fuse the global data.
 
-The default values for ``map_frame``, ``odom_frame``, and ``base_link_frame`` are *map*, *odom,* and *base_link,* respectively. The ``world_frame`` parameter defaults to the value of ``odom_frame``.
+The default values for ``map_frame``, ``odom_frame``, and ``base_link_frame`` are *map*, *odom,* and *base_link,* respectively. The ``base_link_output_frame`` parameter defaults to the value of ``base_link_frame``. The ``world_frame`` parameter defaults to the value of ``odom_frame``.
 
 ~transform_time_offset
 ^^^^^^^^^^^^^^^^^^^^^^
@@ -286,6 +288,14 @@ The estimate covariance, commonly denoted *P*, defines the error in the current
 ^^^^^^^^^^^^^^^^^^^
 If set to *true* and ``ros::Time::isSimTime()`` is *true*, the filter will reset to its uninitialized state when a jump back in time is detected on a topic. This is useful when working with bag data, in that the bag can be restarted without restarting the node.
 
+~predict_to_current_time
+^^^^^^^^^^^^^^^^^^^^^^^^
+If set to *true*, the filter predicts and corrects up to the time of the latest measurement (by default) but will now also predict up to the current time step.
+
+~disabled_at_startup
+^^^^^^^^^^^^^^^^^^^^
+If set to *true* will not run the filter on start.
+
 Node-specific Parameters
 ------------------------
 The standard and advanced parameters are common to all state estimation nodes in ``robot_localization``. This section details parameters that are unique to their respective state estimation nodes.
@@ -319,4 +329,4 @@ Published Transforms
 Services
 ========
 
-* ``set_pose`` - By issuing a `geometry_msgs/PoseWithCovarianceStamped <http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html>`_ message to the ``set_pose`` topic, users can manually set the state of the filter. This is useful for resetting the filter during testing, and allows for interaction with ``rviz``. Alternatively, the state estimation nodes advertise a ``SetPose`` service, whose type is `robot_localization/SetPose <http://docs.ros.org/api/robot_localization/html/srv/SetPose.html>`_.rejection
+* ``set_pose`` - By issuing a `geometry_msgs/PoseWithCovarianceStamped <http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html>`_ message to the ``set_pose`` topic, users can manually set the state of the filter. This is useful for resetting the filter during testing, and allows for interaction with ``rviz``. Alternatively, the state estimation nodes advertise a ``SetPose`` service, whose type is `robot_localization/SetPose <http://docs.ros.org/api/robot_localization/html/srv/SetPose.html>`_.

include/robot_localization/filter_common.hpp

@@ -90,6 +90,7 @@ const int POSE_SIZE = 6;
 const int TWIST_SIZE = 6;
 const int POSITION_SIZE = 3;
 const int ORIENTATION_SIZE = 3;
+const int LINEAR_VELOCITY_SIZE = 3;
 const int ACCELERATION_SIZE = 3;
 
 /**

include/robot_localization/navsat_conversions.hpp

@@ -204,13 +204,17 @@ static inline char UTMLetterDesignator(double Lat)
  * East Longitudes are positive, West longitudes are negative.
  * North latitudes are positive, South latitudes are negative
  * Lat and Long are in fractional degrees
+ * Meridian convergence is computed as for Spherical Transverse Mercator,
+ * which gives quite good approximation.
+ *
+ * @param[out] gamma meridian convergence at point (degrees).
  *
  * Written by Chuck Gantz- [email protected]
  */
 static inline void LLtoUTM(
   const double Lat, const double Long,
   double & UTMNorthing, double & UTMEasting,
-  std::string & UTMZone)
+  std::string & UTMZone, double & gamma)
 {
   double a = WGS84_A;
   double eccSquared = UTM_E2;
@@ -253,7 +257,9 @@ static inline void LLtoUTM(
 
   // Compute the UTM Zone from the latitude and longitude
   char zone_buf[] = {0, 0, 0, 0};
-  snprintf(zone_buf, sizeof(zone_buf), "%d%c", ZoneNumber,
+  // We &0x3fU to let GCC know the size of ZoneNumber. In this case, it's under
+  // 63 (6bits)
+  snprintf(zone_buf, sizeof(zone_buf), "%d%c", ZoneNumber & 0x3fU,
     UTMLetterDesignator(Lat));
   UTMZone = std::string(zone_buf);
 
@@ -290,25 +296,49 @@ static inline void LLtoUTM(
     (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A *
     A * A * A * A * A / 720)));
 
+  gamma = atan(tan(LongRad - LongOriginRad) * sin(LatRad)) * DEGREES_PER_RADIAN;
+
   if (Lat < 0) {
     // 10000000 meter offset for southern hemisphere
     UTMNorthing += 10000000.0;
   }
 }
 
+/**
+ * Convert lat/long to UTM coords.  Equations from USGS Bulletin 1532
+ *
+ * East Longitudes are positive, West longitudes are negative.
+ * North latitudes are positive, South latitudes are negative
+ * Lat and Long are in fractional degrees
+ *
+ * Written by Chuck Gantz- [email protected]
+ */
+static inline void LLtoUTM(
+  const double Lat, const double Long,
+  double & UTMNorthing, double & UTMEasting,
+  std::string & UTMZone)
+{
+  double gamma;
+  LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, UTMZone, gamma);
+}
+
 /**
  * Converts UTM coords to lat/long.  Equations from USGS Bulletin 1532
  *
  * East Longitudes are positive, West longitudes are negative.
  * North latitudes are positive, South latitudes are negative
  * Lat and Long are in fractional degrees.
+ * Meridian convergence is computed as for Spherical Transverse Mercator,
+ * which gives quite good approximation.
+ *
+ * @param[out] gamma meridian convergence at point (degrees).
  *
  * Written by Chuck Gantz- [email protected]
  */
 static inline void UTMtoLL(
   const double UTMNorthing, const double UTMEasting,
   const std::string & UTMZone, double & Lat,
-  double & Long)
+  double & Long, double & gamma)
 {
   double k0 = UTM_K0;
   double a = WGS84_A;
@@ -367,6 +397,25 @@ static inline void UTMtoLL(
     D * D * D * D * D / 120) /
     cos(phi1Rad));
   Long = LongOrigin + Long * DEGREES_PER_RADIAN;
+
+  gamma = atan(tanh(x / (k0 * a)) * tan(y / (k0 * a))) * DEGREES_PER_RADIAN;
+}
+
+/**
+* Converts UTM coords to lat/long.  Equations from USGS Bulletin 1532
+*
+* East Longitudes are positive, West longitudes are negative.
+* North latitudes are positive, South latitudes are negative
+* Lat and Long are in fractional degrees.
+*
+* Written by Chuck Gantz- [email protected]
+*/
+static inline void UTMtoLL(
+  const double UTMNorthing, const double UTMEasting,
+  const std::string & UTMZone, double & Lat, double & Long)
+{
+  double gamma;
+  UTMtoLL(UTMNorthing, UTMEasting, UTMZone, Lat, Long, gamma);
 }
 
 }  // namespace navsat_conversions

include/robot_localization/navsat_transform.hpp

@@ -34,6 +34,8 @@
 #define ROBOT_LOCALIZATION__NAVSAT_TRANSFORM_HPP_
 
 #include <robot_localization/srv/set_datum.hpp>
+#include <robot_localization/srv/to_ll.hpp>
+#include <robot_localization/srv/from_ll.hpp>
 
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -83,6 +85,18 @@ class NavSatTransform : public rclcpp::Node
     request,
     std::shared_ptr<robot_localization::srv::SetDatum::Response>);
 
+  //! @brief Callback for the to Lat Long service
+  //!
+  bool toLLCallback(
+    const std::shared_ptr<robot_localization::srv::ToLL::Request> request,
+    std::shared_ptr<robot_localization::srv::ToLL::Response> response);
+
+  //! @brief Callback for the from Lat Long service
+  //!
+  bool fromLLCallback(
+    const std::shared_ptr<robot_localization::srv::FromLL::Request> request,
+    std::shared_ptr<robot_localization::srv::FromLL::Response> response);
+
   /**
    * @brief Given the pose of the navsat sensor in the UTM frame, removes the
    * offset from the vehicle's centroid and returns the UTM-frame pose of said
@@ -147,6 +161,20 @@ class NavSatTransform : public rclcpp::Node
    */
   void setTransformOdometry(const nav_msgs::msg::Odometry::SharedPtr & msg);
 
+  /**
+   * @brief Transforms the passed in pose from utm to map frame
+   *  @param[in] utm_pose the pose in utm frame to use to transform
+   */
+  nav_msgs::msg::Odometry utmToMap(const tf2::Transform & utm_pose) const;
+
+  /**
+   * @brief Transforms the passed in point from map frame to lat/long
+   * @param[in] point the point in map frame to use to transform
+   */
+  void mapToLL(
+    const tf2::Vector3 & point, double & latitude, double & longitude,
+    double & altitude) const;
+
   /**
    * @brief Frame ID of the robot's body frame
    *
@@ -171,6 +199,16 @@ class NavSatTransform : public rclcpp::Node
    */
   rclcpp::Service<robot_localization::srv::SetDatum>::SharedPtr datum_srv_;
 
+  /**
+   * @brief Service for to Lat Long
+   */
+  rclcpp::Service<robot_localization::srv::ToLL>::SharedPtr to_ll_srv_;
+
+  /**
+   * @brief Service for from Lat Long
+   */
+  rclcpp::Service<robot_localization::srv::FromLL>::SharedPtr from_ll_srv_;
+
   /**
    * @brief Navsatfix publisher
    */
@@ -337,9 +375,14 @@ class NavSatTransform : public rclcpp::Node
   tf2_ros::StaticTransformBroadcaster utm_broadcaster_;
 
   /**
-   * @brief Stores the yaw we need to compute the transform
+   * @brief UTM's meridian convergence
+   *
+   * Angle between projected meridian (True North) and UTM's grid Y-axis.
+   * For UTM projection (Ellipsoidal Transverse Mercator) it is zero on the
+   * equator and non-zero everywhere else. It increases as the poles are
+   * approached or as we're getting farther from central meridian.
    */
-  double utm_odom_tf_yaw_;
+  double utm_meridian_convergence_;
 
   /**
    * @brief Holds the UTM->odom transform

include/robot_localization/ros_filter.hpp

@@ -554,6 +554,14 @@ class RosFilter : public rclcpp::Node
   //!
   std::string base_link_frame_id_;
 
+  //! @brief tf frame name for the robot's body frame
+  //!
+  //! When the final state is computed, we "override" the output transform and
+  //! message to have this frame for its child_frame_id. This helps to enable
+  //! disconnected TF trees when multiple EKF instances are being run.
+  //!
+  std::string base_link_output_frame_id_;
+
   //! @brief tf frame name for the robot's map (world-fixed) frame
   //!
   std::string map_frame_id_;