Port code to mrpt3

CMakeLists.txt

@@ -98,7 +98,6 @@ if (DETECTED_ROS2)
 
   find_package(rclcpp REQUIRED)
 
-  # *** TEMPORARY ***!!!
   find_package(ament_cmake REQUIRED)
   find_package(geometry_msgs REQUIRED)
   find_package(nav_msgs REQUIRED)
@@ -193,15 +192,10 @@ endif()
 # Dependency: MRPT
 # Use the first cmd to set the minimum required version
 # ----------------------------------------------------------
-find_package(mrpt-maps  REQUIRED)
-find_package(mrpt-tclap  REQUIRED)
-find_package(mrpt-gui  REQUIRED)
-find_package(mrpt-tfest  REQUIRED)
-find_package(mrpt-topography REQUIRED)
-
-if (mrpt-maps_VERSION VERSION_LESS "2.14.1")
-    message(FATAL_ERROR "MRPT version 2.14.1 or newer is required, but found ${mrpt-maps_VERSION}")
-endif()
+find_package(mrpt_maps  REQUIRED)
+find_package(mrpt_gui  REQUIRED)
+find_package(mrpt_tfest  REQUIRED)
+find_package(mrpt_topography REQUIRED)
 
 # --------------------------
 # Dependency: Box2D

definitions/jackal.vehicle.xml

@@ -30,8 +30,8 @@
         <!--   Motor controller -->
         <controller class="${controller|twist_ideal}">  <!-- "twist_ideal" or "twist_pid" -->
           <!-- Optimal parameters from the mvsim-pid-tuner tool -->
-          <KP>20</KP>
-          <KI>10</KI>
+          <KP>18.2909</KP>
+          <KI>474.4889</KI>
           <KD>0.0000</KD>
           <max_torque>100</max_torque>
         </controller>

docs/mvsim-pid-tuner.rst

@@ -4,8 +4,9 @@ mvsim-pid-tuner
 ===============
 
 ``mvsim-pid-tuner`` is a command-line tool for automatic PID parameter tuning of MVSim
-vehicle controllers. It identifies the vehicle's dynamic response and proposes optimal
-PID gains using the IMC (Internal Model Control) tuning method.
+vehicle controllers. It identifies the vehicle's dynamic response (both linear and
+rotational) and proposes optimal PID gains using the IMC (Internal Model Control)
+tuning method.
 
 .. contents::
    :depth: 1
@@ -18,27 +19,38 @@ Overview
 Tuning PID controllers for simulated vehicles can be tedious. This tool automates the
 process by:
 
-1. Running an **open-loop step response** test (constant torque applied to wheels)
-2. **Identifying the plant model** as a first-order system: :math:`G(s) = K / (\tau s + 1)`
-3. Computing **optimal PID parameters** using IMC tuning rules
-4. **Validating** the proposed parameters with a closed-loop step-up/step-down simulation
+1. Running **open-loop step response** tests for both linear and rotational motion
+2. **Identifying the plant models** as first-order systems: :math:`G(s) = K / (\tau s + 1)`
+3. Computing **optimal PID parameters** using IMC tuning rules, balanced for both modes
+4. **Validating** the proposed parameters with closed-loop step-up/step-down simulations
+   for both linear velocity and angular velocity
 
 Algorithm
 ---------
 
-**Phase 1: Plant Identification**
+**Phase 1a: Linear Plant Identification**
 
-A constant torque is applied to all wheels (via a ``raw`` controller) and the average
-wheel velocity is recorded over time. The steady-state velocity :math:`v_{ss}` is
-computed from the last 20% of samples, and the plant gain and time constant are:
+A constant torque is applied equally to all wheels (via a ``raw`` controller) and the
+average wheel velocity is recorded over time. The steady-state velocity :math:`v_{ss}`
+is computed from the last 20% of samples, and the plant gain and time constant are:
 
 .. math::
 
