diff_drive: Use speed_limiter from control_toolbox

ros-controls · Nov 1, 2024 · 66f531a · 66f531a
1 parent 87f21b3
commit 66f531a
Show file tree

Hide file tree

Showing 6 changed files with 7 additions and 250 deletions.
diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt
@@ -6,6 +6,7 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
 endif()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
+  control_toolbox
   controller_interface
   generate_parameter_library
   geometry_msgs
@@ -35,7 +36,6 @@ generate_parameter_library(diff_drive_controller_parameters
 add_library(diff_drive_controller SHARED
   src/diff_drive_controller.cpp
   src/odometry.cpp
-  src/speed_limiter.cpp
 )
 target_compile_features(diff_drive_controller PUBLIC cxx_std_17)
 target_include_directories(diff_drive_controller PUBLIC

diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp
@@ -26,9 +26,9 @@
 #include <string>
 #include <vector>
 
+#include "control_toolbox/speed_limiter.hpp"
 #include "controller_interface/controller_interface.hpp"
 #include "diff_drive_controller/odometry.hpp"
-#include "diff_drive_controller/speed_limiter.hpp"
 #include "diff_drive_controller/visibility_control.h"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
@@ -135,8 +135,8 @@ class DiffDriveController : public controller_interface::ControllerInterface
   std::queue<Twist> previous_commands_;  // last two commands
 
   // speed limiters
-  SpeedLimiter limiter_linear_;
-  SpeedLimiter limiter_angular_;
+  control_toolbox::SpeedLimiter limiter_linear_;
+  control_toolbox::SpeedLimiter limiter_angular_;
 
   bool publish_limited_velocity_ = false;
   std::shared_ptr<rclcpp::Publisher<Twist>> limited_velocity_publisher_ = nullptr;

diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp
diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml
@@ -12,6 +12,7 @@
   <build_depend>generate_parameter_library</build_depend>
 
   <depend>backward_ros</depend>
+  <depend>control_toolbox</depend>
   <depend>controller_interface</depend>
   <depend>geometry_msgs</depend>
   <depend>hardware_interface</depend>

diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp
@@ -296,13 +296,13 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
   cmd_vel_timeout_ = std::chrono::milliseconds{static_cast<int>(params_.cmd_vel_timeout * 1000.0)};
   publish_limited_velocity_ = params_.publish_limited_velocity;
 
-  limiter_linear_ = SpeedLimiter(
+  limiter_linear_ = control_toolbox::SpeedLimiter(
     params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits,
     params_.linear.x.has_jerk_limits, params_.linear.x.min_velocity, params_.linear.x.max_velocity,
     params_.linear.x.min_acceleration, params_.linear.x.max_acceleration, params_.linear.x.min_jerk,
     params_.linear.x.max_jerk);
 
-  limiter_angular_ = SpeedLimiter(
+  limiter_angular_ = control_toolbox::SpeedLimiter(
     params_.angular.z.has_velocity_limits, params_.angular.z.has_acceleration_limits,
     params_.angular.z.has_jerk_limits, params_.angular.z.min_velocity,
     params_.angular.z.max_velocity, params_.angular.z.min_acceleration,

diff --git a/diff_drive_controller/src/speed_limiter.cpp b/diff_drive_controller/src/speed_limiter.cpp