Proposal for refactored steering and 6 wheeler steering.

doc/images/six_wheeler_steering.drawio

@@ -0,0 +1,164 @@
+<mxfile host="app.diagrams.net" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/138.0.0.0 Safari/537.36" version="28.0.6">
+  <diagram name="Page-1" id="vPOEDBpcrtD8xKoLgtPd">
+    <mxGraphModel dx="2850" dy="1153" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="1" shadow="0">
+      <root>
+        <mxCell id="0" />
+        <mxCell id="1" parent="0" />
+        <mxCell id="fpfmOAwSDF_yLItyLWh9-1" value="" style="rounded=1;whiteSpace=wrap;html=1;direction=south;rotation=15;" parent="1" vertex="1">
+          <mxGeometry x="240" y="760" width="60" height="120" as="geometry" />
+        </mxCell>
+        <mxCell id="fpfmOAwSDF_yLItyLWh9-3" value="" style="rounded=1;whiteSpace=wrap;html=1;direction=south;" parent="1" vertex="1">
+          <mxGeometry x="600" y="470" width="60" height="120" as="geometry" />
+        </mxCell>
+        <mxCell id="fpfmOAwSDF_yLItyLWh9-4" value="" style="rounded=1;whiteSpace=wrap;html=1;direction=south;rotation=15;" parent="1" vertex="1">
+          <mxGeometry x="500" y="760" width="60" height="120" as="geometry" />
+        </mxCell>
+        <mxCell id="fpfmOAwSDF_yLItyLWh9-5" value="" style="rounded=1;whiteSpace=wrap;html=1;direction=south;rotation=-15;" parent="1" vertex="1">
+          <mxGeometry x="500" y="108" width="60" height="120" as="geometry" />
+        </mxCell>
+        <mxCell id="fpfmOAwSDF_yLItyLWh9-6" value="" style="rounded=1;whiteSpace=wrap;html=1;direction=south;" parent="1" vertex="1">
+          <mxGeometry x="180" y="470" width="60" height="120" as="geometry" />
+        </mxCell>
+        <mxCell id="fpfmOAwSDF_yLItyLWh9-7" value="" style="rounded=1;whiteSpace=wrap;html=1;direction=south;rotation=-15;" parent="1" vertex="1">
+          <mxGeometry x="240" y="130" width="60" height="120" as="geometry" />
+        </mxCell>
+        <mxCell id="fpfmOAwSDF_yLItyLWh9-8" value="" style="rounded=0;whiteSpace=wrap;html=1;" parent="1" vertex="1">
+          <mxGeometry x="260" y="280" width="290" height="440" as="geometry" />
+        </mxCell>
+        <mxCell id="JtxlL-9a8NOayIRht31E-1" value="" style="endArrow=none;html=1;rounded=0;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="50" y="520" as="sourcePoint" />
+            <mxPoint x="450" y="520" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="JtxlL-9a8NOayIRht31E-4" value="" style="endArrow=none;html=1;rounded=0;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="50" y="520" as="sourcePoint" />
+            <mxPoint x="430" y="470" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="JtxlL-9a8NOayIRht31E-6" value="" style="endArrow=none;html=1;rounded=0;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="269.5" y="200" as="sourcePoint" />
+            <mxPoint x="269.5" y="40" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="JtxlL-9a8NOayIRht31E-7" value="" style="endArrow=none;html=1;rounded=0;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="260" y="970" as="sourcePoint" />
+            <mxPoint x="260" y="810" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="JtxlL-9a8NOayIRht31E-10" value="" style="endArrow=none;html=1;rounded=0;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="310" y="190" as="sourcePoint" />
+            <mxPoint x="270" y="40" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="JtxlL-9a8NOayIRht31E-19" value="" style="endArrow=none;dashed=1;html=1;dashPattern=1 3;strokeWidth=2;rounded=0;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="-330" y="520" as="sourcePoint" />
+            <mxPoint x="440" y="520" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="JtxlL-9a8NOayIRht31E-20" value="" style="endArrow=none;dashed=1;html=1;dashPattern=1 3;strokeWidth=2;rounded=0;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="-340" y="520" as="sourcePoint" />
+            <mxPoint x="270" y="200" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="JtxlL-9a8NOayIRht31E-21" value="" style="endArrow=none;dashed=1;html=1;dashPattern=1 3;strokeWidth=2;rounded=0;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="-340" y="520" as="sourcePoint" />
+            <mxPoint x="530" y="190" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="JtxlL-9a8NOayIRht31E-23" value="" style="endArrow=none;html=1;rounded=0;" parent="1" edge="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="260" y="970" as="sourcePoint" />
+            <mxPoint x="330" y="840" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-2" value="$$\sqrt{d_2^2+(r-d_1)^2}$$&lt;div&gt;&lt;br&gt;&lt;/div&gt;" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;convertToSvg=1;" vertex="1" parent="1">
+          <mxGeometry x="-20" y="360" width="210" height="60" as="geometry" />
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-4" value="&lt;div&gt;$$\sqrt{d_2^2+(r+d_1)^2}$$&lt;/div&gt;" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;convertToSvg=1;" vertex="1" parent="1">
+          <mxGeometry x="-180" y="310" width="210" height="60" as="geometry" />
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-5" value="" style="endArrow=classic;startArrow=classic;html=1;rounded=0;" edge="1" parent="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="750" y="850" as="sourcePoint" />
+            <mxPoint x="750" y="519.96" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-6" value="" style="endArrow=classic;startArrow=classic;html=1;rounded=0;" edge="1" parent="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="750" y="519.96" as="sourcePoint" />
+            <mxPoint x="750" y="160" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-7" value="" style="endArrow=classic;startArrow=classic;html=1;rounded=0;" edge="1" parent="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="260" y="990" as="sourcePoint" />
+            <mxPoint x="404.5" y="990" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-8" value="" style="endArrow=classic;startArrow=classic;html=1;rounded=0;" edge="1" parent="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="230" y="1040" as="sourcePoint" />
+            <mxPoint x="404.5" y="1040" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-11" value="D4" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" vertex="1" parent="1">
+          <mxGeometry x="290" y="1050" width="60" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-13" value="D1" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" vertex="1" parent="1">
+          <mxGeometry x="318.36" y="950" width="60" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-15" value="D2" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" vertex="1" parent="1">
+          <mxGeometry x="770" y="340" width="60" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-16" value="D3" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" vertex="1" parent="1">
+          <mxGeometry x="770" y="730" width="60" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-19" value="" style="curved=1;endArrow=classic;html=1;rounded=0;" edge="1" parent="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="400" y="520" as="sourcePoint" />
+            <mxPoint x="420" y="470" as="targetPoint" />
+            <Array as="points">
+              <mxPoint x="420" y="520" />
+              <mxPoint x="430" y="490" />
+            </Array>
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-21" value="" style="curved=1;endArrow=classic;html=1;rounded=0;" edge="1" parent="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="260" y="860" as="sourcePoint" />
+            <mxPoint x="300" y="890" as="targetPoint" />
+            <Array as="points">
+              <mxPoint x="300" y="850" />
+            </Array>
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-22" value="" style="curved=1;endArrow=classic;html=1;rounded=0;" edge="1" parent="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="270" y="180" as="sourcePoint" />
+            <mxPoint x="300" y="150" as="targetPoint" />
+            <Array as="points">
+              <mxPoint x="300" y="180" />
+            </Array>
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-24" value="" style="endArrow=classic;startArrow=classic;html=1;rounded=0;" edge="1" parent="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="-340" y="20" as="sourcePoint" />
+            <mxPoint x="425" y="20" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="UuVBM5KiAZeAReH1xgpa-25" value="R" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;strokeColor=default;fillColor=none;strokeWidth=0;" vertex="1" parent="1">
+          <mxGeometry x="-20" y="40" width="60" height="30" as="geometry" />
+        </mxCell>
+      </root>
+    </mxGraphModel>
+  </diagram>
+</mxfile>