-   K = \frac{v_{ss}}{\tau_{applied}}, \quad \tau = \text{time to reach } 63.2\% \text{ of } v_{ss}
+   K_{lin} = \frac{v_{ss}}{\tau_{applied}}, \quad \tau_{lin} = \text{time to reach } 63.2\% \text{ of } v_{ss}
+
+**Phase 1b: Rotation Plant Identification**
+
+Opposite torques are applied to left and right wheels (left forward, right backward)
+to induce pure rotation. The angular velocity :math:`\omega` is recorded from the
+vehicle's odometry estimate. The rotational plant model is identified analogously:
+
+.. math::
+
+   K_{rot} = \frac{\omega_{ss}}{\tau_{applied}}, \quad \tau_{rot} = \text{time to reach } 63.2\% \text{ of } \omega_{ss}
 
 **Phase 2: IMC PID Tuning**
 
-For a first-order plant, the IMC method yields a PI controller:
+For each first-order plant (linear and rotational), the IMC method yields a PI controller:
 
 .. math::
 
@@ -47,12 +59,20 @@ For a first-order plant, the IMC method yields a PI controller:
 where :math:`\tau_{cl} = \tau \cdot \alpha` is the desired closed-loop time constant and
 :math:`\alpha` is the aggressiveness factor (smaller = faster response).
 
+Since the ``twist_pid`` controller uses the same PID gains for both wheels, the tool
+computes PID parameters for both linear and rotational plants independently, then uses
+the **geometric mean** to balance performance across both modes.
+
 The ``max_torque`` is set to 80% of the estimated friction limit to avoid wheel slip.
 
 **Phase 3: Closed-Loop Validation**
 
-The proposed PID parameters are tested with a step-up to 1.0 m/s (3 seconds) followed
-by a step-down to 0 m/s (3 seconds). The tool reports:
+The proposed PID parameters are validated with two separate tests:
+
+* **Linear test**: step-up to 1.0 m/s (3 seconds) followed by step-down to 0 m/s (3 seconds)
+* **Rotation test**: step-up to 1.0 rad/s (3 seconds) followed by step-down to 0 rad/s (3 seconds)
+
+For each test, the tool reports:
 
 * Rise time (time to 90% of setpoint)
 * Settling time (time to enter 2% band)
@@ -111,41 +131,65 @@ Sample output:
     MVSim PID Auto-Tuner
    ======================================
 
-   --- Phase 1: Open-loop plant identification ---
+   --- Phase 1a: Open-loop LINEAR plant identification ---
    Vehicle: small_robot (2 wheels)
      Wheel 0: pos=(0.000, 0.500) R=0.200 mass=4.00
      Wheel 1: pos=(0.000, -0.500) R=0.200 mass=4.00
      Chassis mass: 15.0 kg
+     Wheel track: 1.000 m
      Estimated max friction torque per wheel: 18.10 Nm (mu=0.8)
-     Test torque: 9.05 Nm
+     Test torque: 9.05 Nm (linear)
 
-   === Open-Loop Step Response Analysis ===
+   === Open-Loop Step Response Analysis (linear) ===
    Applied torque:        9.050 Nm
    Steady-state velocity: 1.2345 m/s
    Plant gain K:          0.1364 (m/s)/Nm
-   Time constant τ:       0.1500 s
+   Time constant tau:     0.1500 s
+
+   --- Phase 1b: Open-loop ROTATION plant identification ---
+     Test torque: 9.05 Nm (rotation)
+
+   === Open-Loop Step Response Analysis (rotation) ===
+   Applied torque:        9.050 Nm
+   Steady-state omega:    2.5000 rad/s
+   Plant gain K:          0.2762 (rad/s)/Nm
+   Time constant tau:     0.0800 s
 
    --- Phase 2: IMC PID tuning (aggressiveness=0.25) ---
 
