From 2a75e5d0729638b620ddfbbf5da0998a5b1f9020 Mon Sep 17 00:00:00 2001
From: Vincent Belpois <vincent.belpois@gmail.com>
Date: Wed, 24 Apr 2024 21:23:36 +0200
Subject: [PATCH] Didn't limit the correct speed I think

---
 .../config/omnidirectional_controller.yaml       |  4 ++--
 .../src/omnidirectional_controller.cpp           | 16 ++++++++++------
 2 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/src/ezbot_robot/config/omnidirectional_controller.yaml b/src/ezbot_robot/config/omnidirectional_controller.yaml
index 9550bed..d3db8c8 100644
--- a/src/ezbot_robot/config/omnidirectional_controller.yaml
+++ b/src/ezbot_robot/config/omnidirectional_controller.yaml
@@ -35,7 +35,7 @@ omnidirectional_controller:
     use_stamped_vel: false
 
     linear_has_velocity_limits: true
-    linear_min_velocity: -1.0
-    linear_max_velocity: 1.0
+    linear_min_velocity: -0.1
+    linear_max_velocity: 0.1
 
 
diff --git a/src/omnidirectional_controllers/src/omnidirectional_controller.cpp b/src/omnidirectional_controllers/src/omnidirectional_controller.cpp
index 9bb49a8..517dc73 100644
--- a/src/omnidirectional_controllers/src/omnidirectional_controller.cpp
+++ b/src/omnidirectional_controllers/src/omnidirectional_controller.cpp
@@ -437,6 +437,10 @@ controller_interface::return_type OmnidirectionalController::update(
   }
 
   Twist command = *cmd_vel_;
+  double & linear_x_command = command.twist.linear.x;
+  double & linear_y_command = command.twist.linear.y;
+  double & angular_command = command.twist.angular.z;
+
 
   if (odom_params_.open_loop) {
     odometry_.updateOpenLoop(
@@ -486,12 +490,12 @@ controller_interface::return_type OmnidirectionalController::update(
   auto & last_command = previous_commands_.back().twist;
   auto & second_to_last_command = previous_commands_.front().twist;
 
-  limiter_linear_.limit(command.twist.linear.x, second_to_last_command.linear.x, last_command.linear.x, 
+  limiter_linear_.limit(linear_x_command, second_to_last_command.linear.x, last_command.linear.x, 
                         period.seconds());
-  limiter_linear_.limit(command.twist.linear.y, second_to_last_command.linear.y, last_command.linear.y,
+  limiter_linear_.limit(linear_y_command, second_to_last_command.linear.y, last_command.linear.y,
                         period.seconds());
 
-  limiter_angular_.limit(command.twist.angular.z, second_to_last_command.angular.z, last_command.angular.z,
+  limiter_angular_.limit(angular_command, second_to_last_command.angular.z, last_command.angular.z,
                         period.seconds());
 
 
@@ -500,9 +504,9 @@ controller_interface::return_type OmnidirectionalController::update(
 
   // Compute wheels velocities:
   RobotVelocity body_vel_setpoint;
-  body_vel_setpoint.vx = cmd_vel_->twist.linear.x;
-  body_vel_setpoint.vy = cmd_vel_->twist.linear.y;
-  body_vel_setpoint.omega = cmd_vel_->twist.angular.z;
+  body_vel_setpoint.vx = linear_x_command;
+  body_vel_setpoint.vy = linear_y_command;
+  body_vel_setpoint.omega = angular_command;
 
   std::vector<double> wheels_angular_velocity;
   wheels_angular_velocity = omni_robot_kinematics_.getWheelsAngularVelocities(body_vel_setpoint);