Husarion_mecanum_drive_controller

.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/CHANGELOG.rst → husarion_mecanum_drive_controller/CHANGELOG.rst

@@ -1,3 +1,3 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package mecanum_drive_controller
+Changelog for package husarion_mecanum_drive_controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

husarion_mecanum_drive_controller/CMakeLists.txt

@@ -0,0 +1,70 @@
+# Copied and adapted from diff_drive_controller
+# (https://github.com/ros-controls/ros2_controllers)
+
+cmake_minimum_required(VERSION 3.11)
+project(husarion_mecanum_drive_controller)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Wshadow -Wold-style-cast)
+endif()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+    control_toolbox
+    controller_interface
+    generate_parameter_library
+    geometry_msgs
+    hardware_interface
+    nav_msgs
+    pluginlib
+    rclcpp
+    rclcpp_lifecycle
+    rcpputils
+    realtime_tools
+    tf2
+    tf2_msgs)
+
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+# get include dirs from control_toolbox for the custom validators
+get_target_property(TB_INCLUDE_DIRS control_toolbox::rate_limiter_parameters
+                    INTERFACE_INCLUDE_DIRECTORIES)
+generate_parameter_library(
+  mecanum_drive_controller_parameters
+  src/mecanum_drive_controller_parameter.yaml
+  ${TB_INCLUDE_DIRS}/control_toolbox/custom_validators.hpp)
+
+add_library(mecanum_drive_controller SHARED src/mecanum_drive_controller.cpp
+                                            src/odometry.cpp)
+target_include_directories(
+  mecanum_drive_controller
+  PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include/mecanum_drive_controller>)
+target_link_libraries(
+  mecanum_drive_controller PUBLIC mecanum_drive_controller_parameters
+                                  control_toolbox::rate_limiter_parameters)
+ament_target_dependencies(mecanum_drive_controller PUBLIC
+                          ${THIS_PACKAGE_INCLUDE_DEPENDS})
+pluginlib_export_plugin_description_file(controller_interface
+                                         mecanum_drive_plugin.xml)
+
+install(DIRECTORY include/ DESTINATION include/husarion_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 → husarion_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.

husarion_mecanum_drive_controller/include/husarion_mecanum_drive_controller/mecanum_drive_controller.hpp

@@ -0,0 +1,160 @@
+// Copyright 2020 PAL Robotics S.L.
+// Copyright 2022 Husarion
+//
+// 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.
+
+/*
+ * Author: Bence Magyar, Enrique Fernández, Manuel Meraz
+ */
+
+// Copied and adapted from diff_drive_controller (https://github.com/ros-controls/ros2_controllers)
+// Based on: https://ecam-eurobot.github.io/Tutorials/mechanical/mecanum.html
+// Author: Maciej Stępień
+
+#ifndef HUSARION_MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_
+#define HUSARION_MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_
+
+#include <chrono>
+#include <cmath>
+#include <memory>
+#include <queue>
+#include <string>
+#include <vector>
+
+#include <controller_interface/chainable_controller_interface.hpp>
+#include <hardware_interface/handle.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/state.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 "husarion_mecanum_drive_controller/mecanum_drive_controller_parameters.hpp"
+#include "husarion_mecanum_drive_controller/odometry.hpp"
+#include "husarion_mecanum_drive_controller/speed_limiter.hpp"
+
+namespace husarion_mecanum_drive_controller
+{
+class MecanumDriveController : public controller_interface::ChainableControllerInterface
+{
+  using TwistStamped = geometry_msgs::msg::TwistStamped;
+
+public:
+  MecanumDriveController();
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  // Chainable controller replaces update() with the following two functions
+  controller_interface::return_type update_reference_from_subscribers(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  controller_interface::return_type update_and_write_commands(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  controller_interface::CallbackReturn on_init() 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::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;
+
+  struct WheelHandle
+  {
+    std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback;
+    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);
+
+  std::string front_left_wheel_name_;
+  std::string front_right_wheel_name_;
+  std::string rear_left_wheel_name_;
+  std::string rear_right_wheel_name_;
+
+  std::unique_ptr<WheelHandle> registered_front_left_wheel_handle_;
+  std::unique_ptr<WheelHandle> registered_front_right_wheel_handle_;
+  std::unique_ptr<WheelHandle> registered_rear_left_wheel_handle_;
+  std::unique_ptr<WheelHandle> registered_rear_right_wheel_handle_;
+
+  // Parameters from ROS for diff_drive_controller
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  Odometry odometry_;
+
+  // Timeout to consider cmd_vel commands old
+  rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5);
+
+  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<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;
+  bool subscriber_is_active_ = false;
+  rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
+
+  realtime_tools::RealtimeBuffer<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};
+
+  std::queue<std::array<double, 3>> previous_two_commands_;  // last two commands
+
+  // speed limiters
+  std::unique_ptr<SpeedLimiter> limiter_linear_x_;
+  std::unique_ptr<SpeedLimiter> limiter_linear_y_;
+  std::unique_ptr<SpeedLimiter> limiter_angular_;
+
+  bool publish_limited_velocity_ = false;
+  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};
+
+  // publish rate limiter
+  double publish_rate_ = 50.0;
+  rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);
+  rclcpp::Time previous_publish_timestamp_{0};
+
+  bool is_halted = false;
+
+  bool reset();
+  void halt();
+
+private:
+  void reset_buffers();
+};
+}  // namespace husarion_mecanum_drive_controller
+#endif  // HUSARION_MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_