Add cmd_vel_controller

cmd_vel_controller/CMakeLists.txt

@@ -0,0 +1,87 @@
+cmake_minimum_required(VERSION 3.16)
+project(cmd_vel_controller)
+
+find_package(ros2_control_cmake REQUIRED)
+set_compiler_options()
+export_windows_symbols()
+
+# Find dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  ament_cmake
+  control_toolbox
+  controller_interface
+  generate_parameter_library
+  geometry_msgs
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+)
+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(${PROJECT_NAME}_parameters
+  src/${PROJECT_NAME}_parameters.yaml
+  ${TB_INCLUDE_DIRS}/control_toolbox/custom_validators.hpp
+)
+
+add_library(${PROJECT_NAME} SHARED
+  src/${PROJECT_NAME}.cpp
+)
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+target_include_directories(${PROJECT_NAME}
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  ${PROJECT_NAME}_parameters
+  control_toolbox::control_toolbox
+  controller_interface::controller_interface
+  hardware_interface::hardware_interface
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  realtime_tools::realtime_tools
+  ${geometry_msgs_TARGETS}
+)
+pluginlib_export_plugin_description_file(controller_interface cmd_vel_controller.xml)
+
+# Install
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}/
+)
+install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_parameters
+  EXPORT export_${PROJECT_NAME}
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+if(BUILD_TESTING)
+  # find_package(ament_cmake_gmock REQUIRED)
+  # find_package(controller_manager REQUIRED)
+  # find_package(ros2_control_test_assets REQUIRED)
+
+  # ament_add_gmock(test_${PROJECT_NAME} test/test_${PROJECT_NAME}.cpp)
+  # target_link_libraries(test_${PROJECT_NAME}
+    # ${PROJECT_NAME}
+  # )
+
+  # add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  # ament_add_gmock(test_load_${PROJECT_NAME} test/test_load_${PROJECT_NAME}.cpp)
+  # find_package(ament_lint_auto REQUIRED)
+  # ament_lint_auto_find_test_dependencies()
+  # target_link_libraries(test_load_${PROJECT_NAME}
+    # controller_manager::controller_manager
+    # ros2_control_test_assets::ros2_control_test_assets
+  # )
+endif()
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

cmd_vel_controller/cmd_vel_controller.xml

@@ -0,0 +1,7 @@
+<library path="cmd_vel_controller">
+  <class name="cmd_vel_controller/CmdVelController" type="cmd_vel_controller::CmdVelController" base_class_type="controller_interface::ChainableControllerInterface">
+    <description>
+      Controller to limit cmd_vel.
+    </description>
+  </class>
+</library>

cmd_vel_controller/include/cmd_vel_controller/cmd_vel_controller.hpp

@@ -0,0 +1,98 @@
+// Copyright 2025 Aarav Gupta
+//
+// 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 CMD_VEL_CONTROLLER__CMD_VEL_CONTROLLER_HPP_
+#define CMD_VEL_CONTROLLER__CMD_VEL_CONTROLLER_HPP_
+
+#include <memory>
+#include <vector>
+
+#include "control_toolbox/rate_limiter.hpp"
+#include "controller_interface/chainable_controller_interface.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "realtime_tools/realtime_publisher.hpp"
+#include "realtime_tools/realtime_thread_safe_box.hpp"
+
+// auto-generated by generate_parameter_library
+#include "cmd_vel_controller/cmd_vel_controller_parameters.hpp"
+
+namespace cmd_vel_controller
+{
+class CmdVelController : public controller_interface::ChainableControllerInterface
+{
+  using TwistStamped = geometry_msgs::msg::TwistStamped;
+
+public:
+  CmdVelController();
+
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const 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::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_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;
+
+  // Parameters from ROS for cmd_vel_controller
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  // Timeout to consider cmd_vel commands old
+  rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5);
+
+  bool subscriber_is_active_ = false;
+  rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
+  // Realtime container to exchange the reference from subscriber
+  realtime_tools::RealtimeThreadSafeBox<TwistStamped> received_velocity_msg_;
+  // Save the last reference in case of unable to get value from box
+  TwistStamped received_command_msg_;
+
+  std::vector<control_toolbox::RateLimiter<double>> velocity_limiters_;
+  std::queue<std::vector<double>> previous_two_commands_;
+
+  std::shared_ptr<rclcpp::Publisher<TwistStamped>> cmd_vel_publisher_ = nullptr;
+  std::shared_ptr<realtime_tools::RealtimePublisher<TwistStamped>> realtime_cmd_vel_publisher_;
+  TwistStamped cmd_vel_msg_;
+
+  bool on_set_chained_mode(bool chained_mode) override;
+  bool reset();
+
+private:
+  void reset_buffers();
+};
+}  // namespace cmd_vel_controller
+
+#endif  // CMD_VEL_CONTROLLER__CMD_VEL_CONTROLLER_HPP_

cmd_vel_controller/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>cmd_vel_controller</name>
+  <version>0.0.0</version>
+  <description>Controller to limit cmd_vel</description>
+  <maintainer email="aarav@spikonado.com">Aarav Gupta</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>ros2_control_cmake</build_depend>
+
+  <depend>controller_interface</depend>
+  <depend>control_toolbox</depend>
+  <depend>geometry_msgs</depend>
+  <depend>generate_parameter_library</depend>
+  <depend>hardware_interface</depend>
+  <depend>lifecycle_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>