Albert/humble/omni

doc/controllers_index.rst

@@ -29,6 +29,7 @@ Controllers for Wheeled Mobile Robots
 
    Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst>
    Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst>
+   Omni Wheel Drive Controller <../omni_wheel_drive_controller/doc/userdoc.rst>
    Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst>
    Tricycle Controller <../tricycle_controller/doc/userdoc.rst>
 

doc/release_notes.rst

@@ -50,6 +50,10 @@ mecanum_drive_controller
 ************************
 * 🚀 The mecanum_drive_controller was added 🎉 (`#512 <https://github.com/ros-controls/ros2_controllers/pull/512>`_).
 
+omni_wheel_drive_controller
+*********************************
+* 🚀 The omni_wheel_drive_controller was added 🎉 (`#1535 <https://github.com/ros-controls/ros2_controllers/pull/1535>`_).
+
 pid_controller
 ************************
 * 🚀 The PID controller was added 🎉 (`#434 <https://github.com/ros-controls/ros2_controllers/pull/434>`_).

omni_wheel_drive_controller/CMakeLists.txt

@@ -0,0 +1,82 @@
+cmake_minimum_required(VERSION 3.16)
+project(omni_wheel_drive_controller LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra -Wconversion)
+endif()
+
+# Find dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_interface
+  generate_parameter_library
+  geometry_msgs
+  hardware_interface
+  nav_msgs
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  tf2
+  tf2_msgs
+  Eigen3
+)
+
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(
+  ${PROJECT_NAME}_parameters
+  src/${PROJECT_NAME}_parameters.yaml
+)
+
+add_library(${PROJECT_NAME} SHARED
+  src/${PROJECT_NAME}.cpp
+  src/odometry.cpp
+)
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+target_include_directories(${PROJECT_NAME}
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+target_link_libraries(${PROJECT_NAME} PUBLIC ${PROJECT_NAME}_parameters)
+ament_target_dependencies(${PROJECT_NAME} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+pluginlib_export_plugin_description_file(controller_interface omni_wheel_drive_plugin.xml)
+
+# Install
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}/
+)
+install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_parameters
+  EXPORT export_${PROJECT_NAME}
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+# if(BUILD_TESTING)
+#   find_package(ament_cmake_gmock REQUIRED)
+#   find_package(controller_manager REQUIRED)
+#   find_package(ros2_control_test_assets REQUIRED)
+
+#   ament_add_gmock(test_${PROJECT_NAME} test/test_${PROJECT_NAME}.cpp)
+#   target_link_libraries(test_${PROJECT_NAME}
+#     ${PROJECT_NAME}
+#   )
+
+#   add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+#   ament_add_gmock(test_load_${PROJECT_NAME} test/test_load_${PROJECT_NAME}.cpp)
+#   find_package(ament_lint_auto REQUIRED)
+#   ament_lint_auto_find_test_dependencies()
+#   target_link_libraries(test_load_${PROJECT_NAME}
+#     controller_manager::controller_manager
+#     ros2_control_test_assets::ros2_control_test_assets
+#   )
+# endif()
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

omni_wheel_drive_controller/doc/userdoc.rst

