mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Return MoveItErrorCode from task::plan (#319)
... to know whether the plan failed due to timeout, preemption, or actual planning failure
This commit is contained in:
parent
f9c0a894f9
commit
7dbe0b87e1
@ -47,6 +47,7 @@
|
||||
#include <moveit/macros/class_forward.h>
|
||||
|
||||
#include <moveit_msgs/MoveItErrorCodes.h>
|
||||
#include <moveit/utils/moveit_error_code.h>
|
||||
|
||||
namespace moveit {
|
||||
namespace core {
|
||||
@ -117,11 +118,11 @@ public:
|
||||
void init();
|
||||
|
||||
/// reset, init scene (if not yet done), and init all stages, then start planning
|
||||
bool plan(size_t max_solutions = 0);
|
||||
moveit::core::MoveItErrorCode plan(size_t max_solutions = 0);
|
||||
/// interrupt current planning (or execution)
|
||||
void preempt();
|
||||
/// execute solution, return the result
|
||||
moveit_msgs::MoveItErrorCodes execute(const SolutionBase& s);
|
||||
moveit::core::MoveItErrorCode execute(const SolutionBase& s);
|
||||
|
||||
/// print current task state (number of found solutions and propagated states) to std::cout
|
||||
void printState(std::ostream& os = std::cout) const;
|
||||
|
||||
@ -246,30 +246,37 @@ void Task::compute() {
|
||||
stages()->pimpl()->runCompute();
|
||||
}
|
||||
|
||||
bool Task::plan(size_t max_solutions) {
|
||||
moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
|
||||
auto impl = pimpl();
|
||||
init();
|
||||
|
||||
// Print state and return success if there are solutions otherwise the input error_code
|
||||
const auto success_or = [this](const int32_t error_code) {
|
||||
printState();
|
||||
return numSolutions() > 0 ? moveit::core::MoveItErrorCode::SUCCESS : error_code;
|
||||
};
|
||||
impl->preempt_requested_ = false;
|
||||
const double available_time = timeout();
|
||||
const auto start_time = std::chrono::steady_clock::now();
|
||||
while (!impl->preempt_requested_ && canCompute() && (max_solutions == 0 || numSolutions() < max_solutions) &&
|
||||
std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() < available_time) {
|
||||
while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) {
|
||||
if (impl->preempt_requested_)
|
||||
return success_or(moveit::core::MoveItErrorCode::PREEMPTED);
|
||||
if (std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() > available_time)
|
||||
return success_or(moveit::core::MoveItErrorCode::TIMED_OUT);
|
||||
compute();
|
||||
for (const auto& cb : impl->task_cbs_)
|
||||
cb(*this);
|
||||
if (impl->introspection_)
|
||||
impl->introspection_->publishTaskState();
|
||||
}
|
||||
printState();
|
||||
return numSolutions() > 0;
|
||||
};
|
||||
return success_or(moveit::core::MoveItErrorCode::PLANNING_FAILED);
|
||||
}
|
||||
|
||||
void Task::preempt() {
|
||||
pimpl()->preempt_requested_ = true;
|
||||
}
|
||||
|
||||
moveit_msgs::MoveItErrorCodes Task::execute(const SolutionBase& s) {
|
||||
moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
|
||||
actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> ac("execute_task_solution");
|
||||
ac.waitForServer();
|
||||
|
||||
|
||||
@ -655,7 +655,7 @@ TEST(Task, timeout) {
|
||||
// zero timeout fails
|
||||
t.reset();
|
||||
t.setTimeout(0.0);
|
||||
EXPECT_FALSE(t.plan());
|
||||
EXPECT_EQ(t.plan(), moveit::core::MoveItErrorCode::TIMED_OUT);
|
||||
|
||||
// time for 1 solution
|
||||
t.reset();
|
||||
|
||||
@ -490,7 +490,7 @@ bool PickPlaceTask::plan() {
|
||||
ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions");
|
||||
int max_solutions = pnh_.param<int>("max_solutions", 10);
|
||||
|
||||
return task_->plan(max_solutions);
|
||||
return static_cast<bool>(task_->plan(max_solutions));
|
||||
}
|
||||
|
||||
bool PickPlaceTask::execute() {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user