ROS1 recent feature port

CMakeLists.txt

@@ -31,6 +31,10 @@ find_package(tf2_ros REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(Boost REQUIRED)
 
+# Geographiclib installs FindGeographicLib.cmake to this non-standard location
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/")
+find_package(GeographicLib REQUIRED COMPONENTS STATIC)
+
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(YAML_CPP yaml-cpp)
 
@@ -95,6 +99,7 @@ add_executable(
 
 target_link_libraries(
   ${library_name}
+  ${GeographicLib_LIBRARIES}
   ${EIGEN3_LIBRARIES}
   ${YAML_CPP_LIBRARIES}
 )

doc/images/navsat_transform_workflow.tex

@@ -0,0 +1,49 @@
+\documentclass{standalone}
+\usepackage{tikz}
+\usetikzlibrary{positioning}
+
+\begin{document}
+
+\sffamily
+
+\begin{tikzpicture}[>=stealth,
+  node distance = 3cm,
+  box/.style={shape=rectangle,draw,rounded corners},]
+    % Nodes
+    \node (wheel) [box] {Wheel Odometry};
+    \node (imu) [box, right =of wheel]{IMU};
+    \node (ekfLocal) [box, below =of wheel]{EKF Local};
+    \node (ekfGlobal) [box, below =of imu]{EKF Global};
+    \node (navsat) [box, right =7cm of ekfGlobal]{navsat\_transform};
+    \node (gps) [box, above =of navsat]{GPS};
+    % Coordinates for edges pointing to empty space
+    \node (gpsF) [right =of navsat]{};
+    \node (tfLocal) [below =of ekfLocal]{};
+    \node (odomLocal) [left =of ekfLocal]{};
+    \node (tfGlobal) [below =of ekfGlobal]{};
+    % Edges
+    \draw[->] (wheel.270) -- (ekfLocal.90);
+    \draw[->] (wheel.315) -- (ekfGlobal.135)
+      node [fill=white, pos=0.2, align=center] {\ttfamily"/wheel/odometry"\\nav\_msgs/Odometry};
+    \draw[->] (imu.225) -- (ekfLocal.45);
+    \draw[->] (imu.315) -- (navsat.135);
+    \draw[->] (imu.270) -- (ekfGlobal.90)
+      node [fill=white, pos=0.2, align=center] {\ttfamily"/imu/data"\\sensor\_msgs/Imu};
+    \draw[->] (gps.270) -- (navsat.90)
+      node [fill=white, pos=0.2, align=center] {\ttfamily"/gps/fix"\\sensor\_msgs/NavSatFix};
+    \draw[->, transform canvas={yshift=1mm}] (ekfGlobal) -- (navsat)
+      node [fill=white, above=1mm, pos=0.5, align=center] {\ttfamily"/odometry/filtered/global"\\nav\_msgs/Odometry};
+    \draw[->, transform canvas={yshift=-1mm}] (navsat) -- (ekfGlobal)
+      node [fill=white, below=1mm, pos=0.5, align=center] {\ttfamily"/odometry/gps"\\nav\_msgs/Odometry};
+    % Outputs not cycled back into the graph
+    \draw[->, dashed] (navsat) -- (gpsF)
+      node [fill=white, above=1mm, pos=1.0, align=center] {\ttfamily"/gps/filtered"\\sensor\_msgs/NavSatFix};
+    \draw[->, dashed] (ekfLocal) -- (tfLocal)
+      node [fill=white, pos=0.7, align=center] {\ttfamily"/tf" odom -> base\\tf2\_msgs/TFMessage};
+    \draw[->, dashed] (ekfLocal) -- (odomLocal)
+      node [fill=white, above=1mm, pos=1.0, align=center] {\ttfamily"/odometry/filtered/local"\\nav\_msgs/Odometry};
+    \draw[->, dashed] (ekfGlobal) -- (tfGlobal)
+      node [fill=white, pos=0.7, align=center] {\ttfamily"/tf" map -> odom\\tf2\_msgs/TFMessage};
+\end{tikzpicture}
+
+\end{document}

doc/integrating_gps.rst

@@ -18,6 +18,14 @@ This is just a suggestion, however, and users are free to fuse the GPS data into
 Using navsat_transform_node
 ***************************
 
+The diagram below illustrates an example setup:
+
+.. image:: images/navsat_transform_workflow.png
+  :width: 800px
+  :align: center
+  :alt: navsat_transform workflow
+
+
 Required Inputs
 ===============
 
@@ -129,22 +137,22 @@ You should have no need to modify the ``_differential`` setting within the state
 Details
 =======
 
-We'll start with a picture. Consider a robot that starts at some latitude and longitude and with some heading. We assume in this tutorial that the heading comes from an IMU that reads 0 when facing east, and increases according to the ROS spec (i.e., counter-clockwise). The remainder of this tutorial will refer to Figure 1:
+We'll start with a picture. Consider a robot that starts at some latitude and longitude and with some heading. We assume in this tutorial that the heading comes from an IMU that reads 0 when facing east, and increases according to the ROS spec (i.e., counter-clockwise). The remainder of this tutorial will refer to Figure 2:
 
-.. image:: images/figure1.png
+.. image:: images/figure2.png
   :width: 800px
   :align: center
-  :alt: Figure 1
+  :alt: Figure 2
 
 
-`REP-105 <http://www.ros.org/reps/rep-0105.html>`_ suggests four coordinate frames: *base_link*, *odom*, *map*, and *earth*. *base_link* is the coordinate frame that is rigidly attached to the vehicle. The *odom* and *map* frames are world-fixed frames and generally have their origins at the vehicle's start position and orientation. The *earth* frame is used as a common reference frame for multiple map frames, and is not yet supported by ``navsat_transform_node``. Note that in Figure 1, the robot has just started (``t = 0``), and so its *base_link*, *odom*, and *map* frames are aligned. We can also define a coordinate frame for the UTM grid, which we will call *utm*. For the purposes of this tutorial, we will refer to the UTM grid coordinate frame as *utm*. Therefore, what we want to do is create a *utm*->*map* transform.
+`REP-105 <http://www.ros.org/reps/rep-0105.html>`_ suggests four coordinate frames: *base_link*, *odom*, *map*, and *earth*. *base_link* is the coordinate frame that is rigidly attached to the vehicle. The *odom* and *map* frames are world-fixed frames and generally have their origins at the vehicle's start position and orientation. The *earth* frame is used as a common reference frame for multiple map frames, and is not yet supported by ``navsat_transform_node``. Note that in Figure 2, the robot has just started (``t = 0``), and so its *base_link*, *odom*, and *map* frames are aligned. We can also define a coordinate frame for the UTM grid, which we will call *utm*. For the purposes of this tutorial, we will refer to the UTM grid coordinate frame as *utm*. Therefore, what we want to do is create a *utm*->*map* transform.
 
-Referring to Figure 1, these ideas are (hopefully) made clear. The UTM origin is the :math:`(0_{UTM}, 0_{UTM})` point of the UTM zone that is associated with the robot's GPS location. The robot begins somewhere within the UTM zone at location :math:`(x_{UTM}, y_{UTM})`. The robot's initial orientation is some angle :math:`\theta` above the UTM grid's :math:`X`-axis. Our transform will therefore require that we know :math:`x_{UTM}, y_{UTM}` and :math:`\theta`.
+Referring to Figure 2, these ideas are (hopefully) made clear. The UTM origin is the :math:`(0_{UTM}, 0_{UTM})` point of the UTM zone that is associated with the robot's GPS location. The robot begins somewhere within the UTM zone at location :math:`(x_{UTM}, y_{UTM})`. The robot's initial orientation is some angle :math:`\theta` above the UTM grid's :math:`X`-axis. Our transform will therefore require that we know :math:`x_{UTM}, y_{UTM}` and :math:`\theta`.
 
 We now need to convert our latitude and longitude to UTM coordinates. The UTM grid assumes that the :math:`X`-axis faces east, the :math:`Y`-axis faces (true) north, and the :math:`Z`-axis points up out of the ground. This complies with the right-handed coordinate frame as dictated by `REP-105 <http://www.ros.org/reps/rep-0105.html>`_. The REP also states that a yaw angle of :math:`0` means that we are facing straight down the :math:`X`-axis, and that the yaw increases counter-clockwise. ``navsat_transform_node`` assumes your heading data conforms to this standard. However, there are two factors that need to be considered: 
 
 1. The IMU driver may not allow the user to apply the magnetic declination correction factor
-2. The IMU driver may incorrectly report :math:`0` when facing north, and not when facing east (even though its headings increase and decrease correctly). Fortunately, ``navsat_transform_node`` exposes two parameters to adddress these possible shortcomings in IMU data: ``magnetic_declination_radians`` and ``yaw_offset``. Referring to Figure 1, for an IMU that is currently measuring a yaw value of ``imu_yaw``, 
+2. The IMU driver may incorrectly report :math:`0` when facing north, and not when facing east (even though its headings increase and decrease correctly). Fortunately, ``navsat_transform_node`` exposes two parameters to adddress these possible shortcomings in IMU data: ``magnetic_declination_radians`` and ``yaw_offset``. Referring to Figure 2, for an IMU that is currently measuring a yaw value of ``imu_yaw``, 
 
  :math:`yaw_{imu} = -\omega - offset_{yaw} + \theta`
 

doc/navsat_transform_node.rst

@@ -18,7 +18,7 @@ The time, in seconds, to wait before calculating the transform from GPS coordina
 
 ~magnetic_declination_radians
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Enter the magnetic declination for your location. If you don't know it, see `http://www.ngdc.noaa.gov/geomag-web <http://www.ngdc.noaa.gov/geomag-web>`_ (make sure to convert the value to radians). This parameter is needed if your IMU prodives its orientation with respect to the magnetic north.
+Enter the magnetic declination for your location. If you don't know it, see `http://www.ngdc.noaa.gov/geomag-web <http://www.ngdc.noaa.gov/geomag-web>`_ (make sure to convert the value to radians). This parameter is needed if your IMU provides its orientation with respect to the magnetic north.
 
 ~yaw_offset
 ^^^^^^^^^^^

doc/state_estimation_nodes.rst

@@ -166,6 +166,14 @@ Starts the filter with the specified state. The state is given as a 15-D vector
                                   0.0,  0.0,  0.0,
                                   0.0,  0.0,  0.0]</rosparam>
 
+~permit_corrected_publication
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+When the state estimation nodes publish the state at time `t`, but then receive a measurement with a timestamp < `t`, they re-publish the corrected state, with the same time stamp as the previous publication. Setting this parameter to *false* disables that behavior. Defaults to *false*.
+
+~print_diagnostics
+^^^^^^^^^^^^^^^^^^
+If true, the state estimation node will publish diagnostic messages to the ``/diagnostics`` topic. This is useful for debugging your configuration and sensor data.
+
 ~publish_tf
 ^^^^^^^^^^^
 If *true*, the state estimation node will publish the transform from the frame specified by the ``world_frame`` parameter to the frame specified by the ``base_link_frame`` parameter. Defaults to *true*.
@@ -174,10 +182,6 @@ If *true*, the state estimation node will publish the transform from the frame s
 ^^^^^^^^^^^^^^^^^^^^^
 If *true*, the state estimation node will publish the linear acceleration state. Defaults to *false*.
 
-~print_diagnostics
-^^^^^^^^^^^^^^^^^^
-If true, the state estimation node will publish diagnostic messages to the ``/diagnostics`` topic. This is useful for debugging your configuration and sensor data.
-
 Advanced Parameters
 -------------------
 

include/robot_localization/navsat_transform.hpp

@@ -37,6 +37,9 @@
 #include <robot_localization/srv/to_ll.hpp>
 #include <robot_localization/srv/from_ll.hpp>
 
+#include <Eigen/Dense>
+#include <GeographicLib/Geocentric.hpp>
+#include <GeographicLib/LocalCartesian.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/imu.hpp>
@@ -46,7 +49,6 @@
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 
-#include <Eigen/Dense>
 #include <memory>
 #include <string>
 
@@ -73,7 +75,7 @@ class NavSatTransform : public rclcpp::Node
   void transformCallback();
 
   /**
-   * @brief Computes the transform from the UTM frame to the odom frame
+   * @brief Computes the transform from the Cartesian frame to the odom frame
    */
   void computeTransform();
 
@@ -98,13 +100,13 @@ class NavSatTransform : public rclcpp::Node
     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
+   * @brief Given the pose of the navsat sensor in the Cartesian frame, removes the
+   * offset from the vehicle's centroid and returns the Cartesian-frame pose of said
    * centroid.
    */
-  void getRobotOriginUtmPose(
-    const tf2::Transform & gps_utm_pose,
-    tf2::Transform & robot_utm_pose,
+  void getRobotOriginCartesianPose(
+    const tf2::Transform & gps_cartesian_pose,
+    tf2::Transform & robot_cartesian_pose,
     const rclcpp::Time & transform_time);
 
   /**
@@ -162,10 +164,10 @@ 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
+   * @brief Transforms the passed in pose from Cartesian to map frame
+   *  @param[in] cartesian_pose the pose in Cartesian frame to use to transform
    */
-  nav_msgs::msg::Odometry utmToMap(const tf2::Transform & utm_pose) const;
+  nav_msgs::msg::Odometry cartesianToMap(const tf2::Transform & cartesian_pose) const;
 
   /**
    * @brief Transforms the passed in point from map frame to lat/long
@@ -184,15 +186,15 @@ class NavSatTransform : public rclcpp::Node
   std::string base_link_frame_id_;
 
   /**
-   * @brief Whether or not we broadcast the UTM transform
+   * @brief Whether or not we broadcast the Cartesian transform
    */
-  bool broadcast_utm_transform_;
+  bool broadcast_cartesian_transform_;
 
   /**
-   * @brief Whether to broadcast the UTM transform as parent frame, default as
+   * @brief Whether to broadcast the Cartesian transform as parent frame, default as
    * child
    */
-  bool broadcast_utm_transform_as_parent_frame_;
+  bool broadcast_cartesian_transform_as_parent_frame_;
 
   /**
    * @brief TimerBase for publish callback
@@ -271,14 +273,14 @@ class NavSatTransform : public rclcpp::Node
   Eigen::MatrixXd latest_odom_covariance_;
 
   /**
-   * @brief Covariance for most recent GPS/UTM data
+   * @brief Covariance for most recent GPS/UTM/LocalCartesian data
    */
-  Eigen::MatrixXd latest_utm_covariance_;
+  Eigen::MatrixXd latest_cartesian_covariance_;
 
   /**
-   * @brief Latest GPS data, stored as UTM coords
+   * @brief Latest GPS data, stored as Cartesian coords
    */
-  tf2::Transform latest_utm_pose_;
+  tf2::Transform latest_cartesian_pose_;
 
   /**
    * @brief Latest odometry pose data
@@ -349,15 +351,24 @@ class NavSatTransform : public rclcpp::Node
   tf2::Duration transform_timeout_;
 
   /**
-   * @brief Holds the UTM pose that is used to compute the transform
+   * @brief Holds the Cartesian (UTM or local ENU) pose that is used to compute the transform
    */
-  tf2::Transform transform_utm_pose_;
+  tf2::Transform transform_cartesian_pose_;
 
   /**
    * @brief Latest IMU orientation
    */
   tf2::Transform transform_world_pose_;
 
+  /**
+   * @brief Whether we use a Local Cartesian (tangent plane ENU) or the UTM coordinates as our cartesian
+   */
+  bool use_local_cartesian_;
+
+  //! @brief Local Cartesian projection around gps origin
+  //!
+  GeographicLib::LocalCartesian gps_local_cartesian_;
+
   /**
    * @brief Whether we get our datum from the first GPS message or from the
    * set_datum service/parameter
@@ -370,32 +381,32 @@ class NavSatTransform : public rclcpp::Node
   bool use_odometry_yaw_;
 
   /**
-   * @brief Used for publishing the static world_frame->utm transform
+   * @brief Used for publishing the static world_frame->cartesian transform
    */
-  tf2_ros::StaticTransformBroadcaster utm_broadcaster_;
+  tf2_ros::StaticTransformBroadcaster cartesian_broadcaster_;
 
   /**
    * @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
+   * Angle between projected meridian (True North) and Cartesian's grid Y-axis.
+   * For Cartesian 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_meridian_convergence_;
 
   /**
-   * @brief Holds the UTM->odom transform
+   * @brief Holds the cartesian->odom transform
    */
-  tf2::Transform utm_world_transform_;
+  tf2::Transform cartesian_world_transform_;
 
   /**
-   * @brief Holds the odom->UTM transform for filtered GPS broadcast
+   * @brief Holds the odom->Cartesian transform for filtered GPS broadcast
    */
-  tf2::Transform utm_world_trans_inverse_;
+  tf2::Transform cartesian_world_trans_inverse_;
 
   /**
-   * @brief UTM zone as determined after transforming GPS message
+   * @brief Cartesian zone as determined after transforming GPS message
    */
   std::string utm_zone_;
 

include/robot_localization/ros_filter.hpp

@@ -525,6 +525,9 @@ class RosFilter : public rclcpp::Node
   //! @brief Whether the filter is enabled or not. See disabledAtStartup_.
   bool enabled_;
 
+  //! @brief Whether we'll allow old measurements to cause a re-publication of the updated state
+  bool permit_corrected_publication_;
+
   //! @brief The max (worst) dynamic diagnostic level.
   //!
   int dynamic_diag_error_level_;
@@ -595,6 +598,10 @@ class RosFilter : public rclcpp::Node
   //!
   rclcpp::Time last_diag_time_;
 
+  //! @brief The time of the most recent published state
+  //!
+  rclcpp::Time last_published_stamp_;
+
   //! @brief We process measurements by queueing them up in
   //! callbacks and processing them all at once within each
   //! iteration

package.xml

@@ -25,6 +25,7 @@
   <depend>geometry_msgs</depend>
   <depend>diagnostic_msgs</depend>
   <depend>diagnostic_updater</depend>
+  <depend>geographiclib</depend>
   <depend>nav_msgs</depend>
   <build_depend>rclcpp</build_depend>
   <build_depend>rmw_implementation</build_depend>

params/ekf.yaml

@@ -38,12 +38,15 @@ ekf_filter_node:
 # Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
         debug_out_file: /path/to/debug/file.txt
 
-# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
-        publish_tf: true
+# Whether we'll allow old measurements to cause a re-publication of the updated state
+        permit_corrected_publication: false
 
 # Whether to publish the acceleration state. Defaults to false if unspecified.
         publish_acceleration: false
 
+# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
+        publish_tf: true
+
 # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
 # earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
 # The robot's position in the odom frame will drift over time, but is accurate in the short term and should be

params/ukf.yaml

@@ -38,6 +38,15 @@ ukf_filter_node:
 # Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
         debug_out_file: /path/to/debug/file.txt
 
+# Whether we'll allow old measurements to cause a re-publication of the updated state
+        permit_corrected_publication: false
+
+# Whether to publish the acceleration state. Defaults to false if unspecified.
+        publish_acceleration: false
+
+# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
+        publish_tf: true
+
 # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
 # earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
 # The robot's position in the odom frame will drift over time, but is accurate in the short term and should be

src/ekf.cpp

@@ -366,7 +366,7 @@ void Ekf::predict(
     (x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) *
     one_half_at_squared;
   double dFY_dP =
-    (sr * tp * cpi * pitch_vel - cr * tp * cpi * yaw_vel) * delta_sec;
+    (sr * tp * cpi * pitch_vel + cr * tp * cpi * yaw_vel) * delta_sec;
 
   // Much of the transfer function Jacobian is identical to the transfer
   // function