diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index d942debff..1814340ba 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -62,10 +62,7 @@ class PreemptStageException : public std::exception { public: explicit PreemptStageException() {} - const char* what() const noexcept override { - static const char* msg = ""; - return msg; - } + const char* what() const noexcept override { return ""; } }; class ContainerBase; @@ -174,9 +171,8 @@ class StagePrivate /** compute cost for solution through configured CostTerm */ void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution); - void setPreemptedCheck(const std::atomic* preempt_requested); - /// is the stage preempted ? defaults to false - bool preempted() const; + void setPreemptRequestedMember(const std::atomic* preempt_requested); + bool preempted() const { return preempt_requested_ != nullptr && *preempt_requested_; } protected: StagePrivate& operator=(StagePrivate&& other); diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 97786e9c2..d1c123740 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -130,7 +130,6 @@ class Task : protected WrapperBase /// interrupt current planning void preempt(); void resetPreemptRequest(); - bool isPreempted(); /// execute solution, return the result moveit::core::MoveItErrorCode execute(const SolutionBase& s); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 1fb7437d7..36187ed16 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -306,17 +306,10 @@ void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& } } -void StagePrivate::setPreemptedCheck(const std::atomic* preempt_requested) { +void StagePrivate::setPreemptRequestedMember(const std::atomic* preempt_requested) { preempt_requested_ = preempt_requested; } -bool StagePrivate::preempted() const { - if (preempt_requested_) - return *preempt_requested_; - - return false; -} - Stage::Stage(StagePrivate* impl) : pimpl_(impl) { assert(impl); auto& p = properties(); diff --git a/core/src/task.cpp b/core/src/task.cpp index 5b61c1b75..df7ff14a3 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -218,7 +218,7 @@ void Task::init() { impl->traverseStages( [introspection, impl](Stage& stage, int /*depth*/) { stage.pimpl()->setIntrospection(introspection); - stage.pimpl()->setPreemptedCheck(&impl->preempt_requested_); + stage.pimpl()->setPreemptRequestedMember(&impl->preempt_requested_); return true; }, 1, UINT_MAX); @@ -279,10 +279,6 @@ void Task::resetPreemptRequest() { pimpl()->preempt_requested_ = false; } -bool Task::isPreempted() { - return pimpl()->preempt_requested_; -} - moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) { actionlib::SimpleActionClient ac("execute_task_solution"); if (!ac.waitForServer(ros::Duration(0.5))) { diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 09f3ca6f1..5d7b722c9 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -702,17 +702,10 @@ TEST_F(TaskTestBase, preempt) { EXPECT_EQ(fwd2->runs_, 0u); EXPECT_TRUE(t.plan(1)); // make sure the preempt request has been resetted on the previous call to plan() - // prempt in between stage computation - // Failing stage(s): - // 0 - ← 0 → - 0 / task pipeline - // 1 - ← 1 → - 0 / GEN1 - // - 0 → 1 → - 1 / FWD1 - // - 1 → 0 → - 0 / FWD2 t.reset(); - resetMockupIds(); { std::thread thread{ [&ec, this] { ec = t.plan(1); } }; - std::this_thread::sleep_for(timeout); + std::this_thread::sleep_for(timeout / 2.0); t.preempt(); thread.join(); }