Skip to content

Commit b21a7e1

Browse files
Amronoschristophfroehlichsaikishor
authored
Add omni_wheel_drive_controller (#1535)
Co-authored-by: Christoph Fröhlich <[email protected]> Co-authored-by: Sai Kishor Kothakota <[email protected]>
1 parent 1321ae8 commit b21a7e1

17 files changed

+2427
-0
lines changed

‎doc/controllers_index.rst‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ Controllers for Wheeled Mobile Robots
2929

3030
Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst>
3131
Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst>
32+
Omni Wheel Drive Controller <../omni_wheel_drive_controller/doc/userdoc.rst>
3233
Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst>
3334
Tricycle Controller <../tricycle_controller/doc/userdoc.rst>
3435

‎doc/release_notes.rst‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,10 @@ joint_trajectory_controller
1616
"stretches the time" with which it progresses in the trajectory. Scaling can either be set
1717
manually or it can be synchronized with the hardware. See :ref:`jtc_speed_scaling` for details.
1818

19+
omni_wheel_drive_controller
20+
*********************************
21+
* 🚀 The omni_wheel_drive_controller was added 🎉 (`#1535 <https://github.com/ros-controls/ros2_controllers/pull/1535>`_).
22+
1923
pid_controller
2024
*******************************
2125
* The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior (`#1585 <https://github.com/ros-controls/ros2_controllers/pull/1585>`__).

‎omni_wheel_drive_controller/CMakeLists.txt‎

Lines changed: 92 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,92 @@
1+
cmake_minimum_required(VERSION 3.16)
2+
project(omni_wheel_drive_controller)
3+
4+
find_package(ros2_control_cmake REQUIRED)
5+
set_compiler_options()
6+
export_windows_symbols()
7+
8+
# Find dependencies
9+
set(THIS_PACKAGE_INCLUDE_DEPENDS
10+
ament_cmake
11+
controller_interface
12+
generate_parameter_library
13+
geometry_msgs
14+
hardware_interface
15+
nav_msgs
16+
pluginlib
17+
rclcpp
18+
rclcpp_lifecycle
19+
realtime_tools
20+
tf2
21+
tf2_msgs
22+
Eigen3
23+
)
24+
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
25+
find_package(${Dependency} REQUIRED)
26+
endforeach()
27+
28+
generate_parameter_library(
29+
${PROJECT_NAME}_parameters
30+
src/${PROJECT_NAME}_parameters.yaml
31+
)
32+
33+
add_library(${PROJECT_NAME} SHARED
34+
src/${PROJECT_NAME}.cpp
35+
src/odometry.cpp
36+
)
37+
target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
38+
target_include_directories(${PROJECT_NAME}
39+
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
40+
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>
41+
)
42+
target_link_libraries(${PROJECT_NAME} PUBLIC
43+
${PROJECT_NAME}_parameters
44+
Eigen3::Eigen
45+
controller_interface::controller_interface
46+
hardware_interface::hardware_interface
47+
pluginlib::pluginlib
48+
rclcpp::rclcpp
49+
rclcpp_lifecycle::rclcpp_lifecycle
50+
realtime_tools::realtime_tools
51+
tf2::tf2
52+
${geometry_msgs_TARGETS}
53+
${nav_msgs_TARGETS}
54+
${tf2_msgs_TARGETS}
55+
)
56+
pluginlib_export_plugin_description_file(controller_interface omni_wheel_drive_plugin.xml)
57+
58+
# Install
59+
install(
60+
DIRECTORY include/
61+
DESTINATION include/${PROJECT_NAME}/
62+
)
63+
install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_parameters
64+
EXPORT export_${PROJECT_NAME}
65+
RUNTIME DESTINATION bin
66+
ARCHIVE DESTINATION lib
67+
LIBRARY DESTINATION lib
68+
)
69+
70+
if(BUILD_TESTING)
71+
find_package(ament_cmake_gmock REQUIRED)
72+
find_package(controller_manager REQUIRED)
73+
find_package(ros2_control_test_assets REQUIRED)
74+
75+
ament_add_gmock(test_${PROJECT_NAME} test/test_${PROJECT_NAME}.cpp)
76+
target_link_libraries(test_${PROJECT_NAME}
77+
${PROJECT_NAME}
78+
)
79+
80+
add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
81+
ament_add_gmock(test_load_${PROJECT_NAME} test/test_load_${PROJECT_NAME}.cpp)
82+
find_package(ament_lint_auto REQUIRED)
83+
ament_lint_auto_find_test_dependencies()
84+
target_link_libraries(test_load_${PROJECT_NAME}
85+
controller_manager::controller_manager
86+
ros2_control_test_assets::ros2_control_test_assets
87+
)
88+
endif()
89+
90+
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
91+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
92+
ament_package()

