Cancel in-progress trajectories. (#400)

* Cancel actionlib goal.

* Rename abort to cancel to match actionlib.

* Add warning when no trajectory is in progress.

* Update CHANGELOG.md.

* Address PR comments.

CHANGELOG.md

@@ -33,6 +33,7 @@
   * 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)
+  * Supported canceling in-progress trajectories: [#400](https://github.com/personalrobotics/aikido/pull/400)
 
 * Perception
 
@@ -75,6 +76,10 @@
   * Renamed NonColliding to CollisionFree: [#256](https://github.com/personalrobotics/aikido/pull/256)
   * Added TestableOutcome class: [#266](https://github.com/personalrobotics/aikido/pull/266)
 
+* Control
+
+  * Added Instantaneous and Queued TrajectoryExecutors: [#259](https://github.com/personalrobotics/aikido/pull/259)
+
 * Perception
 
   * Added RcnnPoseModule: [#264](https://github.com/personalrobotics/aikido/pull/264)

include/aikido/control/InstantaneousTrajectoryExecutor.hpp

@@ -38,12 +38,12 @@ class InstantaneousTrajectoryExecutor : public TrajectoryExecutor
   std::future<void> execute(
       const trajectory::ConstTrajectoryPtr& traj) override;
 
-  // Do nothing.
+  /// Does nothing.
   void step(
       const std::chrono::system_clock::time_point& /*timepoint*/) override;
 
-  // Do nothing.
-  void abort() override;
+  /// Does nothing.
+  void cancel() override;
 
 private:
   /// Skeleton to execute trajectories on

include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp

@@ -46,8 +46,8 @@ class KinematicSimulationTrajectoryExecutor : public TrajectoryExecutor
   /// calling this method.
   void step(const std::chrono::system_clock::time_point& timepoint) override;
 
-  /// Aborts the current trajectory.
-  void abort() override;
+  /// Cancels the current trajectory.
+  void cancel() override;
 
 private:
   /// Skeleton to execute trajectories on

include/aikido/control/QueuedTrajectoryExecutor.hpp

@@ -25,7 +25,7 @@ class QueuedTrajectoryExecutor : public TrajectoryExecutor
 
   /// Execute trajectory and set future upon completion. If another trajectory
   /// is already running, queue the trajectory for later execution. If executing
-  /// a trajectory terminates in an error, all queued trajectories are aborted.
+  /// a trajectory terminates in an error, all queued trajectories are canceled.
   ///
   /// \param traj Trajectory to be executed or queued.
   /// \return future<void> for trajectory execution. If trajectory terminates
@@ -37,10 +37,10 @@ class QueuedTrajectoryExecutor : public TrajectoryExecutor
   // Documentation inherited.
   void step(const std::chrono::system_clock::time_point& timepoint) override;
 
-  /// Abort the current trajectory, as well as all trajectories currently in the
-  /// queue. Does NOT stop the trajectory that is currently executing if the
+  /// Cancel the current trajectory, as well as all trajectories currently in
+  /// the queue. Does NOT stop the trajectory that is currently executing if the
   /// underlying executor does not support it.
-  void abort() override;
+  void cancel() override;
 
 private:
   /// Underlying TrajectoryExecutor

include/aikido/control/TrajectoryExecutor.hpp

@@ -40,9 +40,8 @@ class TrajectoryExecutor
   /// \param timepoint Time to simulate to
   virtual void step(const std::chrono::system_clock::time_point& timepoint) = 0;
 
-  /// Abort the current trajectory.
-  /// \note This is currently only supported in simulation.
-  virtual void abort() = 0;
+  /// Cancel the current trajectory.
+  virtual void cancel() = 0;
 
 protected:
   /// Set of trajectories validated by executor

include/aikido/control/ros/RosTrajectoryExecutor.hpp

@@ -58,8 +58,8 @@ class RosTrajectoryExecutor : public aikido::control::TrajectoryExecutor
   /// Regularly checks for the completion of a sent trajectory.
   void step(const std::chrono::system_clock::time_point& timepoint) override;
 
-  // Do nothing.
-  void abort() override;
+  /// \copydoc TrajectoryExecutor::cancel()
+  void cancel() override;
 
 private:
   using TrajectoryActionClient

src/control/InstantaneousTrajectoryExecutor.cpp

@@ -77,7 +77,7 @@ void InstantaneousTrajectoryExecutor::step(
 }
 
 //==============================================================================
-void InstantaneousTrajectoryExecutor::abort()
+void InstantaneousTrajectoryExecutor::cancel()
 {
   // Do nothing
 }

src/control/KinematicSimulationTrajectoryExecutor.cpp

@@ -1,4 +1,5 @@
 #include "aikido/control/KinematicSimulationTrajectoryExecutor.hpp"
+#include <dart/common/Console.hpp>
 #include <dart/common/StlHelpers.hpp>
 #include "aikido/control/TrajectoryRunningException.hpp"
 
@@ -32,7 +33,7 @@ KinematicSimulationTrajectoryExecutor::~KinematicSimulationTrajectoryExecutor()
     {
       mInProgress = false;
       mPromise->set_exception(
-          std::make_exception_ptr(std::runtime_error("Trajectory aborted.")));
+          std::make_exception_ptr(std::runtime_error("Trajectory canceled.")));
     }
   }
 }
@@ -147,7 +148,7 @@ void KinematicSimulationTrajectoryExecutor::step(
 }
 
 //==============================================================================
-void KinematicSimulationTrajectoryExecutor::abort()
+void KinematicSimulationTrajectoryExecutor::cancel()
 {
   std::lock_guard<std::mutex> lock(mMutex);
 
@@ -158,7 +159,12 @@ void KinematicSimulationTrajectoryExecutor::abort()
     mMetaSkeleton.reset();
     mInProgress = false;
     mPromise->set_exception(
-        std::make_exception_ptr(std::runtime_error("Trajectory aborted.")));
+        std::make_exception_ptr(std::runtime_error("Trajectory canceled.")));
+  }
+  else
+  {
+    dtwarn << "[KinematicSimulationTrajectoryExecutor::cancel] Attempting to "
+           << "cancel trajectory, but no trajectory in progress.\n";
   }
 }
 