@@ -0,0 +1,78 @@
+:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/omni_wheel_drive_controller/doc/userdoc.rst
+
+.. _omni_wheel_drive_controller_userdoc:
+
+omni_wheel_drive_controller
+=================================
+
+Controller for mobile robots with omnidirectional drive.
+
+Supports using three or more omni wheels spaced at an equal angle from each other in a circular formation.
+To better understand this, have a look at :ref:`mobile_robot_kinematics`.
+
+The controller uses velocity input, i.e., stamped Twist messages where linear ``x``, ``y``, and angular ``z`` components are used.
+Values in other components are ignored.
+
+Odometry is computed from hardware feedback and published.
+
+Other features
+--------------
+
+   + Realtime-safe implementation.
+   + Odometry publishing
+   + Automatic stop after command time-out
+
+Description of controller's interfaces
+--------------------------------------
+
+References (from a preceding controller)
+,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
+
+When controller is in chained mode, it exposes the following references which can be commanded by the preceding controller:
+
+- ``<controller_name>/linear/x/velocity`` double, in m/s
+- ``<controller_name>/linear/y/velocity`` double, in m/s
+- ``<controller_name>/angular/z/velocity`` double, in rad/s
+
+Together, these represent the body twist (which in unchained-mode would be obtained from ~/cmd_vel).
+
+State interfaces
+,,,,,,,,,,,,,,,,
+
+As feedback interface type the joints' position (``hardware_interface::HW_IF_POSITION``) or velocity (``hardware_interface::HW_IF_VELOCITY``, if parameter ``position_feedback=false``) are used.
+
+Command interfaces
+,,,,,,,,,,,,,,,,,,,,,,
+
+Joints' velocity (``hardware_interface::HW_IF_VELOCITY``) are used.
+
+
+ROS 2 Interfaces
+----------------
+
+Subscribers
+,,,,,,,,,,,
+
+~/cmd_vel [geometry_msgs/msg/TwistStamped]
+  Velocity command for the controller. The controller extracts the x and y component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored.
+
+Publishers
+,,,,,,,,,,
+~/odom [nav_msgs::msg::Odometry]
+  This represents an estimate of the robot's position and velocity in free space.
+
+/tf [tf2_msgs::msg::TFMessage]
+  tf tree. Published only if ``enable_odom_tf=true``
+
+
+Parameters
+,,,,,,,,,,
+
+This controller uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to handle its parameters. The parameter `definition file located in the src folder <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/omni_wheel_drive_controller/src/multi_omni_wheel_drive_controller_parameters.yaml>`_ contains descriptions for all the parameters used by the controller.
+
+.. generate_parameter_library_details:: ../src/omni_wheel_drive_controller_parameters.yaml
+
+An example parameter file for this controller can be found in `the test directory <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/omni_wheel_drive_controller/test/config/test_multi_omni_wheel_drive_controller.yaml>`_:
+
+.. literalinclude:: ../test/config/test_omni_wheel_drive_controller.yaml
+   :language: yaml

omni_wheel_drive_controller/include/omni_wheel_drive_controller/odometry.hpp

@@ -0,0 +1,76 @@
+// Copyright 2025 Aarav Gupta
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef OMNI_WHEEL_DRIVE_CONTROLLER__ODOMETRY_HPP_
+#define OMNI_WHEEL_DRIVE_CONTROLLER__ODOMETRY_HPP_
+
+#include <Eigen/Dense>
+#include <vector>
+
+#include "rclcpp/time.hpp"
+
+namespace omni_wheel_drive_controller
+{
+class Odometry
+{
+public:
+  Odometry();
+
+  bool updateFromPos(const std::vector<double> & wheels_pos, const rclcpp::Time & time);
+  bool updateFromVel(const std::vector<double> & wheels_vel, const rclcpp::Time & time);
+  bool updateOpenLoop(
+    const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel,
+    const rclcpp::Time & time);
+  void resetOdometry();
+
+  double getX() const { return x_; }
+  double getY() const { return y_; }
+  double getHeading() const { return heading_; }
+  double getLinearXVel() const { return linear_x_vel_; }
+  double getLinearYVel() const { return linear_y_vel_; }
+  double getAngularVel() const { return angular_vel_; }
+
+  void setParams(
+    const double & robot_radius, const double & wheel_radius, const double & wheel_offset,
+    const size_t & wheel_count);
+
+private:
+  Eigen::Vector3d compute_robot_velocity(const std::vector<double> & wheels_vel) const;
+  void integrate(const double & dx, const double & dy, const double & dheading);
+
+  // Current timestamp:
+  rclcpp::Time timestamp_;
+
+  // Current pose:
+  double x_;        // [m]
+  double y_;        // [m]
+  double heading_;  // [rads]
+
+  // Current velocity:
+  double linear_x_vel_;  // [m/s]
+  double linear_y_vel_;  // [m/s]
+  double angular_vel_;   // [rads/s]
+
+  // Robot kinematic parameters:
+  double robot_radius_;  // [m]
+  double wheel_radius_;  // [m]
+  double wheel_offset_;  // [rads]
+
+  // Previous wheel positions/states [rads]:
+  std::vector<double> wheels_old_pos_;
+};
+
+}  // namespace omni_wheel_drive_controller
+
+#endif  // OMNI_WHEEL_DRIVE_CONTROLLER__ODOMETRY_HPP_

