Fix bug with negative execution times in Executor::step. (#450)

* Fix bug with negative execution times in KinematicSimulationTrajectoryExecutor. * Fix tests. * Update CHANGELOG.md.
personalrobotics · Jul 19, 2018 · e46aba0 · e46aba0
1 parent cfba2f6
commit e46aba0
Show file tree

Hide file tree

Showing 4 changed files with 9 additions and 8 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -30,7 +30,7 @@
 * Control
 
   * Fixed CollisionGroup bugs in Hand executors: [#299](https://github.com/personalrobotics/aikido/pull/299)
-  * Rewrote executors for faster-than-realtime simulation: [#316](https://github.com/personalrobotics/aikido/pull/316)
+  * Rewrote executors for faster-than-realtime simulation: [#316](https://github.com/personalrobotics/aikido/pull/316), [#450](https://github.com/personalrobotics/aikido/pull/450)
   * Introduced uniform and dart namespaces: [#342](https://github.com/personalrobotics/aikido/pull/342)
   * Removed Barrett-specific hand executors: [#380](https://github.com/personalrobotics/aikido/pull/380)
 

diff --git a/src/control/KinematicSimulationTrajectoryExecutor.cpp b/src/control/KinematicSimulationTrajectoryExecutor.cpp
@@ -125,8 +125,10 @@ void KinematicSimulationTrajectoryExecutor::step(
   const auto executionTime
       = std::chrono::duration<double>(timeSinceBeginning).count();
 
+  // executionTime may be negative if the thread calling \c step is queued
+  // before and dequeued after \c execute is called.
   if (executionTime < 0)
-    throw std::invalid_argument("Timepoint is before execution start time.");
+    return;
 
   auto state = mStateSpace->createState();
   mTraj->evaluate(executionTime, state);

diff --git a/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp b/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp
@@ -210,7 +210,8 @@ TEST_F(
   EXPECT_DOUBLE_EQ(mSkeleton->getDof(0)->getPosition(), 1.0);
 }
 
-TEST_F(KinematicSimulationTrajectoryExecutorTest, step_NegativeTimepoint_Throws)
+TEST_F(
+    KinematicSimulationTrajectoryExecutorTest, step_NegativeTimepoint_NoThrows)
 {
   KinematicSimulationTrajectoryExecutor executor(mSkeleton);
 
@@ -219,8 +220,7 @@ TEST_F(KinematicSimulationTrajectoryExecutorTest, step_NegativeTimepoint_Throws)
   auto simulationClock = std::chrono::system_clock::now();
   auto future = executor.execute(mTraj);
 
-  EXPECT_THROW(
-      executor.step(simulationClock - stepTime), std::invalid_argument);
+  EXPECT_NO_THROW(executor.step(simulationClock - stepTime));
 
   std::future_status status;
   do

diff --git a/tests/control/test_QueuedTrajectoryExecutor.cpp b/tests/control/test_QueuedTrajectoryExecutor.cpp
@@ -191,7 +191,7 @@ TEST_F(
   EXPECT_EQ(f1.wait_for(waitTime), std::future_status::ready);
 }
 
-TEST_F(QueuedTrajectoryExecutorTest, step_NegativeTimepoint_Throws)
+TEST_F(QueuedTrajectoryExecutorTest, step_NegativeTimepoint_NoThrows)
 {
   QueuedTrajectoryExecutor executor(std::move(mExecutor));
 
@@ -202,8 +202,7 @@ TEST_F(QueuedTrajectoryExecutorTest, step_NegativeTimepoint_Throws)
   auto f2 = executor.execute(mTraj2);
   executor.step(simulationClock); // dequeue trajectory
 
-  EXPECT_THROW(
-      executor.step(simulationClock - stepTime), std::invalid_argument);
+  EXPECT_NO_THROW(executor.step(simulationClock - stepTime));
 
   std::future_status status;
   do