Add Mecanum Drive Controller (#512)

(cherry picked from commit 79358e3)

doc/controllers_index.rst

@@ -28,6 +28,7 @@ Controllers for Wheeled Mobile Robots
    :titlesonly:
 
    Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst>
+   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>
 

doc/release_notes.rst

@@ -46,6 +46,10 @@ joint_trajectory_controller
 
 * 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>`_).
 
+mecanum_drive_controller
+************************
+* 🚀 The mecanum_drive_controller was added 🎉 (`#512 <https://github.com/ros-controls/ros2_controllers/pull/512>`_).
+
 pid_controller
 ************************
 * 🚀 The PID controller was added 🎉 (`#434 <https://github.com/ros-controls/ros2_controllers/pull/434>`_).

mecanum_drive_controller/CMakeLists.txt

@@ -0,0 +1,110 @@
+cmake_minimum_required(VERSION 3.16)
+project(mecanum_drive_controller LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
+  -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct
+  -Werror=missing-braces)
+endif()
+
+# find dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_interface
+  hardware_interface
+  generate_parameter_library
+  nav_msgs
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  std_srvs
+  tf2
+  tf2_geometry_msgs
+  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()
+
+generate_parameter_library(mecanum_drive_controller_parameters
+  src/mecanum_drive_controller.yaml
+)
+
+add_library(
+  mecanum_drive_controller
+  SHARED
+  src/mecanum_drive_controller.cpp
+  src/odometry.cpp
+)
+target_compile_features(mecanum_drive_controller PUBLIC cxx_std_17)
+target_include_directories(mecanum_drive_controller PUBLIC
+  "$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(mecanum_drive_controller PUBLIC
+  mecanum_drive_controller_parameters)
+ament_target_dependencies(mecanum_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(mecanum_drive_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(
+  controller_interface mecanum_drive_controller.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_mecanum_drive_controller test/test_load_mecanum_drive_controller.cpp)
+  ament_target_dependencies(test_load_mecanum_drive_controller
+    controller_manager
+    hardware_interface
+    ros2_control_test_assets
+  )
+
+  add_rostest_with_parameters_gmock(
+    test_mecanum_drive_controller test/test_mecanum_drive_controller.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_params.yaml)
+  target_include_directories(test_mecanum_drive_controller PRIVATE include)
+  target_link_libraries(test_mecanum_drive_controller mecanum_drive_controller)
+  ament_target_dependencies(
+    test_mecanum_drive_controller
+    controller_interface
+    hardware_interface
+  )
+
+  add_rostest_with_parameters_gmock(
+    test_mecanum_drive_controller_preceeding test/test_mecanum_drive_controller_preceeding.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_preceeding_params.yaml)
+  target_include_directories(test_mecanum_drive_controller_preceeding PRIVATE include)
+  target_link_libraries(test_mecanum_drive_controller_preceeding mecanum_drive_controller)
+  ament_target_dependencies(
+    test_mecanum_drive_controller_preceeding
+    controller_interface
+    hardware_interface
+  )
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include/mecanum_drive_controller
+)
+
+install(
+  TARGETS mecanum_drive_controller mecanum_drive_controller_parameters
+  EXPORT export_mecanum_drive_controller
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+ament_export_targets(export_mecanum_drive_controller HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

mecanum_drive_controller/doc/userdoc.rst

@@ -0,0 +1,61 @@
+.. _mecanum_drive_controller_userdoc:
+
+mecanum_drive_controller
+=========================
+
+Library with shared functionalities for mobile robot controllers with mecanum drive (four wheels).
+The library implements generic odometry and update methods and defines the main interfaces.
+
+Execution logic of the controller
+----------------------------------
+
+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.
+In the chain mode, the controller provides three reference interfaces, one for linear velocity and one for steering angle position.
+Other relevant features are:
+
+  - odometry publishing as Odometry and TF message;
+  - input command timeout based on a parameter.
+
+Note about odometry calculation:
+In the DiffDRiveController, the velocity is filtered out, but we prefer to return it raw and let the user perform post-processing at will.
+We prefer this way of doing so as filtering introduces delay (which makes it difficult to interpret and compare behavior curves).
+
+
+Description of controller's interfaces
+--------------------------------------
+
+References (from a preceding controller)
+,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
+- <reference_names[i]>/<interface_name>  [double]  # in [rad] or [rad/s]
+
+Commands
+,,,,,,,,,
+- <command_joint_names[i]>/<interface_name>  [double]  # in [rad] or [rad/s]
+
+States
+,,,,,,,
+- <joint_names[i]>/<interface_name>  [double]  # in [rad] or [rad/s]
+  ..note ::
+
+  ``joint_names[i]`` can be of ``state_joint_names`` parameter (if used), ``command_joint_names`` otherwise.
+
+
+Subscribers
+,,,,,,,,,,,,
+Used when the controller is not in chained mode (``in_chained_mode == false``).
+
+- <controller_name>/reference  [geometry_msgs/msg/TwistStamped]
+
+Publishers
+,,,,,,,,,,,
+- <controller_name>/odometry          [nav_msgs/msg/Odometry]
+- <controller_name>/tf_odometry       [tf2_msgs/msg/TFMessage]
+- <controller_name>/controller_state  [control_msgs/msg/MecanumDriveControllerState]
+
+Parameters
+,,,,,,,,,,,
+
+For a list of parameters and their meaning, see the YAML file in the ``src`` folder of the controller's package.
+
+For an exemplary parameterization, see the ``test`` folder of the controller's package.

mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp

@@ -0,0 +1,162 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// 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 MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_
+#define MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_
+
+#include <chrono>
+#include <cmath>
+#include <memory>
+#include <queue>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "controller_interface/chainable_controller_interface.hpp"
+#include "mecanum_drive_controller/odometry.hpp"
+#include "mecanum_drive_controller/visibility_control.h"
+#include "mecanum_drive_controller_parameters.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "realtime_tools/realtime_publisher.h"
+#include "std_srvs/srv/set_bool.hpp"
+
+#include "control_msgs/msg/mecanum_drive_controller_state.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "tf2_msgs/msg/tf_message.hpp"
+namespace mecanum_drive_controller
+{
+// name constants for state interfaces
+static constexpr size_t NR_STATE_ITFS = 4;
+
+// name constants for command interfaces
+static constexpr size_t NR_CMD_ITFS = 4;
+
+// name constants for reference interfaces
+static constexpr size_t NR_REF_ITFS = 3;
+
+class MecanumDriveController : public controller_interface::ChainableControllerInterface
+{
+public:
+  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
+  MecanumDriveController();
+
+  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::CallbackReturn on_init() override;
+
+  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::return_type update_reference_from_subscribers(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::return_type update_and_write_commands(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  using ControllerReferenceMsg = geometry_msgs::msg::TwistStamped;
+  using OdomStateMsg = nav_msgs::msg::Odometry;
+  using TfStateMsg = tf2_msgs::msg::TFMessage;
+  using ControllerStateMsg = control_msgs::msg::MecanumDriveControllerState;
+
+protected:
+  std::shared_ptr<mecanum_drive_controller::ParamListener> param_listener_;
+  mecanum_drive_controller::Params params_;
+
+  /**
+   * The list is sorted in the following order:
+   *  - front left wheel
+   *  - front right wheel
+   *  - back right wheel
+   *  - back left wheel
+   */
+  enum WheelIndex : std::size_t
+  {
+    FRONT_LEFT = 0,
+    FRONT_RIGHT = 1,
+    REAR_RIGHT = 2,
+    REAR_LEFT = 3
+  };
+
+  /**
+   * Internal lists with joint names sorted as in `WheelIndex` enum.
+   */
+  std::vector<std::string> command_joint_names_;
+
+  /**
+   * Internal lists with joint names sorted as in `WheelIndex` enum.
+   * If parameters for state joint names are *not* defined, this list is the same as
+   * `command_joint_names_`.
+   */
+  std::vector<std::string> state_joint_names_;
+
+  // Names of the references, ex: high level vel commands from MoveIt, Nav2, etc.
+  // used for preceding controller
+  std::vector<std::string> reference_names_;
+
+  // Command subscribers and Controller State, odom state, tf state publishers
+  rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_;
+  rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0);
+
+  using OdomStatePublisher = realtime_tools::RealtimePublisher<OdomStateMsg>;
+  rclcpp::Publisher<OdomStateMsg>::SharedPtr odom_s_publisher_;
+  std::unique_ptr<OdomStatePublisher> rt_odom_state_publisher_;
+
+  using TfStatePublisher = realtime_tools::RealtimePublisher<TfStateMsg>;
+  rclcpp::Publisher<TfStateMsg>::SharedPtr tf_odom_s_publisher_;
+  std::unique_ptr<TfStatePublisher> rt_tf_odom_state_publisher_;
+
+  using ControllerStatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
+  rclcpp::Publisher<ControllerStateMsg>::SharedPtr controller_s_publisher_;
+  std::unique_ptr<ControllerStatePublisher> controller_state_publisher_;
+
+  // override methods from ChainableControllerInterface
+  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
+
+  bool on_set_chained_mode(bool chained_mode) override;
+
+  Odometry odometry_;
+
+private:
+  // callback for topic interface
+  MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL
+  void reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg);
+
+  double velocity_in_center_frame_linear_x_;   // [m/s]
+  double velocity_in_center_frame_linear_y_;   // [m/s]
+  double velocity_in_center_frame_angular_z_;  // [rad/s]
+};
+
+}  // namespace mecanum_drive_controller
+
+#endif  // MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_