omni_wheel_drive_controller/include/omni_wheel_drive_controller/omni_wheel_drive_controller.hpp

@@ -0,0 +1,118 @@
+// Copyright 2025 Aarav Gupta
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef OMNI_WHEEL_DRIVE_CONTROLLER__OMNI_WHEEL_DRIVE_CONTROLLER_HPP_
+#define OMNI_WHEEL_DRIVE_CONTROLLER__OMNI_WHEEL_DRIVE_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "controller_interface/chainable_controller_interface.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "omni_wheel_drive_controller/odometry.hpp"
+#include "realtime_tools/realtime_publisher.hpp"
+#include "realtime_tools/realtime_box.hpp"
+#include "tf2_msgs/msg/tf_message.hpp"
+
+// auto-generated by generate_parameter_library
+#include "omni_wheel_drive_controller/omni_wheel_drive_controller_parameters.hpp"
+
+namespace omni_wheel_drive_controller
+{
+class OmniWheelDriveController : public controller_interface::ChainableControllerInterface
+{
+  using TwistStamped = geometry_msgs::msg::TwistStamped;
+
+public:
+  OmniWheelDriveController();
+
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::return_type update_reference_from_subscribers() override;
+
+  controller_interface::return_type update_and_write_commands(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  controller_interface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::CallbackReturn on_error(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+protected:
+  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
+
+  // Parameters from ROS for omni_wheel_drive_controller
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  struct WheelHandle
+  {
+    std::optional<std::reference_wrapper<const hardware_interface::LoanedStateInterface>> feedback;
+    std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity;
+  };
+  std::vector<WheelHandle> registered_wheel_handles_;
+
+  controller_interface::CallbackReturn configure_wheel_handles(
+    const std::vector<std::string> & wheel_names, std::vector<WheelHandle> & registered_handles);
+
+  // Timeout to consider cmd_vel commands old
+  rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5);
+
+  std::atomic_bool subscriber_is_active_ = false;
+  rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
+  // Realtime container to exchange the reference from subscriber
+  realtime_tools::RealtimeBox<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};
+  // Save the last reference in case of unable to get value from box
+  // TwistStamped command_msg_;
+
+  std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odometry_publisher_ = nullptr;
+  std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
+    realtime_odometry_publisher_;
+  nav_msgs::msg::Odometry odometry_message_;
+
+  Odometry odometry_;
+
+  std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> odometry_transform_publisher_ =
+    nullptr;
+  std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
+    realtime_odometry_transform_publisher_;
+
+  void compute_and_set_wheel_velocities();
+  const char * feedback_type() const;
+  void halt();
+  bool reset();
+  bool on_set_chained_mode(bool chained_mode) override;
+
+private:
+  void reset_buffers();
+};
+}  // namespace omni_wheel_drive_controller
+
+#endif  // OMNI_WHEEL_DRIVE_CONTROLLER__OMNI_WHEEL_DRIVE_CONTROLLER_HPP_

omni_wheel_drive_controller/omni_wheel_drive_plugin.xml

@@ -0,0 +1,9 @@
+<library path="omni_wheel_drive_controller">
+  <class name="omni_wheel_drive_controller/OmniWheelDriveController"
+    type="omni_wheel_drive_controller::OmniWheelDriveController"
+    base_class_type="controller_interface::ChainableControllerInterface">
+    <description>
+      A chainable controller for mobile robots with omnidirectional drive with three or more omni wheels.
+    </description>
+  </class>
+</library>