doc/six_wheel_rover_controller.rst

@@ -0,0 +1,133 @@
+.. _doc_six_wheel_rover_controller:
+
+six_wheel_rover_controller
+=============================
+
+This page introduces the kinematic model for a six-wheeled rover featuring four-wheel steering, commonly used in rocker-bogie style vehicles.
+
+Kinematics of the system
+------------------------
+
+The controller uses a standard Ackermann-style kinematic model adapted for a six-wheel configuration. The core principle is that to avoid wheel slip during a turn, the axes of all wheels must intersect at a single point known as the **Instantaneous Center of Rotation (ICR)**.
+
+The parameters shown in the diagram are essential for the kinematic calculations.
+
+Diagram
+------------------
+.. image:: images/six_wheeler_steering.svg
+   :width: 600px
+   :align: center
+   :alt: Six-wheel steering controller kinematic diagram
+
+Inverse Kinematics
+------------------
+
+Inverse kinematics determine the required joint commands from a desired vehicle body velocity (a ``Twist`` message).
+
+**Steering Joint Angles**
+
+Given a desired turning radius, :math:`R`, the required steering angles for the inner and outer front wheels are calculated as follows:
+
+.. math::
+
+   \theta_{inner} = \arctan\left(\frac{d_3}{|R| - d_1}\right)
+
+.. math::
+
+   \theta_{outer} = \arctan\left(\frac{d_3}{|R| + d_1}\right)
+
+The rear wheels are commanded to the same angle magnitudes but in the opposite direction to articulate the turn.
+
+**Drive Wheel Velocities**
+
+The velocity of each drive wheel is proportional to its distance from the ICR. The vehicle's angular velocity, :math:`\omega_{center}`, is first determined from the desired linear velocity, :math:`v`:
+
+.. math::
+
+   \omega_{center} = \frac{v}{R}
+
+The linear velocity for each wheel (:math:`v_{wheel}`) is then found based on its unique turning radius. This is finally converted to the motor's angular velocity command, :math:`\omega_{wheel}`:
+
+.. math::
+
+   \omega_{wheel} = \frac{v_{wheel}}{r_{wheel}}
+
+Forward Kinematics
+------------------
+
+Forward kinematics estimate the vehicle's current ``Twist`` from its joint states. The rover's linear velocity, :math:`v_x`, is estimated from the average velocity of the two non-steerable middle wheels.
+
+.. math::
+
+   v_x = \frac{(\omega_{left\_middle} + \omega_{right\_middle})}{2} \cdot r_{wheel}
+
+The vehicle's angular velocity, :math:`\omega_z`, is then derived from this linear velocity and an estimation of the current turning radius, :math:`R_{approx}`.
+
+.. math::
+
+   \omega_z = \frac{v_x}{R_{approx}}
+
+Controller Parameters
+---------------------
+
+.. list-table:: Controller Parameters
+   :widths: 20 80
+   :header-rows: 1
+
+   * - Parameter
+     - Description
+   * - ``traction_joints``
+     - A list of the six drive wheel joint names from the robot's URDF model, in the following order: front-left, front-right, middle-left, middle-right, rear-left, rear-right.
+   * - ``steering_joints``
+     - A list of the four corner steering joint names from the robot's URDF model, in the following order: front-left, front-right, rear-left, rear-right.
+   * - ``d1``
+     - Half the front/rear track width (meters). Lateral distance from the longitudinal center to the steering pivot.
+   * - ``d2``
+     - Longitudinal distance from rover center to the rear axle (meters).
+   * - ``d3``
+     - Longitudinal distance from rover center to the front axle (meters).
+   * - ``d4``
+     - Half the middle track width (meters). Lateral distance from the center to the middle wheel's center.
+   * - ``wheel_radius``
+     - Radius of the drive wheels (meters).
+   * - ``odom_frame_id``
+     - The name of the odometry frame. Default: ``odom``.
+   * - ``base_frame_id``
+     - The name of the robot's base frame. Default: ``base_link``.
+
+Example Configuration
+---------------------
+
+.. code-block:: yaml
+
+   six_wheel_steering_controller:
+     ros__parameters:
+
+       # ---- JOINT CONFIGURATION ----
+       # [ACTION REQUIRED] Replace with the exact joint names from your robot's URDF.
+       traction_joints: [
+         "front_left_wheel_joint",
+         "middle_left_wheel_joint",
+         "rear_left_wheel_joint",
+         "front_right_wheel_joint",
+         "middle_right_wheel_joint",
+         "rear_right_wheel_joint"
+       ]
+       steering_joints: [
+         "front_left_steer_joint",
+         "rear_left_steer_joint",
+         "front_right_steer_joint",
+         "rear_right_steer_joint"
+       ]
+
+       # ---- KINEMATIC PARAMETERS (in meters) ----
+       # [ACTION REQUIRED] Measure these values from your specific rover.
+       d1: 0.4
+       d2: 0.5
+       d3: 0.5
+       d4: 0.45
+       wheel_radius: 0.15
+
+       # ---- ODOMETRY CONFIGURATION ----
+       odom_frame_id: "odom"
+       base_frame_id: "base_link"