Added Swerve Drive Controller Package

doc/controllers_index.rst

@@ -31,6 +31,7 @@ Controllers for Wheeled Mobile Robots
    Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst>
    Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst>
    Tricycle Controller <../tricycle_controller/doc/userdoc.rst>
+   Swerve Drive Controller <../swerve_drive_controller/doc/userdoc.rst>
 
 Controllers for Manipulators and Other Robots
 *********************************************

doc/mobile_robot_kinematics.rst

@@ -109,6 +109,98 @@ The body twist of the robot can be obtained from the wheel velocities by using t
     \omega_n
   \end{bmatrix}
 
+
+
+Swerve Drive Robots
+,,,,,,,,,,,,,,,,,,,
+
+The below explains the kinematics of omnidirectional drive robots using four swerve modules, each with independently controlled steering and driving motors. It follows the coordinate conventions defined in `ROS REP 103 <https://www.ros.org/reps/rep-0103.html>`__.
+
+.. image:: images/swerve_drive.svg
+   :width: 550
+   :align: center
+   :alt: Swerve Drive Robot
+
+* :math:`x_b, y_b` is the robot's body-frame coordinate system, located at the geometric center of the robot.
+* :math:`x_w, y_w` is the world coordinate system.
+* :math:`v_{b,x}` is the robot's linear velocity on the x-axis.
+* :math:`v_{b,y}` is the robot's linear velocity on the y-axis.
+* :math:`\omega_{b,z}` is the robot's angular velocity on the z-axis.
+* :math:`l` is the wheelbase (distance between front and rear wheels).
+* :math:`w` is the track width (distance between left and right wheels).
+* Red arrows on wheel :math:`i` signify the direction of the wheel's velocity :math:`v_i`.
+
+Each swerve module :math:`i` (for :math:`i = 0, 1, 2, 3`, typically front-left, front-right, back-left, back-right) is located at :math:`(l_{i,x}, l_{i,y})` relative to the center, typically:
+- Front-left: :math:`(l/2, w/2)`
+- Front-right: :math:`(l/2, -w/2)`
+- Back-left: :math:`(-l/2, w/2)`
+- Back-right: :math:`(-l/2, -w/2)`
+
+**Inverse Kinematics**
+
+The necessary wheel velocities and steering angles to achieve a desired body twist are computed by the `SwerveDriveKinematics` class in the `swerve_drive_controller` package. For each module :math:`i` at position :math:`(l_{i,x}, l_{i,y})`, the velocity vector is:
+
+.. math::
+
+   \begin{bmatrix}
+   v_{i,x} \\
+   v_{i,y}
+   \end{bmatrix}
+   =
+   \begin{bmatrix}
+   v_{b,x} - \omega_{b,z} l_{i,y} \\
+   v_{b,y} + \omega_{b,z} l_{i,x}
+   \end{bmatrix}
+
+The wheel velocity :math:`v_i` and steering angle :math:`\phi_i` are:
+
+.. math::
+
+   v_i = \sqrt{v_{i,x}^2 + v_{i,y}^2}
+
+.. math::
+
+   \phi_i = \arctan2(v_{i,y}, v_{i,x})
+
+
+**Forward Kinematics**
+
+The body twist of the robot is computed from the wheel velocities :math:`v_i` and steering angles :math:`\phi_i`. Each module’s velocity components in the body frame are:
+
+.. math::
+
+  v_{i,x} = v_i \cos(\phi_i), \quad v_{i,y} = v_i \sin(\phi_i)
+
+The chassis velocities are calculated as:
+
+.. math::
+
+  v_{b,x} = \frac{1}{4} \sum_{i=0}^{3} v_{i,x}, \quad v_{b,y} = \frac{1}{4} \sum_{i=0}^{3} v_{i,y}
+
+.. math::
+
+  \omega_{b,z} = \frac{\sum_{i=0}^{3} (v_{i,y} l_{i,x} - v_{i,x} l_{i,y})}{\sum_{i=0}^{3} (l_{i,x}^2 + l_{i,y}^2)}
+
+
+**Odometry**
+
+The `SwerveDriveKinematics` class updates the robot’s pose (:math:`x`, :math:`y`, :math:`\theta`) in the global frame using the computed chassis velocities. The global velocities are:
+
+.. math::
+
+   v_{x,\text{global}} = v_{b,x} \cos(\theta) - v_{b,y} \sin(\theta)
+
+.. math::
+
+   v_{y,\text{global}} = v_{b,x} \sin(\theta) + v_{b,y} \cos(\theta)
+
+The pose is updated via Euler integration over time step :math:`\Delta t`:
+
+.. math::
+
+   \dot{x} = v_{x,\text{global}}, \quad \dot{y} = v_{y,\text{global}}, \quad \dot{\theta} = \omega_{b,z}
+
+
 Nonholonomic Wheeled Mobile Robots
 .....................................
 

swerve_drive_controller/CMakeLists.txt

@@ -0,0 +1,101 @@
+cmake_minimum_required(VERSION 3.8)
+project(swerve_drive_controller)
+
+find_package(ros2_control_cmake REQUIRED)
+set_compiler_options()
+export_windows_symbols()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_interface
+  generate_parameter_library
+  geometry_msgs
+  hardware_interface
+  nav_msgs
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  rcpputils
+  realtime_tools
+  tf2
+  tf2_msgs
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR})
+add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR})
+
+generate_parameter_library(swerve_drive_controller_parameters
+  src/swerve_drive_controller_parameter.yaml
+)
+
+add_library(swerve_drive_controller SHARED
+  src/swerve_drive_controller.cpp
+  src/swerve_drive_kinematics.cpp
+)
+
+target_compile_features(swerve_drive_controller PUBLIC cxx_std_17)
+target_include_directories(swerve_drive_controller PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/swerve_drive_controller>
+)
+
+target_link_libraries(swerve_drive_controller
+  PUBLIC
+    swerve_drive_controller_parameters)
+ament_target_dependencies(swerve_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+pluginlib_export_plugin_description_file(controller_interface swerve_drive_plugin.xml)
+
+if(BUILD_TESTING)
+
+  find_package(controller_manager REQUIRED)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gtest(test_swerve_drive_controller
+    test/test_swerve_drive_controller.cpp
+  )
+
+  target_link_libraries(test_swerve_drive_controller
+    swerve_drive_controller
+  )
+  ament_target_dependencies(test_swerve_drive_controller
+    geometry_msgs
+    controller_interface
+    hardware_interface
+    nav_msgs
+    rclcpp
+    rclcpp_lifecycle
+    realtime_tools
+    tf2
+    tf2_msgs
+  )
+
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_swerve_drive_controller test/test_load_swerve_drive_controller.cpp)
+  ament_target_dependencies(test_load_swerve_drive_controller
+    controller_manager
+    ros2_control_test_assets
+  )
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include/swerve_drive_controller
+)
+
+install(TARGETS swerve_drive_controller swerve_drive_controller_parameters
+  EXPORT export_swerve_drive_controller
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+
+ament_export_libraries(swerve_drive_controller)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()