‎omni_wheel_drive_controller/doc/userdoc.rst‎

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/omni_wheel_drive_controller/doc/userdoc.rst
2+
3+
.. _omni_wheel_drive_controller_userdoc:
4+
5+
omni_wheel_drive_controller
6+
=================================
7+
8+
Controller for mobile robots with omnidirectional drive.
9+
10+
Supports using three or more omni wheels spaced at an equal angle from each other in a circular formation.
11+
To better understand this, have a look at :ref:`mobile_robot_kinematics`.
12+
13+
The controller uses velocity input, i.e., stamped Twist messages where linear ``x``, ``y``, and angular ``z`` components are used.
14+
Values in other components are ignored.
15+
16+
Odometry is computed from hardware feedback and published.
17+
18+
Other features
19+
--------------
20+
21+
+ Realtime-safe implementation.
22+
+ Odometry publishing
23+
+ Automatic stop after command time-out
24+
25+
Description of controller's interfaces
26+
--------------------------------------
27+
28+
References (from a preceding controller)
29+
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
30+
31+
When controller is in chained mode, it exposes the following references which can be commanded by the preceding controller:
32+
33+
- ``<controller_name>/linear/x/velocity`` double, in m/s
34+
- ``<controller_name>/linear/y/velocity`` double, in m/s
35+
- ``<controller_name>/angular/z/velocity`` double, in rad/s
36+
37+
Together, these represent the body twist (which in unchained-mode would be obtained from ~/cmd_vel).
38+
39+
State interfaces
40+
,,,,,,,,,,,,,,,,
41+
42+
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.
43+
44+
Command interfaces
45+
,,,,,,,,,,,,,,,,,,,,,,
46+
47+
Joints' velocity (``hardware_interface::HW_IF_VELOCITY``) are used.
48+
49+
50+
ROS 2 Interfaces
51+
----------------
52+
53+
Subscribers
54+
,,,,,,,,,,,
55+
56+
~/cmd_vel [geometry_msgs/msg/TwistStamped]
57+
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.
58+
59+
Publishers
60+
,,,,,,,,,,
61+
~/odom [nav_msgs::msg::Odometry]
62+
This represents an estimate of the robot's position and velocity in free space.
63+
64+
/tf [tf2_msgs::msg::TFMessage]
65+
tf tree. Published only if ``enable_odom_tf=true``
66+
67+
68+
Parameters
69+
,,,,,,,,,,
70+
71+
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.
72+
73+
.. generate_parameter_library_details:: ../src/omni_wheel_drive_controller_parameters.yaml
74+
75+
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>`_:
76+
77+
.. literalinclude:: ../test/config/test_omni_wheel_drive_controller.yaml
78+
:language: yaml

‎omni_wheel_drive_controller/include/omni_wheel_drive_controller/odometry.hpp‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
// Copyright 2025 Aarav Gupta
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef OMNI_WHEEL_DRIVE_CONTROLLER__ODOMETRY_HPP_
16+
#define OMNI_WHEEL_DRIVE_CONTROLLER__ODOMETRY_HPP_
17+
18+
#include <Eigen/Dense>
19+
#include <vector>
20+
21+
#include "rclcpp/time.hpp"
22+
23+
namespace omni_wheel_drive_controller
24+
{
25+
class Odometry
26+
{
27+
public:
28+
Odometry();
29+
30+
bool updateFromPos(const std::vector<double> & wheels_pos, const rclcpp::Time & time);
31+
bool updateFromVel(const std::vector<double> & wheels_vel, const rclcpp::Time & time);
32+
bool updateOpenLoop(
33+
const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel,
34+
const rclcpp::Time & time);
35+
void resetOdometry();
36+
37+
double getX() const { return x_; }
38+
double getY() const { return y_; }
39+
double getHeading() const { return heading_; }
40+
double getLinearXVel() const { return linear_x_vel_; }
41+
double getLinearYVel() const { return linear_y_vel_; }
42+
double getAngularVel() const { return angular_vel_; }
43+
44+
void setParams(
45+
const double & robot_radius, const double & wheel_radius, const double & wheel_offset,
46+
const size_t & wheel_count);
47+
48+
private:
49+
Eigen::Vector3d compute_robot_velocity(const std::vector<double> & wheels_vel) const;
50+
void integrate(const double & dx, const double & dy, const double & dheading);
51+
52+
// Current timestamp:
53+
rclcpp::Time timestamp_;
54+
55+
// Current pose:
56+
double x_; // [m]
57+
double y_; // [m]
58+
double heading_; // [rads]
59+
60+
// Current velocity:
61+
double linear_x_vel_; // [m/s]
62+
double linear_y_vel_; // [m/s]
63+
double angular_vel_; // [rads/s]
64+
65+
// Robot kinematic parameters:
66+
double robot_radius_; // [m]
67+
double wheel_radius_; // [m]
68+
double wheel_offset_; // [rads]
69+
70+
// Previous wheel positions/states [rads]:
71+
std::vector<double> wheels_old_pos_;
72+
};
73+
74+
} // namespace omni_wheel_drive_controller
75+
76+
#endif // OMNI_WHEEL_DRIVE_CONTROLLER__ODOMETRY_HPP_

‎omni_wheel_drive_controller/include/omni_wheel_drive_controller/omni_wheel_drive_controller.hpp‎

Lines changed: 120 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,120 @@
1+
// Copyright 2025 Aarav Gupta
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef OMNI_WHEEL_DRIVE_CONTROLLER__OMNI_WHEEL_DRIVE_CONTROLLER_HPP_
16+
#define OMNI_WHEEL_DRIVE_CONTROLLER__OMNI_WHEEL_DRIVE_CONTROLLER_HPP_
17+
18+
#include <memory>
19+
#include <string>
20+
#include <vector>
21+
22+
#include "controller_interface/chainable_controller_interface.hpp"
23+
#include "geometry_msgs/msg/twist_stamped.hpp"
24+
#include "nav_msgs/msg/odometry.hpp"
25+
#include "omni_wheel_drive_controller/odometry.hpp"
26+
#include "realtime_tools/realtime_publisher.hpp"
27+
#include "realtime_tools/realtime_thread_safe_box.hpp"
28+
#include "tf2_msgs/msg/tf_message.hpp"
29+
30+
// auto-generated by generate_parameter_library
31+
#include "omni_wheel_drive_controller/omni_wheel_drive_controller_parameters.hpp"
32+
33+
namespace omni_wheel_drive_controller
34+
{
35+
class OmniWheelDriveController : public controller_interface::ChainableControllerInterface
36+
{
37+
using TwistStamped = geometry_msgs::msg::TwistStamped;
38+
39+
public:
40+
OmniWheelDriveController();
41+
42+
controller_interface::CallbackReturn on_init() override;
43+
44+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
45+
46+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
47+
48+
controller_interface::CallbackReturn on_configure(
49+
const rclcpp_lifecycle::State & previous_state) override;
50+
51+
controller_interface::CallbackReturn on_activate(
52+
const rclcpp_lifecycle::State & previous_state) override;
53+
54+
controller_interface::CallbackReturn on_deactivate(
55+
const rclcpp_lifecycle::State & previous_state) override;
56+
57+
controller_interface::return_type update_reference_from_subscribers(
58+
const rclcpp::Time & time, const rclcpp::Duration & period) override;
59+
60+
controller_interface::return_type update_and_write_commands(
61+
const rclcpp::Time & time, const rclcpp::Duration & period) override;
62+
63+
controller_interface::CallbackReturn on_cleanup(
64+
const rclcpp_lifecycle::State & previous_state) override;
65+
66+
controller_interface::CallbackReturn on_error(
67+
const rclcpp_lifecycle::State & previous_state) override;
68+
69+
protected:
70+
std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
71+
72+
// Parameters from ROS for omni_wheel_drive_controller
73+
std::shared_ptr<ParamListener> param_listener_;
74+
Params params_;
75+
76+
struct WheelHandle
77+
{
78+
std::optional<std::reference_wrapper<const hardware_interface::LoanedStateInterface>> feedback;
79+
std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity;
80+
};
81+
std::vector<WheelHandle> registered_wheel_handles_;
82+
83+
controller_interface::CallbackReturn configure_wheel_handles(
84+
const std::vector<std::string> & wheel_names, std::vector<WheelHandle> & registered_handles);
85+
86+
// Timeout to consider cmd_vel commands old
87+
rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5);
88+
89+
std::atomic_bool subscriber_is_active_ = false;
90+
rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
91+
// Realtime container to exchange the reference from subscriber
92+
realtime_tools::RealtimeThreadSafeBox<TwistStamped> received_velocity_msg_;
93+
// Save the last reference in case of unable to get value from box
94+
TwistStamped command_msg_;
95+
96+
std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odometry_publisher_ = nullptr;
97+
std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
98+
realtime_odometry_publisher_;
99+
nav_msgs::msg::Odometry odometry_message_;
100+
101+
Odometry odometry_;
102+
103+
std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> odometry_transform_publisher_ =
104+
nullptr;
105+
std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
106+
realtime_odometry_transform_publisher_;
107+
geometry_msgs::msg::TransformStamped transform_;
108+
109+
void compute_and_set_wheel_velocities();
110+
const char * feedback_type() const;
111+
void halt();
112+
bool reset();
113+
bool on_set_chained_mode(bool chained_mode) override;
114+
115+
private:
116+
void reset_buffers();
117+
};
118+
} // namespace omni_wheel_drive_controller
119+
120+
#endif // OMNI_WHEEL_DRIVE_CONTROLLER__OMNI_WHEEL_DRIVE_CONTROLLER_HPP_

‎omni_wheel_drive_controller/omni_wheel_drive_plugin.xml‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<library path="omni_wheel_drive_controller">
2+
<class name="omni_wheel_drive_controller/OmniWheelDriveController"
3+
type="omni_wheel_drive_controller::OmniWheelDriveController"
4+
base_class_type="controller_interface::ChainableControllerInterface">
5+
<description>
6+
A chainable controller for mobile robots with omnidirectional drive with three or more omni wheels.
7+
</description>
8+
</class>
9+
</library>

0 commit comments

Comments
 (0)