MPC, Ackermann and aerial PlayGrounds, Bonxai planner and 3D goals

developer_guide/design.rst

@@ -478,6 +478,14 @@ and ``max_lateral_decel``. Not configured, they take the linear ones (``max_line
 ``max_linear_acc``, ``max_linear_decel``); ``max_lateral_vel: 0.0`` keeps the robot from moving
 sideways.
 
+Aerial robots also have ``max_vertical_vel`` (``linear.z``, either direction), ``max_vertical_acc``
+and ``max_vertical_decel``. ``max_vertical_vel`` is 0 by default: the robot does not move
+vertically, and ``linear.z`` is always 0.
+
+Robots that cannot turn in place, like cars with Ackermann steering, set ``min_turning_radius``
+(m). Every command then keeps ``|angular.z| <= |linear.x| / min_turning_radius``, also while the
+speed ramps up or down, so the turn follows the speed. It is 0 by default: no limit.
+
 Controller plugins read them with ``ControllerMethodBase::get_robot_limits()`` instead of declaring
 their own, and the smoother enforces the same limits on every command published. A controller's
 former limit parameters (e.g. ``max_linear_speed``) still work, with a deprecation warning, where

developer_guide/safety.rst

@@ -300,7 +300,11 @@ default: more than 10 ms):
   ``Finalized`` and ``system_main`` exits with code 1, since this cycle receives the sensors and
   publishes the commands. Outside safety mode it is only a ``WARN``, like a component that does not
   keep its own frequency, which is reported separately (see :ref:`component_frequencies`);
-- after an activation, the monitor starts over: the time EasyNav was inactive is not a late cycle.
+- after an activation, the monitor starts over: the time EasyNav was inactive is not a late cycle;
+- in the log, the first late cycle is a ``WARN``; after it, at most one ``WARN`` every 5 s counts
+  the late cycles since the previous one (e.g. "20 real-time cycles started late in the last 6 s,
+  the last one 30 ms after the previous one"). ``ERROR`` is always logged, and "on time again" only
+  when it ends.
 
 **Heartbeat.** With ``safety.heartbeat.period`` > 0 (required in safety mode), a
 ``easynav_interfaces/msg/Heartbeat`` is published on ``easynav_heartbeat`` at that period, **from the

playgrounds/ackermann.rst

@@ -0,0 +1,96 @@
+.. _playground_ackermann:
+
+====================
+Ackermann PlayGround
+====================
+
+`easynav_playground_ackermann <https://github.com/EasyNavigation/easynav_playgrounds/tree/rolling/playground_ackermann>`_
+simulates a **Toyota Prius**, a car with Ackermann steering, on the **Sonoma Raceway**. The car is
+localized with GPS, IMU and odometry, plans with A* over a NavMap of the track and follows the path
+with RPP or MPC.
+
+See :ref:`playgrounds` to install it.
+
+Launching EasyNav
+-----------------
+
+.. code-block:: bash
+
+   ros2 launch easynav_playground_ackermann easynav_rpp.launch.yaml   # Regulated Pure Pursuit
+   ros2 launch easynav_playground_ackermann easynav_mpc.launch.yaml   # MPC
+
+Each starts Gazebo, the Prius on the start line facing along the track, EasyNav and RViz2. Send
+goals on the track with the **2D Goal Pose** tool.
+
+.. list-table::
+   :header-rows: 1
+   :widths: 18 32 50
+
+   * - Configuration
+     - Parameters
+     - Controller
+   * - RPP
+     - ``params/rpp.params.yaml``
+     - Regulated Pure Pursuit
+   * - MPC
+     - ``params/mpc.params.yaml``
+     - MPC with ``motion_model: ackermann``: it keeps the curvature within the minimum turning
+       radius (4.2 m) and slows down before curves
+
+Both share the rest of the configuration:
+
+- **Localization:** ``FusionLocalizer`` fuses the GPS, the IMU and the wheel odometry. The fixes are
+  turned into map coordinates east-north-up at the world's origin (``projection: enu``, as Gazebo
+  defines its ``spherical_coordinates``), and corrected for the GPS antenna's mounting on the car.
+  There is no AMCL: a race track has few features for a lidar.
+- **Map:** the NavMap of the track (``maps/sonoma_track.navmap`` in
+  ``easynav_playground_ackermann_worlds``), with the walls as obstacles and a wide inflation, so
+  that the cheapest path runs along the middle of the track.
+- **Planner:** A* over that NavMap (``easynav_navmap_planner``).
+- **Limits:** the car's limits in ``robot_limits``, with ``min_turning_radius`` for its steering
+  (see :ref:`velocity_output`).
+
+Launch arguments
+~~~~~~~~~~~~~~~~
+
+.. list-table::
+   :header-rows: 1
+   :widths: 22 30 48
+
+   * - Argument
+     - Default
+     - Description
+   * - ``params_file``
+     - ``params/rpp.params.yaml`` (``mpc.params.yaml`` for MPC)
+     - EasyNav parameter file
+   * - ``rviz_config``
+     - ``rviz/easynav_navmap.rviz``
+     - RViz2 configuration: the map, the path, the lidar, the camera and a view that follows the car
+   * - ``world``
+     - ``worlds/sonoma_raceway.sdf``
+     - Gazebo world
+   * - ``gui``
+     - ``true``
+     - ``false`` runs Gazebo headless
+   * - ``rviz``
+     - ``true``
+     - ``false`` does not start RViz2
+
+Simulation only
+---------------
+
+To run Gazebo and the Prius, without EasyNav or RViz2:
+
+.. code-block:: bash
+
+   ros2 launch easynav_playground_ackermann_worlds gazebo_sim.launch.yaml
+
+The ``x``, ``y``, ``z`` and ``yaw`` arguments set where the car is spawned; ``prius.launch.yaml``
+spawns it in a running simulation. The car publishes a 3D lidar (``/top_laser/points``), a front
+camera, ``/imu/data``, ``/gps/fix`` and ``/ground_truth``, and listens on ``/cmd_vel``: a
+``twist_stamper`` forwards it to the Ackermann steering controller.
+
+The track map is built from the raceway's mesh with the tools in
+``easynav_playground_ackermann_worlds`` (see its README). The Sonoma Raceway world is by Juan
+Sebastián Cely Gutierrez (`easynav_wheels_worlds <https://github.com/juanscelyg/easynav_wheels_worlds>`_,
+GPL-3.0-only), and its model is Open Robotics' in Gazebo Fuel (CC0-1.0).

