Repository: ros-navigation/navigation2 (C++)

Issue:
[velocity_smoother] accel and deccel are inverted for negative speeds
## Bug report

**Required Info:**

- Operating System:
  - Ubuntu 22.04
- ROS2 Version:
  - Humble
- Version or commit hash:
  - lastest main
- DDS implementation:
  - Cyclone

#### Steps to reproduce issue

```
    velocity_smoother = Node(
        package='nav2_velocity_smoother',
        name='root_velocity_smoother',
        executable='velocity_smoother',
        parameters=[
            # {'use_sim_time': use_sim_time},
            {'smoothing_frequency': 20.0},
            {'scale_velocities': False},
            {'feedback': 'OPEN_LOOP'},
            {'max_velocity': [0.5, 0.0, 0.8]},
            {'min_velocity': [-0.5, 0.0, -0.8]},
            {'velocity_timeout': 0.5},
            {'max_accel': [0.1, 0.0, 0.2]},
            {'max_decel': [-0.5, 0.0, -1.2]},
            ],
        remappings=[
            ('cmd_vel', 'cmd_vel_protected'),
            ('cmd_vel_smoothed', 'cmd_vel')],
        output='screen')
```



#### Expected behavior

accel and deccel are applied similarly on both direction

#### Actual behavior

accel and deccel are inverted when going backward (here 0.1m/s2 accel and 0.5 m/s2 deccel)

Forward:
![Screenshot from 2023-04-06 10-00-10](https://user-images.githubusercontent.com/15727892/230331966-b71b2659-45b7-45ab-920b-b2c730ea9ea5.png)

Backward:
![Screenshot from 2023-04-06 10-02-11](https://user-images.githubusercontent.com/15727892/230332007-d726859b-b60f-4fd1-8e34-50d98fbc8361.png)

Same for angular accel/deccel

![Screenshot from 2023-04-07 23-15-23](https://user-images.githubusercontent.com/15727892/230685667-68b1f976-587e-429d-ab88-694ba2a5ec50.png)

![Screenshot from 2023-04-07 23-15-50](https://user-images.githubusercontent.com/15727892/230685677-a78e13ff-a12d-4645-b66e-5c590b42ba58.png)

EDIT: angular velocities
Reporting for now, PR later







Fix (patch):
diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp
index 1c42ff44fb2..04a32081a2a 100644
--- a/nav2_velocity_smoother/src/velocity_smoother.cpp
+++ b/nav2_velocity_smoother/src/velocity_smoother.cpp
@@ -182,9 +182,21 @@ double VelocitySmoother::findEtaConstraint(
   const double v_curr, const double v_cmd, const double accel, const double decel)
 {
   // Exploiting vector scaling properties
-  const double v_component_max = accel / smoothing_frequency_;
-  const double v_component_min = decel / smoothing_frequency_;
-  const double dv = v_cmd - v_curr;
+  double dv = v_cmd - v_curr;
+
+  double v_component_max;
+  double v_component_min;
+
+  // Accelerating if magnitude of v_cmd is above magnitude of v_curr
+  // and if v_cmd and v_curr have the same sign (i.e. speed is NOT passing through 0.0)
+  // Decelerating otherwise
+  if (abs(v_cmd) >= abs(v_curr) && v_curr * v_cmd >= 0.0) {
+    v_component_max = accel / smoothing_frequency_;
+    v_component_min = -accel / smoothing_frequency_;
+  } else {
+    v_component_max = -decel / smoothing_frequency_;
+    v_component_min = decel / smoothing_frequency_;
+  }
 
   if (dv > v_component_max) {
     return v_component_max / dv;
@@ -202,8 +214,21 @@ double VelocitySmoother::applyConstraints(
   const double accel, const double decel, const double eta)
 {
   double dv = v_cmd - v_curr;
-  const double v_component_max = accel / smoothing_frequency_;
-  const double v_component_min = decel / smoothing_frequency_;
+
+  double v_component_max;
+  double v_component_min;
+
+  // Accelerating if magnitude of v_cmd is above magnitude of v_curr
+  // and if v_cmd and v_curr have the same sign (i.e. speed is NOT passing through 0.0)
+  // Decelerating otherwise
+  if (abs(v_cmd) >= abs(v_curr) && v_curr * v_cmd >= 0.0) {
+    v_component_max = accel / smoothing_frequency_;
+    v_component_min = -accel / smoothing_frequency_;
+  } else {
+    v_component_max = -decel / smoothing_frequency_;
+    v_component_min = decel / smoothing_frequency_;
+  }
+
   return v_curr + std::clamp(eta * dv, v_component_min, v_component_max);
 }