ROS1 recent feature port (cra-ros-pkg#613)

* navsat_transform diagram to address cra-ros-pkg#550 (cra-ros-pkg#570) * add diagram for navsat_transform Signed-off-by: Mabel Zhang <mabel@openrobotics.org> * add diagram to tutorial Signed-off-by: Mabel Zhang <mabel@openrobotics.org> * Fix frame id of imu in differential mode, closes cra-ros-pkg#482 * Added local Cartesian option to navsat_transform * Fix navsat_transform issue with starting on uneven terrain * Fix typo in navsat_transform_node.rst (cra-ros-pkg#588) "prodives" -> "provides" * Fix sign error in dFY_dP in transfer function jacobian * Making repeated state publication optional (cra-ros-pkg#595) * PR feedback * Fixing build issues * Fixing stamp issues * Linting and uncrustifying Co-authored-by: Mabel Zhang <mabel.m.zhang@gmail.com> Co-authored-by: Jeffrey Kane Johnson <jeff@mapless.ai>
nobleo · Apr 13, 2021 · b721844 · b721844
1 parent d27e8cb
commit b721844
Show file tree

Hide file tree

Showing 15 changed files with 385 additions and 186 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -32,6 +32,10 @@ find_package(Eigen3 REQUIRED)
 find_package(Boost REQUIRED)
 find_package(yaml_cpp_vendor 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)
+
 set(library_name rl_lib)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
@@ -93,6 +97,7 @@ add_executable(
 
 target_link_libraries(
   ${library_name}
+  ${GeographicLib_LIBRARIES}
   ${EIGEN3_LIBRARIES}
 )
 

diff --git a/doc/images/figure1.png → doc/images/figure2.png b/doc/images/figure1.png → doc/images/figure2.png
diff --git a/doc/images/navsat_transform_workflow.png b/doc/images/navsat_transform_workflow.png
diff --git a/doc/images/navsat_transform_workflow.tex b/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}
diff --git a/doc/integrating_gps.rst b/doc/integrating_gps.rst
@@ -11,17 +11,25 @@ Notes on Fusing GPS Data
 Before beginning this tutorial, users should be sure to familiarize themselves with `REP-105 <http://www.ros.org/reps/rep-0105.html>`_. It is important for users to realize that using a position estimate that includes GPS data will likely be unfit for use by navigation modules, owing to the fact that GPS data is subject to discrete discontinuities ("jumps"). If you want to fuse data from a GPS into your position estimate, one potential solution is to do the following:
 
 1. Run one instance of a ``robot_localization`` state estimation node that fuses only continuous data, such as odometry and IMU data. Set the ``world_frame`` parameter for this instance to the same value as the ``odom_frame`` parameter. Execute local path plans and motions in this frame.
-2. Run another instance of a ``robot_localization`` state estimation node that fuses all sources of data, including the GPS. Set the ``world_frame`` parameter for this instance to the same value as the ``map_frame`` parameter. 
+2. Run another instance of a ``robot_localization`` state estimation node that fuses all sources of data, including the GPS. Set the ``world_frame`` parameter for this instance to the same value as the ``map_frame`` parameter.
 
-This is just a suggestion, however, and users are free to fuse the GPS data into a single instance of a ``robot_localization`` state estimation node. 
+This is just a suggestion, however, and users are free to fuse the GPS data into a single instance of a ``robot_localization`` state estimation node.
 
 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
 ===============
 
-``navsat_transform_node`` requires three sources of information: the robot's current pose estimate in its world frame, an earth-referenced heading, and a geographic coordinate expressed as a latitude/longitude pair (with optional altitude). 
+``navsat_transform_node`` requires three sources of information: the robot's current pose estimate in its world frame, an earth-referenced heading, and a geographic coordinate expressed as a latitude/longitude pair (with optional altitude).
 
 These data can be obtained in three different ways:
 
@@ -31,7 +39,7 @@ These data can be obtained in three different ways:
  * A `sensor_msgs/Imu <http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html>`_ message with an absolute (earth-referenced) heading.
  * A `nav_msgs/Odometry <http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html>`_ message that contains the robot's current position estimate in the frame specified by its start location (typically the output of a ``robot_localization`` state estimation node).
 
-2. The datum (global frame origin) can be specified via the ``datum`` parameter. 
+2. The datum (global frame origin) can be specified via the ``datum`` parameter.
 
  .. note:: In order to use this mode, the ``wait_for_datum`` parameter must be set to *true*.
 
@@ -41,15 +49,15 @@ These data can be obtained in three different ways:
 
   <rosparam param="datum">[55.944904, -3.186693, 0.0, map, base_link]</rosparam>
 
- The parameter order is ``latitude`` in decimal degrees, ``longitude`` in decimal degrees, ``heading`` in radians) the ``frame_id`` of your robot's world frame (i.e., the value of the ``world_frame`` parameter in a ``robot_localization`` state estimation node), and the ``frame_id`` of your robot's body frame (i.e., the value of the ``base_link_frame`` parameter in a ``robot_localization`` state estimation node). When this mode is used, the robot assumes that your robot's world frame origin is at the specified latitude and longitude and with a heading of :math:`0` (east). 
+ The parameter order is ``latitude`` in decimal degrees, ``longitude`` in decimal degrees, ``heading`` in radians) the ``frame_id`` of your robot's world frame (i.e., the value of the ``world_frame`` parameter in a ``robot_localization`` state estimation node), and the ``frame_id`` of your robot's body frame (i.e., the value of the ``base_link_frame`` parameter in a ``robot_localization`` state estimation node). When this mode is used, the robot assumes that your robot's world frame origin is at the specified latitude and longitude and with a heading of :math:`0` (east).
 
-3. The datum can be set manually via the ``set_datum`` service and using the `robot_localization/SetDatum <http://docs.ros.org/api/robot_localization/html/srv/SetDatum.html>`_ service message. 
+3. The datum can be set manually via the ``set_datum`` service and using the `robot_localization/SetDatum <http://docs.ros.org/api/robot_localization/html/srv/SetDatum.html>`_ service message.
 
 
 GPS Data
 ^^^^^^^^
 
-Please note that all development of ``navsat_transform_node`` was done using a Garmin 18x GPS unit, so there may be intricacies of the data generated by other units that need to be handled. 
+Please note that all development of ``navsat_transform_node`` was done using a Garmin 18x GPS unit, so there may be intricacies of the data generated by other units that need to be handled.
 
 The excellent `nmea_navsat_driver <http://wiki.ros.org/nmea_navsat_driver>`_ package provides the required `sensor_msgs/NavSatFix <http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html>`_ message. Here is the ``nmea_navsat_driver`` launch file we'll use for this tutorial:
 
@@ -110,9 +118,9 @@ Integration with ``robot_localization`` is straightforward at this point. Simply
 
  <param name="odomN" value="/your_state_estimation_node_topic">
 
- <rosparam param="odomN_config">[true,  true,  false, 
-                                 false, false, false, 
-                                 false, false, false, 
+ <rosparam param="odomN_config">[true,  true,  false,
+                                 false, false, false,
+                                 false, false, false,
                                  false, false, false,
                                  false, false, false]</rosparam>
  <param name="odomN_differential" value="false"/>
@@ -129,29 +137,27 @@ 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: 
+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`
 
  :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_utm_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>`_.
-
-
diff --git a/doc/navsat_transform_node.rst b/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
 ^^^^^^^^^^^

diff --git a/doc/state_estimation_nodes.rst b/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
 -------------------