src/control/QueuedTrajectoryExecutor.cpp

@@ -74,7 +74,7 @@ void QueuedTrajectoryExecutor::step(
     catch (const std::exception& e)
     {
       promise->set_exception(std::current_exception());
-      abort();
+      cancel();
     }
   }
 
@@ -90,22 +90,22 @@ void QueuedTrajectoryExecutor::step(
 }
 
 //==============================================================================
-void QueuedTrajectoryExecutor::abort()
+void QueuedTrajectoryExecutor::cancel()
 {
   std::lock_guard<std::mutex> lock(mMutex);
   DART_UNUSED(lock); // Suppress unused variable warning
 
-  std::exception_ptr abort
-      = std::make_exception_ptr(std::runtime_error("Trajectory aborted."));
+  std::exception_ptr cancel
+      = std::make_exception_ptr(std::runtime_error("Trajectory canceled."));
 
   if (mInProgress)
   {
-    mExecutor->abort();
+    mExecutor->cancel();
 
-    // Set our own exception, since abort may not be supported
+    // Set our own exception, since cancel may not be supported
     auto promise = mPromiseQueue.front();
     mPromiseQueue.pop();
-    promise->set_exception(abort);
+    promise->set_exception(cancel);
 
     mInProgress = false;
   }
@@ -115,7 +115,7 @@ void QueuedTrajectoryExecutor::abort()
   {
     auto promise = mPromiseQueue.front();
     mPromiseQueue.pop();
-    promise->set_exception(abort);
+    promise->set_exception(cancel);
 
     mTrajectoryQueue.pop();
   }

src/control/ros/RosTrajectoryExecutor.cpp

@@ -1,4 +1,5 @@
 #include "aikido/control/ros/RosTrajectoryExecutor.hpp"
+#include <dart/common/Console.hpp>
 #include "aikido/control/TrajectoryRunningException.hpp"
 #include "aikido/control/ros/Conversions.hpp"
 #include "aikido/control/ros/RosTrajectoryExecutionException.hpp"
@@ -232,9 +233,20 @@ void RosTrajectoryExecutor::step(
 }
 
 //==============================================================================
-void RosTrajectoryExecutor::abort()
+void RosTrajectoryExecutor::cancel()
 {
-  // TODO: cancel the actionlib goal (once there is support in ReWD controller)
+  std::lock_guard<std::mutex> lock(mMutex);
+  DART_UNUSED(lock); // Suppress unused variable warning.
+
+  if (mInProgress)
+  {
+    mGoalHandle.cancel();
+  }
+  else
+  {
+    dtwarn << "[RosTrajectoryExecutor::cancel] Attempting to "
+           << "cancel trajectory, but no trajectory in progress.\n";
+  }
 }
 
 } // namespace ros

tests/control/test_KinematicSimulationTrajectoryExecutor.cpp

@@ -236,15 +236,16 @@ TEST_F(
 }
 
 TEST_F(
-    KinematicSimulationTrajectoryExecutorTest, abort_TrajectoryInProgress_Halts)
+    KinematicSimulationTrajectoryExecutorTest,
+    cancel_TrajectoryInProgress_Halts)
 {
   KinematicSimulationTrajectoryExecutor executor(mSkeleton);
 
   auto simulationClock = std::chrono::system_clock::now();
   auto future = executor.execute(mTraj);
   future.wait_for(waitTime);
   executor.step(simulationClock + stepTime);
-  executor.abort();
+  executor.cancel();
 
   EXPECT_THROW(future.get(), std::runtime_error);
 

tests/control/test_QueuedTrajectoryExecutor.cpp

@@ -221,7 +221,7 @@ TEST_F(QueuedTrajectoryExecutorTest, step_NegativeTimepoint_NoThrows)
 
 TEST_F(
     QueuedTrajectoryExecutorTest,
-    abort_NoRunningTrajectories_QueuedTrajectoriesAborted)
+    cancel_NoRunningTrajectories_QueuedTrajectoriesCanceled)
 {
   QueuedTrajectoryExecutor executor(std::move(mExecutor));
 
@@ -230,7 +230,7 @@ TEST_F(
   auto f1 = executor.execute(mTraj1);
   auto f2 = executor.execute(mTraj2);
 
-  executor.abort();
+  executor.cancel();
 
   EXPECT_EQ(f1.wait_for(waitTime), std::future_status::ready);
   EXPECT_EQ(f2.wait_for(waitTime), std::future_status::ready);
@@ -243,7 +243,7 @@ TEST_F(
 
 TEST_F(
     QueuedTrajectoryExecutorTest,
-    abort_OneRunningTrajectory_QueuedTrajectoriesAborted)
+    cancel_OneRunningTrajectory_QueuedTrajectoriesCanceled)
 {
   QueuedTrajectoryExecutor executor(std::move(mExecutor));
 
@@ -260,7 +260,7 @@ TEST_F(
   simulationClock += stepTime;
   executor.step(simulationClock);
 
-  executor.abort();
+  executor.cancel();
 
   EXPECT_EQ(f1.wait_for(waitTime), std::future_status::ready);
   EXPECT_EQ(f2.wait_for(waitTime), std::future_status::ready);