diff --git a/CHANGELOG.md b/CHANGELOG.md index 16c4f8c521..53e2e7729e 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/include/aikido/control/InstantaneousTrajectoryExecutor.hpp b/include/aikido/control/InstantaneousTrajectoryExecutor.hpp index 7fb07c295d..d48e32de9a 100644 --- a/include/aikido/control/InstantaneousTrajectoryExecutor.hpp +++ b/include/aikido/control/InstantaneousTrajectoryExecutor.hpp @@ -38,12 +38,12 @@ class InstantaneousTrajectoryExecutor : public TrajectoryExecutor std::future 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 diff --git a/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp b/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp index 0a623108d3..875909f87c 100644 --- a/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp +++ b/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 diff --git a/include/aikido/control/QueuedTrajectoryExecutor.hpp b/include/aikido/control/QueuedTrajectoryExecutor.hpp index de984c9a56..1a3247e0f6 100644 --- a/include/aikido/control/QueuedTrajectoryExecutor.hpp +++ b/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 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 diff --git a/include/aikido/control/TrajectoryExecutor.hpp b/include/aikido/control/TrajectoryExecutor.hpp index ef0d0c4e81..20c3f2a30b 100644 --- a/include/aikido/control/TrajectoryExecutor.hpp +++ b/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 diff --git a/include/aikido/control/ros/RosTrajectoryExecutor.hpp b/include/aikido/control/ros/RosTrajectoryExecutor.hpp index bad514508c..cf6b852812 100644 --- a/include/aikido/control/ros/RosTrajectoryExecutor.hpp +++ b/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 diff --git a/src/control/InstantaneousTrajectoryExecutor.cpp b/src/control/InstantaneousTrajectoryExecutor.cpp index fc13f0151a..db87095094 100644 --- a/src/control/InstantaneousTrajectoryExecutor.cpp +++ b/src/control/InstantaneousTrajectoryExecutor.cpp @@ -77,7 +77,7 @@ void InstantaneousTrajectoryExecutor::step( } //============================================================================== -void InstantaneousTrajectoryExecutor::abort() +void InstantaneousTrajectoryExecutor::cancel() { // Do nothing } diff --git a/src/control/KinematicSimulationTrajectoryExecutor.cpp b/src/control/KinematicSimulationTrajectoryExecutor.cpp index bd6ebbb635..906424bc22 100644 --- a/src/control/KinematicSimulationTrajectoryExecutor.cpp +++ b/src/control/KinematicSimulationTrajectoryExecutor.cpp @@ -1,4 +1,5 @@ #include "aikido/control/KinematicSimulationTrajectoryExecutor.hpp" +#include #include #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 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"; } } diff --git a/src/control/QueuedTrajectoryExecutor.cpp b/src/control/QueuedTrajectoryExecutor.cpp index 597d2527af..cbc71fb984 100644 --- a/src/control/QueuedTrajectoryExecutor.cpp +++ b/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 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(); } diff --git a/src/control/ros/RosTrajectoryExecutor.cpp b/src/control/ros/RosTrajectoryExecutor.cpp index e0eb7ab6e3..9080c074d0 100644 --- a/src/control/ros/RosTrajectoryExecutor.cpp +++ b/src/control/ros/RosTrajectoryExecutor.cpp @@ -1,4 +1,5 @@ #include "aikido/control/ros/RosTrajectoryExecutor.hpp" +#include #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 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 diff --git a/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp b/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp index 3536c526a4..0b0e0a6951 100644 --- a/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp +++ b/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp @@ -236,7 +236,8 @@ TEST_F( } TEST_F( - KinematicSimulationTrajectoryExecutorTest, abort_TrajectoryInProgress_Halts) + KinematicSimulationTrajectoryExecutorTest, + cancel_TrajectoryInProgress_Halts) { KinematicSimulationTrajectoryExecutor executor(mSkeleton); @@ -244,7 +245,7 @@ TEST_F( auto future = executor.execute(mTraj); future.wait_for(waitTime); executor.step(simulationClock + stepTime); - executor.abort(); + executor.cancel(); EXPECT_THROW(future.get(), std::runtime_error); diff --git a/tests/control/test_QueuedTrajectoryExecutor.cpp b/tests/control/test_QueuedTrajectoryExecutor.cpp index b20e3c4fee..0b16812f17 100644 --- a/tests/control/test_QueuedTrajectoryExecutor.cpp +++ b/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);