-   Proposed PID parameters:
-     KP:         29.3200
-     KI:         195.4667
+     Linear-only PID:  KP=29.3200  KI=195.4667
+     Rotation-only PID: KP=14.4800  KI=181.0000
+
+   Proposed PID parameters (balanced linear + rotation):
+     KP:         20.6000
+     KI:         188.0000
      KD:         0.0000
      max_torque: 14.48 Nm
 
    --- Phase 3: Closed-loop validation ---
+
+   [Linear] Step-up (1.0 m/s, 3s) then step-down (stop, 3s)...
    Step-up (0 -> 1.0 m/s):
+     Rise time (90%):       0.150 s
+     Settling time (2%):    0.300 s
+     Overshoot:             3.2 %
+     Steady-state error:    0.0015 m/s
+   Step-down (1.0 -> 0 m/s):
+     Settling time (2%):    0.350 s
+     Rebound:               none
+
+   [Rotation] Step-up (1.0 rad/s, 3s) then step-down (stop, 3s)...
+   Step-up (0 -> 1.0 rad/s):
      Rise time (90%):       0.120 s
      Settling time (2%):    0.250 s
-     Overshoot:             2.5 %
-     Steady-state error:    0.0012 m/s
-
-   Step-down (1.0 -> 0 m/s):
+     Overshoot:             2.8 %
+     Steady-state error:    0.0020 rad/s
+   Step-down (1.0 -> 0 rad/s):
      Settling time (2%):    0.300 s
      Rebound:               none
 
    --- Assessment ---
-   All metrics look good!
+   All metrics look good for both linear and rotation!
 
 The tool prints an XML snippet with the proposed PID values that can be pasted directly
 into the vehicle's ``<controller>`` block.

docs/world_lighting.rst

@@ -4,23 +4,28 @@ Light and shadows configuration
 --------------------------------------------
 
 
-At present, the lighting model of ``mrpt-opengl`` defines
-**one directional light source** (i.e. placed at the infinity),
-with one color and three component intensities (ambient, diffuse, and specular).
-See `mrpt::opengl::TLightParameters <https://docs.mrpt.org/reference/latest/struct_mrpt_opengl_TLightParameters.html>`_
+The lighting model uses MRPT's multi-light rendering pipeline with
+**hemisphere ambient lighting**, a **directional light source** (placed at infinity)
+that casts shadows, and optional **point** and **spot** light sources.
+See `mrpt::viz::TLightParameters <https://docs.mrpt.org/reference/latest/struct_mrpt_opengl_TLightParameters.html>`_
 and `mrpt-opengl <https://docs.mrpt.org/reference/latest/group_mrpt_opengl_grp.html>`_ for further details.
 
-MVSim offers a number of parameters under the global ``<light> ... </light>`` tag
+MVSim offers a number of parameters under the global ``<lights> ... </lights>`` tag
 to tune the performance and appearance of lights and shadows (all are optional).
 
-Light control
-================
+Directional light
+===================
+
+The primary directional light (sun-like, infinitely far away, parallel rays) is always present.
+Its direction is controlled via azimuth and elevation angles, and it is the only light that casts shadows.
 
 - ``<light_color>#ffffff</light_color>``: The light color (see formatting for :ref:`%color <world_value_parsing>`).
 
-- ``<light_ambient>0.5</light_ambient>``: Ambient component of the light (0 to 1).
+- ``<light_diffuse>0.8</light_diffuse>``: Diffuse intensity of the directional light (0 to 1).
 
-- ``<light_azimuth>45.0</light_azimuth>`` and ``<light_elevation>70.0</light_elevation>``: 
+- ``<light_specular>0.6</light_specular>``: Specular intensity of the directional light (0 to 1).
+
+- ``<light_azimuth_deg>45.0</light_azimuth_deg>`` and ``<light_elevation_deg>70.0</light_elevation_deg>``:
   The orbit-like azimuth and elevation angles (in **degrees**) of the directional light source.
   For example, an elevation of ``90`` means a pure vertical (downwards) light.
 
