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:
Jafar Abdi 2022-01-02 17:32:37 +03:00 committed by GitHub
parent f9c0a894f9
commit 7dbe0b87e1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 19 additions and 11 deletions

View File

@ -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;

View File

@ -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();

View File

@ -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();

View File

@ -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() {