playgrounds/aerial.rst

@@ -0,0 +1,84 @@
+.. _playground_aerial:
+
+=================
+Aerial PlayGround
+=================
+
+`easynav_playground_aerial <https://github.com/EasyNavigation/easynav_playgrounds/tree/rolling/playground_aerial>`_
+simulates an **aerial robot**, a 1 m flying saucer (the UFO), in a **town**. The UFO moves in x, y,
+z and yaw. It plans 3D paths over a Bonxai map of the town, keeping its distance from buildings and
+the ground, and follows them with MPC.
+
+See :ref:`playgrounds` to install it.
+
+Launching EasyNav
+-----------------
+
+.. code-block:: bash
+
+   ros2 launch easynav_playground_aerial easynav_mpc.launch.yaml
+
+It starts Gazebo, the UFO hovering at 6 m in a street of the town, EasyNav and RViz2.
+
+Goals in the air
+~~~~~~~~~~~~~~~~
+
+RViz2's **2D Goal Pose** puts goals at z = 0, on the ground. To send goals in the air, use the
+**3D Goal Pose** tool of ``navmap_rviz_plugin`` (shortcut ``h``), already in the toolbar of this
+PlayGround's RViz2 configuration:
+
+- a left click places the goal on the line from the camera through the cursor, 10 m away by
+  default;
+- the mouse wheel changes that distance;
+- dragging sets the goal's yaw;
+- a line down to the ground, and a cross where it falls, show where the goal is.
+
+The tool can be added to any RViz2 configuration, for any robot that takes goals above the ground.
+
+Configuration
+~~~~~~~~~~~~~
+
+``params/mpc.params.yaml``:
+
+- **Localization:** ``FusionLocalizer`` in 3D, fusing the GPS (east-north-up at the world's
+  origin, ``projection: enu``), the IMU and the odometry.
+- **Map:** the Bonxai map of the town (``maps/nathan_benderson_town.pcd`` in
+  ``easynav_playground_aerial_worlds``), at 0.5 m.
+- **Planner:** ``easynav_bonxai_planner``. It treats buildings as solid blocks down to the ground
+  (``fill_below: highest``), keeps at least 2 m from them and prefers 5 m, and flies no higher than
+  70 m.
+- **Controller:** MPC with ``motion_model: aerial``. It follows the 3D path within the UFO's
+  horizontal and vertical limits (``robot_limits.max_vertical_vel`` and the rest, see
+  :ref:`velocity_output`).
+
+The launcher accepts ``params_file``, ``gui`` (``false`` runs Gazebo headless) and ``rviz``
+(``false`` does not start RViz2).
+
+Simulation only
+---------------
+
+To fly the UFO by hand, with Gazebo and RViz2 but without EasyNav:
+
+.. code-block:: bash
+
+   ros2 launch easynav_playground_aerial ufo_sim.launch.yaml
+
+Without RViz2 either:
+
+.. code-block:: bash
+
+   ros2 launch easynav_playground_aerial_worlds gazebo_sim.launch.yaml
+
+The ``x``, ``y``, ``z`` and ``yaw`` arguments set where the UFO is spawned; ``ufo.launch.yaml``
+spawns it in a running simulation. It publishes two 3D lidars (``/top_laser/points``,
+``/bottom_laser/points``), a front camera, ``/imu/data``, ``/gps/fix`` and ``/odom``, and listens on
+``/cmd_vel`` (body frame: linear x, y, z and angular z).
+
+The UFO has no gravity: a flight controller in ``easynav_playground_aerial_worlds`` turns the
+command into its velocity as a body with inertia would (first-order response, within its velocity,
+acceleration and deceleration limits), and brakes to hover without commands for 0.5 s.
+
+The town world is Juan Sebastián Cely Gutierrez's
+`disaster_world <https://github.com/juanscelyg/disaster_world>`_ (Apache-2.0), cut down to the
+ground, the roads and the buildings. How the world and the map were built is in the
+``easynav_playground_aerial_worlds`` README.

