Skip to content

Commit 79358e3

Browse files
destogldestogl
authored
Add Mecanum Drive Controller (ros-controls#512)
1 parent 29e84b9 commit 79358e3

20 files changed

+2436
-1
lines changed

‎doc/controllers_index.rst‎

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

3030
Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst>
31+
Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst>
3132
Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst>
3233
Tricycle Controller <../tricycle_controller/doc/userdoc.rst>
3334

‎doc/release_notes.rst‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,10 @@ joint_trajectory_controller
5050
5151
* Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 <https://github.com/ros-controls/ros2_controllers/pull/1231>`_).
5252

53+
mecanum_drive_controller
54+
************************
55+
* 🚀 The mecanum_drive_controller was added 🎉 (`#512 <https://github.com/ros-controls/ros2_controllers/pull/512>`_).
56+
5357
pid_controller
5458
************************
5559
* 🚀 The PID controller was added 🎉 (`#434 <https://github.com/ros-controls/ros2_controllers/pull/434>`_).

‎mecanum_drive_controller/CMakeLists.txt‎

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
cmake_minimum_required(VERSION 3.16)
2+
project(mecanum_drive_controller LANGUAGES CXX)
3+
4+
if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
5+
add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
6+
-Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
7+
-Werror=missing-braces)
8+
endif()
9+
10+
# find dependencies
11+
set(THIS_PACKAGE_INCLUDE_DEPENDS
12+
controller_interface
13+
hardware_interface
14+
generate_parameter_library
15+
nav_msgs
16+
pluginlib
17+
rclcpp
18+
rclcpp_lifecycle
19+
realtime_tools
20+
std_srvs
21+
tf2
22+
tf2_geometry_msgs
23+
tf2_msgs
24+
)
25+
26+
find_package(ament_cmake REQUIRED)
27+
find_package(backward_ros REQUIRED)
28+
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
29+
find_package(${Dependency} REQUIRED)
30+
endforeach()
31+
32+
generate_parameter_library(mecanum_drive_controller_parameters
33+
src/mecanum_drive_controller.yaml
34+
)
35+
36+
add_library(
37+
mecanum_drive_controller
38+
SHARED
39+
src/mecanum_drive_controller.cpp
40+
src/odometry.cpp
41+
)
42+
target_compile_features(mecanum_drive_controller PUBLIC cxx_std_17)
43+
target_include_directories(mecanum_drive_controller PUBLIC
44+
"$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>"
45+
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
46+
target_link_libraries(mecanum_drive_controller PUBLIC
47+
mecanum_drive_controller_parameters)
48+
ament_target_dependencies(mecanum_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
49+
50+
# Causes the visibility macros to use dllexport rather than dllimport,
51+
# which is appropriate when building the dll but not consuming it.
52+
target_compile_definitions(mecanum_drive_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL")
53+
54+
pluginlib_export_plugin_description_file(
55+
controller_interface mecanum_drive_controller.xml)
56+
57+
if(BUILD_TESTING)
58+
find_package(ament_cmake_gmock REQUIRED)
59+
find_package(controller_manager REQUIRED)
60+
find_package(hardware_interface REQUIRED)
61+
find_package(ros2_control_test_assets REQUIRED)
62+
63+
64+
add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
65+
ament_add_gmock(test_load_mecanum_drive_controller test/test_load_mecanum_drive_controller.cpp)
66+
ament_target_dependencies(test_load_mecanum_drive_controller
67+
controller_manager
68+
hardware_interface
69+
ros2_control_test_assets
70+
)
71+
72+
add_rostest_with_parameters_gmock(
73+
test_mecanum_drive_controller test/test_mecanum_drive_controller.cpp
74+
${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_params.yaml)
75+
target_include_directories(test_mecanum_drive_controller PRIVATE include)
76+
target_link_libraries(test_mecanum_drive_controller mecanum_drive_controller)
77+
ament_target_dependencies(
78+
test_mecanum_drive_controller
79+
controller_interface
80+
hardware_interface
81+
)
82+
83+
add_rostest_with_parameters_gmock(
84+
test_mecanum_drive_controller_preceeding test/test_mecanum_drive_controller_preceeding.cpp
85+
${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_preceeding_params.yaml)
86+
target_include_directories(test_mecanum_drive_controller_preceeding PRIVATE include)
87+
target_link_libraries(test_mecanum_drive_controller_preceeding mecanum_drive_controller)
88+
ament_target_dependencies(
89+
test_mecanum_drive_controller_preceeding
90+
controller_interface
91+
hardware_interface
92+
)
93+
endif()
94+
95+
install(
96+
DIRECTORY include/
97+
DESTINATION include/mecanum_drive_controller
98+
)
99+
100+
install(
101+
TARGETS mecanum_drive_controller mecanum_drive_controller_parameters
102+
EXPORT export_mecanum_drive_controller
103+
RUNTIME DESTINATION bin
104+
ARCHIVE DESTINATION lib
105+
LIBRARY DESTINATION lib
106+
)
107+
108+
ament_export_targets(export_mecanum_drive_controller HAS_LIBRARY_TARGET)
109+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
110+
ament_package()

‎mecanum_drive_controller/doc/userdoc.rst‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
.. _mecanum_drive_controller_userdoc:
2+
3+
mecanum_drive_controller
4+
=========================
5+
6+
Library with shared functionalities for mobile robot controllers with mecanum drive (four wheels).
7+
The library implements generic odometry and update methods and defines the main interfaces.
8+
9+
Execution logic of the controller
10+
----------------------------------
11+
12+
The controller uses velocity input, i.e., stamped Twist messages where linear ``x``, ``y``, and angular ``z`` components are used.
13+
Values in other components are ignored.
14+
In the chain mode, the controller provides three reference interfaces, one for linear velocity and one for steering angle position.
15+
Other relevant features are:
16+
17+
- odometry publishing as Odometry and TF message;
18+
- input command timeout based on a parameter.
19+
20+
Note about odometry calculation:
21+
In the DiffDRiveController, the velocity is filtered out, but we prefer to return it raw and let the user perform post-processing at will.
22+
We prefer this way of doing so as filtering introduces delay (which makes it difficult to interpret and compare behavior curves).
23+
24+
25+
Description of controller's interfaces
26+
--------------------------------------
27+
28+
References (from a preceding controller)
29+
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
30+
- <reference_names[i]>/<interface_name> [double] # in [rad] or [rad/s]
31+
32+
Commands
33+
,,,,,,,,,
34+
- <command_joint_names[i]>/<interface_name> [double] # in [rad] or [rad/s]
35+
36+
States
37+
,,,,,,,
38+
- <joint_names[i]>/<interface_name> [double] # in [rad] or [rad/s]
39+
..note ::
40+
41+
``joint_names[i]`` can be of ``state_joint_names`` parameter (if used), ``command_joint_names`` otherwise.
42+
43+
44+
Subscribers
45+
,,,,,,,,,,,,
46+
Used when the controller is not in chained mode (``in_chained_mode == false``).
47+
48+
- <controller_name>/reference [geometry_msgs/msg/TwistStamped]
49+
50+
Publishers
51+
,,,,,,,,,,,
52+
- <controller_name>/odometry [nav_msgs/msg/Odometry]
53+
- <controller_name>/tf_odometry [tf2_msgs/msg/TFMessage]
54+
- <controller_name>/controller_state [control_msgs/msg/MecanumDriveControllerState]
55+
56+
Parameters
57+
,,,,,,,,,,,
58+
59+
For a list of parameters and their meaning, see the YAML file in the ``src`` folder of the controller's package.
60+
61+
For an exemplary parameterization, see the ``test`` folder of the controller's package.

‎mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp‎

Lines changed: 162 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,162 @@
1+
// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
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 MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_
16+
#define MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_
17+
18+
#include <chrono>
19+
#include <cmath>
20+
#include <memory>
21+
#include <queue>
22+
#include <string>
23+
#include <utility>
24+
#include <vector>
25+
26+
#include "controller_interface/chainable_controller_interface.hpp"
27+
#include "mecanum_drive_controller/odometry.hpp"
28+
#include "mecanum_drive_controller/visibility_control.h"
29+
#include "mecanum_drive_controller_parameters.hpp"
30+
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
31+
#include "rclcpp_lifecycle/state.hpp"
32+
#include "realtime_tools/realtime_buffer.h"
33+
#include "realtime_tools/realtime_publisher.h"
34+
#include "std_srvs/srv/set_bool.hpp"
35+
36+
#include "control_msgs/msg/mecanum_drive_controller_state.hpp"
37+
#include "geometry_msgs/msg/twist_stamped.hpp"
38+
#include "nav_msgs/msg/odometry.hpp"
39+
#include "tf2_msgs/msg/tf_message.hpp"
40+
namespace mecanum_drive_controller
41+
{
42+
// name constants for state interfaces
43+
static constexpr size_t NR_STATE_ITFS = 4;
44+
45+
// name constants for command interfaces
46+
static constexpr size_t NR_CMD_ITFS = 4;
47+
48+
// name constants for reference interfaces
49+
static constexpr size_t NR_REF_ITFS = 3;
50+
51+
class MecanumDriveController : public controller_interface::ChainableControllerInterface
52+
{
53+
public:
54+
MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
55+
MecanumDriveController();
56+
57+
MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
58+
controller_interface::CallbackReturn on_init() override;
59+
60+
MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
61+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
62+
63+
MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
64+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
65+
66+
MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
67+
controller_interface::CallbackReturn on_configure(
68+
const rclcpp_lifecycle::State & previous_state) override;
69+
70+
MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
71+
controller_interface::CallbackReturn on_activate(
72+
const rclcpp_lifecycle::State & previous_state) override;
73+
74+
MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
75+
controller_interface::CallbackReturn on_deactivate(
76+
const rclcpp_lifecycle::State & previous_state) override;
77+
78+
MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
79+
controller_interface::return_type update_reference_from_subscribers(
80+
const rclcpp::Time & time, const rclcpp::Duration & period) override;
81+
82+
MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
83+
controller_interface::return_type update_and_write_commands(
84+
const rclcpp::Time & time, const rclcpp::Duration & period) override;
85+
86+
using ControllerReferenceMsg = geometry_msgs::msg::TwistStamped;
87+
using OdomStateMsg = nav_msgs::msg::Odometry;
88+
using TfStateMsg = tf2_msgs::msg::TFMessage;
89+
using ControllerStateMsg = control_msgs::msg::MecanumDriveControllerState;
90+
91+
protected:
92+
std::shared_ptr<mecanum_drive_controller::ParamListener> param_listener_;
93+
mecanum_drive_controller::Params params_;
94+
95+
/**
96+
* The list is sorted in the following order:
97+
* - front left wheel
98+
* - front right wheel
99+
* - back right wheel
100+
* - back left wheel
101+
*/
102+
enum WheelIndex : std::size_t
103+
{
104+
FRONT_LEFT = 0,
105+
FRONT_RIGHT = 1,
106+
REAR_RIGHT = 2,
107+
REAR_LEFT = 3
108+
};
109+
110+
/**
111+
* Internal lists with joint names sorted as in `WheelIndex` enum.
112+
*/
113+
std::vector<std::string> command_joint_names_;
114+
115+
/**
116+
* Internal lists with joint names sorted as in `WheelIndex` enum.
117+
* If parameters for state joint names are *not* defined, this list is the same as
118+
* `command_joint_names_`.
119+
*/
120+
std::vector<std::string> state_joint_names_;
121+
122+
// Names of the references, ex: high level vel commands from MoveIt, Nav2, etc.
123+
// used for preceding controller
124+
std::vector<std::string> reference_names_;
125+
126+
// Command subscribers and Controller State, odom state, tf state publishers
127+
rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
128+
realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_;
129+
rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0);
130+
131+
using OdomStatePublisher = realtime_tools::RealtimePublisher<OdomStateMsg>;
132+
rclcpp::Publisher<OdomStateMsg>::SharedPtr odom_s_publisher_;
133+
std::unique_ptr<OdomStatePublisher> rt_odom_state_publisher_;
134+
135+
using TfStatePublisher = realtime_tools::RealtimePublisher<TfStateMsg>;
136+
rclcpp::Publisher<TfStateMsg>::SharedPtr tf_odom_s_publisher_;
137+
std::unique_ptr<TfStatePublisher> rt_tf_odom_state_publisher_;
138+
139+
using ControllerStatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
140+
rclcpp::Publisher<ControllerStateMsg>::SharedPtr controller_s_publisher_;
141+
std::unique_ptr<ControllerStatePublisher> controller_state_publisher_;
142+
143+
// override methods from ChainableControllerInterface
144+
std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
145+
146+
bool on_set_chained_mode(bool chained_mode) override;
147+
148+
Odometry odometry_;
149+
150+
private:
151+
// callback for topic interface
152+
MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL
153+
void reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg);
154+
155+
double velocity_in_center_frame_linear_x_; // [m/s]
156+
double velocity_in_center_frame_linear_y_; // [m/s]
157+
double velocity_in_center_frame_angular_z_; // [rad/s]
158+
};
159+
160+
} // namespace mecanum_drive_controller
161+
162+
#endif // MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_

0 commit comments

Comments
 (0)