diff --git a/doc/images/figure1.png b/doc/images/figure2.png similarity index 100% rename from doc/images/figure1.png rename to doc/images/figure2.png diff --git a/doc/images/navsat_transform_workflow.png b/doc/images/navsat_transform_workflow.png new file mode 100644 index 000000000..450bcf2af Binary files /dev/null and b/doc/images/navsat_transform_workflow.png differ diff --git a/doc/images/navsat_transform_workflow.tex b/doc/images/navsat_transform_workflow.tex new file mode 100644 index 000000000..2dd0ab097 --- /dev/null +++ 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 index c50178481..65a785093 100644 --- a/doc/integrating_gps.rst +++ b/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 `_ 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 `_ 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 `_. 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`