Fix cmdvel scaling bug (Closes #1175)

MRPT · Sep 23, 2021 · 4be5056 · 4be5056
1 parent c27988a
commit 4be5056
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 15 deletions.
diff --git 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
@@ -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