update mecanum controller (#12)

* update mecanum controller

* add pre-commit

Author: KmakD

.clang-format

@@ -0,0 +1,21 @@
+Language: Cpp
+BasedOnStyle: Google
+
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+BraceWrapping:
+  AfterClass: true
+  AfterFunction: true
+  AfterNamespace: true
+  AfterStruct: true
+BreakBeforeBraces: Custom
+ColumnLimit: 100
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+DerivePointerAlignment: false
+PointerAlignment: Middle
+ReflowComments: true
+IncludeBlocks: Preserve
+AlignOperands: true
+PenaltyBreakAssignment: 21
+PenaltyBreakBeforeFirstCallParameter: 1

.github/workflows/pre-commit.yaml

@@ -0,0 +1,11 @@
+---
+name: Pre-commit
+
+on:
+  push:
+
+jobs:
+  pre-commit:
+    uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master
+    with:
+      ros_distro: jazzy

.gitignore

@@ -1 +1 @@
-.vscode/*
+.vscode/*

.pre-commit-config.yaml

@@ -0,0 +1,98 @@
+repos:
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v5.0.0
+    hooks:
+      - id: check-added-large-files
+        # mesh files has to be taken into account
+        args: ["--maxkb=3000"]
+      - id: check-ast
+      - id: check-json
+        # vscode .json files do not follow the standard JSON format
+        exclude: ^.vscode/
+      - id: check-merge-conflict
+      - id: check-symlinks
+      - id: check-xml
+      - id: check-yaml
+      - id: debug-statements
+      - id: destroyed-symlinks
+      - id: detect-private-key
+      - id: end-of-file-fixer
+      - id: fix-byte-order-marker
+      - id: name-tests-test
+      - id: mixed-line-ending
+      - id: trailing-whitespace
+
+  - repo: https://github.com/PyCQA/isort
+    rev: 5.13.2
+    hooks:
+      - id: isort
+        args: ["--profile", "black"]
+
+  - repo: https://github.com/cheshirekow/cmake-format-precommit
+    rev: v0.6.13
+    hooks:
+      - id: cmake-format
+
+  - repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v19.1.3
+    hooks:
+      - id: clang-format
+
+  - repo: https://github.com/codespell-project/codespell
+    rev: v2.3.0
+    hooks:
+      - id: codespell
+        name: codespell
+        description: Checks for common misspellings in text files.
+        entry: codespell
+        args:
+          [
+            "--ignore-words-list",
+            "ned" # north, east, down (NED)
+          ]
+        exclude_types: [rst, svg]
+        language: python
+        types: [text]
+
+  - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt
+    rev: 0.2.3
+    hooks:
+      - id: yamlfmt
+        files: ^.github|./\.yaml
+        args: [--mapping, '2', --sequence, '4', --offset, '2', --width, '100']
+
+  - repo: https://github.com/psf/black
+    rev: 24.10.0
+    hooks:
+      - id: black
+        args: ["--line-length=99"]
+
+  - repo: https://github.com/PyCQA/flake8
+    rev: 7.1.1
+    hooks:
+      - id: flake8
+        args:
+          ["--ignore=E501,W503"] # ignore too long line and line break before binary operator,
+          # black checks it
+
+  - repo: local
+    hooks:
+      - id: ament_copyright
+        name: ament_copyright
+        description: Check if copyright notice is available in all files.
+        entry: ament_copyright
+        language: system
+
+    # Docs - RestructuredText hooks
+  - repo: https://github.com/PyCQA/doc8
+    rev: v1.1.2
+    hooks:
+      - id: doc8
+        args: ["--max-line-length=100", "--ignore=D001"]
+        exclude: ^.*\/CHANGELOG\.rst$
+
+  - repo: https://github.com/tier4/pre-commit-hooks-ros
+    rev: v0.10.0
+    hooks:
+      - id: prettier-package-xml
+      - id: sort-package-xml

README.md

@@ -1,2 +1,3 @@
 # husarion_controllers
+
 Robotic controllers to accompany ros2_control for Husarion robots.

mecanum_drive_controller/CMakeLists.txt

@@ -1,4 +1,5 @@
-# Copied and adapted from diff_drive_controller (https://github.com/ros-controls/ros2_controllers)
+# Copied and adapted from diff_drive_controller
+# (https://github.com/ros-controls/ros2_controllers)
 
 cmake_minimum_required(VERSION 3.11)
 project(mecanum_drive_controller)
@@ -25,45 +26,42 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
     rcpputils
     realtime_tools
     tf2
-    tf2_msgs
-)
+    tf2_msgs)
 
 find_package(ament_cmake 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_parameter.yaml
-)
+                           src/mecanum_drive_controller_parameter.yaml)
 
-add_library(mecanum_drive_controller SHARED
-  src/mecanum_drive_controller.cpp
-  src/odometry.cpp
-  src/speed_limiter.cpp
-)
-target_include_directories(mecanum_drive_controller PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include/mecanum_drive_controller>
-)
-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 "MECANUM_DRIVE_CONTROLLER_BUILDING_DLL")
-pluginlib_export_plugin_description_file(controller_interface mecanum_drive_plugin.xml)
+add_library(
+  mecanum_drive_controller SHARED src/mecanum_drive_controller.cpp
+                                  src/odometry.cpp src/speed_limiter.cpp)
+target_include_directories(
+  mecanum_drive_controller
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include/mecanum_drive_controller>)
+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 "MECANUM_DRIVE_CONTROLLER_BUILDING_DLL")
+pluginlib_export_plugin_description_file(controller_interface
+                                         mecanum_drive_plugin.xml)
 
-install(
-  DIRECTORY include/
-  DESTINATION include/mecanum_drive_controller
-)
+install(DIRECTORY include/ DESTINATION include/mecanum_drive_controller)
 
-install(TARGETS mecanum_drive_controller mecanum_drive_controller_parameters
+install(
+  TARGETS mecanum_drive_controller mecanum_drive_controller_parameters
   EXPORT export_mecanum_drive_controller
   RUNTIME DESTINATION bin
   ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-)
+  LIBRARY DESTINATION lib)
 
 ament_export_targets(export_mecanum_drive_controller HAS_LIBRARY_TARGET)
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})

mecanum_drive_controller/doc/userdoc.rst

@@ -3,7 +3,7 @@
 .. Copied and adapted from diff_drive_controller (https://github.com/ros-controls/ros2_controllers)
 
 mecanum_drive_controller
-=====================
+========================
 
 Controller for mobile robots with mecanum drive based on diff_drive_controller (https://github.com/ros-controls/ros2_controllers).
 Input for control are robot body velocity commands which are translated to wheel commands for the mecanum drive base.

mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp

@@ -31,29 +31,30 @@
 #include <string>
 #include <vector>
 
-#include "controller_interface/controller_interface.hpp"
+#include <controller_interface/controller_interface.hpp>
+#include <hardware_interface/handle.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/state.hpp>
+#include <realtime_tools/realtime_box.hpp>
+#include <realtime_tools/realtime_buffer.hpp>
+#include <realtime_tools/realtime_publisher.hpp>
+
+#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
+#include "mecanum_drive_controller/mecanum_drive_controller_parameters.hpp"
 #include "mecanum_drive_controller/odometry.hpp"
 #include "mecanum_drive_controller/speed_limiter.hpp"
 #include "mecanum_drive_controller/visibility_control.h"
-#include "geometry_msgs/msg/twist.hpp"
-#include "geometry_msgs/msg/twist_stamped.hpp"
-#include "hardware_interface/handle.hpp"
-#include "nav_msgs/msg/odometry.hpp"
 #include "odometry.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_lifecycle/state.hpp"
-#include "realtime_tools/realtime_box.hpp"
-#include "realtime_tools/realtime_buffer.hpp"
-#include "realtime_tools/realtime_publisher.hpp"
-#include "tf2_msgs/msg/tf_message.hpp"
-
-#include <mecanum_drive_controller/mecanum_drive_controller_parameters.hpp>
 
 namespace mecanum_drive_controller
 {
 class MecanumDriveController : public controller_interface::ControllerInterface
 {
-  using Twist = geometry_msgs::msg::TwistStamped;
+  using TwistStamped = geometry_msgs::msg::TwistStamped;
 
 public:
   MECANUM_DRIVE_CONTROLLER_PUBLIC
@@ -66,28 +67,35 @@ class MecanumDriveController : public controller_interface::ControllerInterface
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
   MECANUM_DRIVE_CONTROLLER_PUBLIC
-  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   MECANUM_DRIVE_CONTROLLER_PUBLIC
   controller_interface::CallbackReturn on_init() override;
 
   MECANUM_DRIVE_CONTROLLER_PUBLIC
-  controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
 
   MECANUM_DRIVE_CONTROLLER_PUBLIC
-  controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
 
   MECANUM_DRIVE_CONTROLLER_PUBLIC
-  controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
 
   MECANUM_DRIVE_CONTROLLER_PUBLIC
-  controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override;
+  controller_interface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
 
   MECANUM_DRIVE_CONTROLLER_PUBLIC
-  controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state) override;
+  controller_interface::CallbackReturn on_error(
+    const rclcpp_lifecycle::State & previous_state) override;
 
   MECANUM_DRIVE_CONTROLLER_PUBLIC
-  controller_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state) override;
+  controller_interface::CallbackReturn on_shutdown(
+    const rclcpp_lifecycle::State & previous_state) override;
 
 protected:
   struct WheelHandle
@@ -96,9 +104,9 @@ class MecanumDriveController : public controller_interface::ControllerInterface
     std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity;
   };
 
-  const char* feedback_type() const;
-  controller_interface::CallbackReturn configure_wheel(const std::string& wheel_name,
-                                                       std::unique_ptr<WheelHandle>& registered_handle);
+  const char * feedback_type() const;
+  controller_interface::CallbackReturn configure_wheel(
+    const std::string & wheel_name, std::unique_ptr<WheelHandle> & registered_handle);
 
   std::string front_left_wheel_name_;
   std::string front_right_wheel_name_;
@@ -117,42 +125,48 @@ class MecanumDriveController : public controller_interface::ControllerInterface
   Odometry odometry_;
 
   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_ = nullptr;
+  std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
+    realtime_odometry_publisher_ = nullptr;
 
-  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_ =
-      nullptr;
+  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_ = nullptr;
 
   // Timeout to consider cmd_vel commands old
-  std::chrono::milliseconds cmd_vel_timeout_{ 500 };
+  std::chrono::milliseconds cmd_vel_timeout_{500};
 
   bool subscriber_is_active_ = false;
-  rclcpp::Subscription<Twist>::SharedPtr velocity_command_subscriber_ = nullptr;
+  rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
 
-  realtime_tools::RealtimeBox<std::shared_ptr<Twist>> received_velocity_msg_ptr_{ nullptr };
+  realtime_tools::RealtimeBuffer<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};
 
-  std::queue<Twist> previous_commands_;  // last two commands
+  std::queue<TwistStamped> previous_commands_;  // last two commands
 
   // speed limiters
   SpeedLimiter limiter_linear_x_;
   SpeedLimiter limiter_linear_y_;
   SpeedLimiter limiter_angular_;
 
   bool publish_limited_velocity_ = false;
-  std::shared_ptr<rclcpp::Publisher<Twist>> limited_velocity_publisher_ = nullptr;
-  std::shared_ptr<realtime_tools::RealtimePublisher<Twist>> realtime_limited_velocity_publisher_ = nullptr;
+  std::shared_ptr<rclcpp::Publisher<TwistStamped>> limited_velocity_publisher_ = nullptr;
+  std::shared_ptr<realtime_tools::RealtimePublisher<TwistStamped>>
+    realtime_limited_velocity_publisher_ = nullptr;
 
-  rclcpp::Time previous_update_timestamp_{ 0 };
+  rclcpp::Time previous_update_timestamp_{0};
 
   // publish rate limiter
   double publish_rate_ = 50.0;
   rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);
-  rclcpp::Time previous_publish_timestamp_{ 0 };
+  rclcpp::Time previous_publish_timestamp_{0};
 
   bool is_halted = false;
 
   bool reset();
   void halt();
+
+private:
+  void reset_buffers();
 };
 }  // namespace mecanum_drive_controller
 #endif  // MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_