@@ -29,12 +34,76 @@ Light control
    <video controls autoplay loop muted> <source src="https://mrpt.github.io/mvsim-models/anims/mvsim-docs-light-direction.mp4" type="video/mp4"> </video>
 
 
+Ambient lighting
+=================
+
+MVSim uses **hemisphere ambient lighting**: surfaces facing up receive the sky ambient color,
+surfaces facing down receive the ground ambient color, with smooth interpolation for
+surfaces at intermediate angles. This produces a much more natural look than flat ambient,
+especially for outdoor scenes.
+
+- ``<light_ambient>0.4</light_ambient>``: Overall ambient intensity scale (0 to 1).
+
+- ``<ambient_sky_color>#e0e8ff</ambient_sky_color>``: The ambient color for upward-facing surfaces.
+  Default is a slight blue tint (``#e0e8ff``), simulating sky light.
+
+- ``<ambient_ground_color>#403a30</ambient_ground_color>``: The ambient color for downward-facing surfaces.
+  Default is a dark warm brown (``#403a30``), simulating ground-bounce light.
+
+To get flat (non-hemisphere) ambient lighting, set both colors to the same value (e.g. ``#ffffff``).
+
+
+Point lights
+=============
+
+Point lights emit in all directions from a position in world coordinates.
+They do **not** cast shadows. Multiple point lights can be defined by adding
+``<point_light>`` child elements inside ``<lights>``.
+
+Each ``<point_light>`` supports the following child tags (all optional, shown with defaults):
+
+- ``<position>X Y Z</position>``: Position in world coordinates (default: ``0 0 3``).
+- ``<color>#ffffff</color>``: Light color in ``#RRGGBB[AA]`` format (default: white).
+- ``<diffuse>0.8</diffuse>``: Diffuse intensity (0 to 1).
+- ``<specular>0.5</specular>``: Specular intensity (0 to 1).
+- ``<attenuation_constant>1.0</attenuation_constant>``: Constant attenuation factor.
+- ``<attenuation_linear>0.09</attenuation_linear>``: Linear attenuation factor.
+- ``<attenuation_quadratic>0.032</attenuation_quadratic>``: Quadratic attenuation factor.
+
+The light intensity at distance *d* is: ``1 / (constant + linear*d + quadratic*d²)``.
+
+
+Spot lights
+============
+
+Spot lights emit in a cone from a position along a direction.
+They do **not** cast shadows. Multiple spot lights can be defined by adding
+``<spot_light>`` child elements inside ``<lights>``.
+
+Each ``<spot_light>`` supports the following child tags (all optional, shown with defaults):
+
+- ``<position>X Y Z</position>``: Position in world coordinates (default: ``0 0 3``).
+- ``<direction>X Y Z</direction>``: Direction vector the spot points towards (default: ``0 0 -1``).
+- ``<color>#ffffff</color>``: Light color in ``#RRGGBB[AA]`` format (default: white).
+- ``<diffuse>0.8</diffuse>``: Diffuse intensity (0 to 1).
+- ``<specular>0.5</specular>``: Specular intensity (0 to 1).
+- ``<inner_cutoff_deg>12.5</inner_cutoff_deg>``: Inner cone half-angle in degrees (full intensity).
+- ``<outer_cutoff_deg>17.5</outer_cutoff_deg>``: Outer cone half-angle in degrees (light fades to zero).
+- ``<attenuation_constant>1.0</attenuation_constant>``: Constant attenuation factor.
+- ``<attenuation_linear>0.09</attenuation_linear>``: Linear attenuation factor.
+- ``<attenuation_quadratic>0.032</attenuation_quadratic>``: Quadratic attenuation factor.
+
+.. note::
+
+   Up to 8 simultaneous light sources are supported (including the primary directional light).
+   This limit is defined by the shader pipeline.
+
 
 Shadows control
 ================
 
 - ``<enable_shadows>true</enable_shadows>``. Can be used to disable casting shadows (enabled by default).
-  Note that shadows may not be visible on certain ground objects, most notably, occupancy grid maps, so 
+  Note that shadows may not be visible on certain ground objects, most notably, occupancy grid maps, so
   shadows not being visible on grids is not a bug.
 
 .. raw:: html
@@ -47,7 +116,7 @@ Shadows control
 - ``<shadow_map_size>2048</shadow_map_size>``. The resolution of the `shadow map texture <https://en.wikipedia.org/wiki/Shadow_mapping>`_ in pixels.
   Larger values will give more well-defined shadow borders, at the cost of higher GPU usage. Smaller values may slightly improve speed.
 
-.. list-table:: 
+.. list-table::
 
     * - .. figure:: https://mrpt.github.io/mvsim-models/screenshots/shadow_map_size-512.png
 
@@ -67,8 +136,8 @@ Shadows control
   shadows, defined by ``light_clip_plane_min``. Ideally, the smaller the range between these two numbers, the more accurate shadows will be.
 
 
-- ``<shadow_bias>1e-5</shadow_bias>``, ``<shadow_bias_cam2frag>1e-5</shadow_bias_cam2frag>``, and 
-  ``<shadow_bias_normal>1e-4</shadow_bias_normal>`` are all ``mrpt-opengl`` heuristic parameters to 
+- ``<shadow_bias>1e-5</shadow_bias>``, ``<shadow_bias_cam2frag>1e-5</shadow_bias_cam2frag>``, and
+  ``<shadow_bias_normal>1e-4</shadow_bias_normal>`` are all ``mrpt-opengl`` heuristic parameters to
   fight the "Peter-Panning" and "shadow acne" artifacts in shadow casting [Microsoft_Shadows]_.
 
 - ``<eye_distance_to_shadow_map_extension>2.0</eye_distance_to_shadow_map_extension>``: This unitless (meter/meter)
@@ -83,18 +152,46 @@ Shadows control
 
 
 .. code-block:: xml
-   :caption: Light and shadow tuning parameters: example
+   :caption: Complete lighting configuration example
 
    <mvsim_world version="1.0">
     ...
-    <!-- light and shadow options -->
-    <light>
-      <enable_shadows>true</enable_shadows>
-      <!-- <shadow_map_size>1024</shadow_map_size> -->
-
+    <lights>
+      <!-- Primary directional light (sun) -->
       <light_azimuth_deg>160.0</light_azimuth_deg>
       <light_elevation_deg>40.0</light_elevation_deg>
-    </light>
+      <light_color>#ffffff</light_color>
+      <light_diffuse>0.8</light_diffuse>
+      <light_specular>0.6</light_specular>
+
+      <!-- Hemisphere ambient lighting -->
+      <light_ambient>0.4</light_ambient>
+      <ambient_sky_color>#e0e8ff</ambient_sky_color>
+      <ambient_ground_color>#403a30</ambient_ground_color>
+
+      <!-- Shadow settings -->
+      <enable_shadows>true</enable_shadows>
+      <!-- <shadow_map_size>4096</shadow_map_size> -->
+
+      <!-- Point light: overhead lamp -->
+      <point_light>
+        <position>5.0 3.0 4.0</position>
+        <color>#ffe0a0</color>
+        <diffuse>0.6</diffuse>
+        <specular>0.3</specular>
+        <attenuation_quadratic>0.05</attenuation_quadratic>
+      </point_light>
+
+      <!-- Spot light: ceiling spotlight -->
+      <spot_light>
+        <position>0.0 0.0 5.0</position>
+        <direction>0.0 0.0 -1.0</direction>
+        <color>#ffffff</color>
+        <diffuse>0.7</diffuse>
+        <inner_cutoff_deg>15.0</inner_cutoff_deg>
+        <outer_cutoff_deg>25.0</outer_cutoff_deg>
+      </spot_light>
+    </lights>
     ...
    </mvsim_world>