From 4be505626fe0122e321755217301b200d9e3f888 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
Date: Thu, 23 Sep 2021 05:55:42 +0200
Subject: [PATCH] Fix cmdvel scaling bug (Closes #1175)

---
 doc/source/doxygen-docs/changelog.md          |  1 +
 .../reactive/CAbstractPTGBasedReactive.cpp    | 37 +++++++++++--------
 2 files changed, 23 insertions(+), 15 deletions(-)

diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md
index d653f3d9f6..a2c376d38f 100644
--- a/doc/source/doxygen-docs/changelog.md
+++ b/doc/source/doxygen-docs/changelog.md
@@ -55,6 +55,7 @@
   - Fix error generating and parsing TUM RGBD dataset rawlog files.
   - Fix regresion in mrpt::opengl::CFBORender::render() throwing an exception if the input image was empty.
   - Fix incorrect handling of negative, fractional viewport sizes in mrpt::opengl::COpenGLViewport
+  - Fix: Should not scale velocity commands when in slow down, in CAbstractPTGBasedReactive::generate_vel_cmd() (Closes [#1175](https://github.com/MRPT/mrpt/issues/1175)).
 
 # Version 2.3.2: Released Jul 14, 2021
 - Changes in applications:
diff --git a/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp b/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp
index d181913755..574dfd69eb 100644
--- a/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp
+++ b/libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp
@@ -375,7 +375,7 @@ void CAbstractPTGBasedReactive::performNavigationStep()
 			 * <---+--------------->|<--------------+-------->| |
 			 * estimator:                        |                |
 			 * |                                   |
-			 *                timoff_obstacles <-+                |
+			 *                timoff_obstacles <-   |
 			 * +--> timoff_curPoseVelAge           |
 			 *                                                    |<---------------------------------+--------------->|
 			 *                                                                                       +-->
@@ -1415,25 +1415,32 @@ double CAbstractPTGBasedReactive::generate_vel_cmd(
 		}
 		else
 		{
+			const bool is_slowdown = in_movement.props.count("is_slowdown") != 0
+				? in_movement.props.at("is_slowdown") != 0
+				: false;
+
 			// Take the normalized movement command:
 			new_vel_cmd = in_movement.PTG->directionToMotionCommand(
 				in_movement.PTG->alpha2index(in_movement.direction));
 
 			// Scale holonomic speeds to real-world one:
-			new_vel_cmd->cmdVel_scale(in_movement.speed);
-			cmdvel_speed_scale *= in_movement.speed;
-
-			if (!m_last_vel_cmd)  // first iteration? Use default values:
-				m_last_vel_cmd =
-					in_movement.PTG->getSupportedKinematicVelocityCommand();
-
-			// Honor user speed limits & "blending":
-			const double beta = meanExecutionPeriod.getLastOutput() /
-				(meanExecutionPeriod.getLastOutput() +
-				 params_abstract_ptg_navigator.speedfilter_tau);
-			cmdvel_speed_scale *= new_vel_cmd->cmdVel_limits(
-				*m_last_vel_cmd, beta,
-				params_abstract_ptg_navigator.robot_absolute_speed_limits);
+			if (!is_slowdown)
+			{
+				new_vel_cmd->cmdVel_scale(in_movement.speed);
+				cmdvel_speed_scale *= in_movement.speed;
+
+				if (!m_last_vel_cmd)  // first iteration? Use default values:
+					m_last_vel_cmd =
+						in_movement.PTG->getSupportedKinematicVelocityCommand();
+
+				// Honor user speed limits & "blending":
+				const double beta = meanExecutionPeriod.getLastOutput() /
+					(meanExecutionPeriod.getLastOutput() +
+					 params_abstract_ptg_navigator.speedfilter_tau);
+				cmdvel_speed_scale *= new_vel_cmd->cmdVel_limits(
+					*m_last_vel_cmd, beta,
+					params_abstract_ptg_navigator.robot_absolute_speed_limits);
+			}
 		}
 
 		m_last_vel_cmd = new_vel_cmd;  // Save for filtering in next step