playgrounds/index.rst

@@ -56,6 +56,12 @@ robot's parameter file.
    * - :doc:`omni`
      - Three- to six-wheel omnidirectional robots in two mazes
      - Costmap with holonomic MPPI, in each heading mode
+   * - :doc:`ackermann`
+     - Toyota Prius (Ackermann steering) on the Sonoma Raceway
+     - NavMap with RPP or Ackermann MPC, GPS fusion
+   * - :doc:`aerial`
+     - A UFO (x, y, z, yaw) in a town
+     - Bonxai 3D planner, aerial MPC, GPS fusion, 3D goals
 
 Installation
 ------------
@@ -67,6 +73,13 @@ Install the PlayGround you want with APT in jazzy, kilted or lyrical, or with
 
    sudo apt install ros-<distro>-easynav-playground-kobuki
 
+.. only:: latest
+
+   .. note::
+
+      The Ackermann and Aerial PlayGrounds are new in this development version: until the next
+      release they are only available from source (below).
+
 To modify them, build them from source instead: clone the repository into your workspace, install
 their dependencies and build them (all, or ``--packages-up-to`` the one you want):
 
@@ -117,3 +130,5 @@ geometry and limits, and the map. See:
    summit
    tiago
    omni
+   ackermann
+   aerial

plugins/index.rst

@@ -77,6 +77,9 @@ ready-to-run configurations for each of them.
    * - Package
      - Description
      - Documentation
+   * - ``easynav_bonxai_planner``
+     - 3D A* over a Bonxai map for aerial robots, keeping a clearance from obstacles.
+     - `README <https://github.com/EasyNavigation/easynav_plugins/blob/rolling/planners/easynav_bonxai_planner/README.md>`__
    * - ``easynav_costmap_planner``
      - A* planner over ``Costmap2D``.
      - `README <https://github.com/EasyNavigation/easynav_plugins/blob/rolling/planners/easynav_costmap_planner/README.md>`__
@@ -107,7 +110,8 @@ ready-to-run configurations for each of them.
        for holonomic robots.
      - `README <https://github.com/EasyNavigation/easynav_plugins/blob/rolling/controllers/easynav_mppi_controller/README.md>`__
    * - ``easynav_mpc_controller``
-     - Model Predictive Controller (MPC) for trajectory tracking.
+     - Linear time-varying Model Predictive Controller (OSQP) for differential, omnidirectional,
+       Ackermann and aerial robots, with obstacles as constraints.
      - `README <https://github.com/EasyNavigation/easynav_plugins/blob/rolling/controllers/easynav_mpc_controller/README.md>`__
    * - ``easynav_serest_controller``
      - SeReST (Smooth Error-Responsive Speed and Turning) path tracking controller.
@@ -169,7 +173,8 @@ ready-to-run configurations for each of them.
      - AMCL for NavMap stacks, scoring 3D point clouds against a Bonxai map.
      - `README <https://github.com/EasyNavigation/easynav_plugins/blob/rolling/localizers/easynav_navmap_localizer/README.md>`__
    * - ``easynav_fusion_localizer``
-     - Multi-sensor fusion (UKF, based on ``robot_localization``): GPS, odometry, IMU.
+     - Multi-sensor fusion (UKF, based on ``robot_localization``): GPS, odometry, IMU. GPS fixes
+       in UTM or east-north-up maps, corrected for the antenna's mounting.
      - `README <https://github.com/EasyNavigation/easynav_plugins/blob/rolling/localizers/easynav_fusion_localizer/README.md>`__
    * - ``easynav_simple_localizer``
      - AMCL over ``SimpleMap``.