From 8567614eed55c12caa878862ad56bfefcc12cf2f Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <luca@openrobotics.org>
Date: Tue, 6 Apr 2021 06:47:15 +0800
Subject: [PATCH] Validate step size and RTF parameters (#740)

Only set them if they are strictly positive.

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>
---
 src/SimulationRunner.cc           | 40 ++++++++++++++++++++-----------
 test/integration/user_commands.cc | 17 +++++++++++++
 2 files changed, 43 insertions(+), 14 deletions(-)

diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc
index e3d0bc8a4b..35c0158098 100644
--- a/src/SimulationRunner.cc
+++ b/src/SimulationRunner.cc
@@ -355,20 +355,32 @@ void SimulationRunner::UpdatePhysicsParams()
   if (newStepSize != this->stepSize ||
       std::abs(newRTF - this->desiredRtf) > eps)
   {
-    this->SetStepSize(
-      std::chrono::duration_cast<std::chrono::steady_clock::duration>(
-        newStepSize));
-    this->desiredRtf = newRTF;
-    this->updatePeriod = std::chrono::nanoseconds(
-        static_cast<int>(this->stepSize.count() / this->desiredRtf));
-
-    this->simTimes.clear();
-    this->realTimes.clear();
-    // Update physics components
-    physicsComp->Data().SetMaxStepSize(physicsParams.max_step_size());
-    physicsComp->Data().SetRealTimeFactor(newRTF);
-    this->entityCompMgr.SetChanged(worldEntity, components::Physics::typeId,
-        ComponentState::OneTimeChange);
+    bool updated = false;
+    // Make sure the values are valid before setting them
+    if (newStepSize.count() > 0.0)
+    {
+      this->SetStepSize(
+        std::chrono::duration_cast<std::chrono::steady_clock::duration>(
+          newStepSize));
+      physicsComp->Data().SetMaxStepSize(physicsParams.max_step_size());
+      updated = true;
+    }
+    if (newRTF > 0.0)
+    {
+      this->desiredRtf = newRTF;
+      this->updatePeriod = std::chrono::nanoseconds(
+          static_cast<int>(this->stepSize.count() / this->desiredRtf));
+      physicsComp->Data().SetRealTimeFactor(newRTF);
+      updated = true;
+    }
+    if (updated)
+    {
+      this->simTimes.clear();
+      this->realTimes.clear();
+      // Set as OneTimeChange to make sure the update is not missed
+      this->entityCompMgr.SetChanged(worldEntity, components::Physics::typeId,
+          ComponentState::OneTimeChange);
+    }
   }
   this->entityCompMgr.RemoveComponent<components::PhysicsCmd>(worldEntity);
 }
diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc
index aceaaafaff..e1e244cba6 100644
--- a/test/integration/user_commands.cc
+++ b/test/integration/user_commands.cc
@@ -981,4 +981,21 @@ TEST_F(UserCommandsTest, Physics)
   physicsComp = ecm->Component<components::Physics>(worldEntity);
   EXPECT_DOUBLE_EQ(0.123, physicsComp->Data().MaxStepSize());
   EXPECT_DOUBLE_EQ(4.567, physicsComp->Data().RealTimeFactor());
+
+  // Send invalid values (not > 0) and make sure they are not updated
+  req.set_max_step_size(0.0);
+  req.set_real_time_factor(0.0);
+
+  EXPECT_TRUE(node.Request(service, req, timeout, res, result));
+  EXPECT_TRUE(result);
+  EXPECT_TRUE(res.data());
+
+  // Run two iterations, in the first one the PhysicsCmd component is created
+  // in the second one it is processed
+  server.Run(true, 2, false);
+
+  // Check updated physics properties
+  physicsComp = ecm->Component<components::Physics>(worldEntity);
+  EXPECT_DOUBLE_EQ(0.123, physicsComp->Data().MaxStepSize());
+  EXPECT_DOUBLE_EQ(4.567, physicsComp->Data().RealTimeFactor());
 }