allow handling of failures

- store "failure" solutions to facilitate debugging
- Introspection assigns solution IDs as soon as they are created in a stage
  Thus, solution IDs represent their creation order.
  In contrast, the order of publishing (in StageStatistics) should
  represent the cost order.
- Storing failures is disabled if Introspection is not available.
This commit is contained in:
Robert Haschke 2018-01-12 13:50:44 +01:00
parent 57cd11e3a9
commit 97bb6ef2c6
18 changed files with 232 additions and 73 deletions

View File

@ -62,7 +62,7 @@ int main(int argc, char** argv){
{
auto move= std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
move->addSolutionCallback(std::ref(t.introspection()));
move->addSolutionCallback(std::bind(&Introspection::publishSolution, &t.introspection(), std::placeholders::_1));
move->setGroup("left_arm");
move->setLink("l_gripper_tool_frame");
move->setMinMaxDistance(.03, 0.1);

View File

@ -64,7 +64,7 @@ int main(int argc, char** argv){
{
auto move = std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
// move->addSolutionCallback(std::ref(t.introspection()));
// move->addSolutionCallback(std::bind(&Introspection::publishSolution, &t.introspection(), std::placeholders::_1));
move->properties().configureInitFrom(Stage::PARENT);
move->setMinMaxDistance(.03, 0.1);
move->setCartesianStepSize(0.02);

View File

@ -90,6 +90,7 @@ public:
size_t numSolutions() const override;
void processSolutions(const SolutionProcessor &processor) const override;
void processFailures(const SolutionProcessor &processor) const override {}
/// container used to represent a serial solution
typedef std::vector<const SolutionBase*> solution_container;
@ -181,6 +182,7 @@ public:
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
size_t numSolutions() const override;
void processFailures(const SolutionProcessor &processor) const override {}
/// insertion is only allowed if children() is empty
bool insert(Stage::pointer&& stage, int before = -1) override;
@ -215,6 +217,7 @@ public:
size_t numSolutions() const override;
void processSolutions(const SolutionProcessor &processor) const override;
void processFailures(const SolutionProcessor &processor) const override;
void spawn(InterfaceState &&state, std::unique_ptr<SolutionBase>&& s);
void spawn(InterfaceState &&state, double cost) {

View File

@ -65,6 +65,7 @@ public:
typedef StagePrivate::container_type container_type;
typedef container_type::iterator iterator;
typedef container_type::const_iterator const_iterator;
typedef std::function<bool(Stage&, int depth)> NonConstStageCallback;
inline const container_type& children() const { return children_; }
@ -73,10 +74,19 @@ public:
* Contrary to std::advance(), iterator limits are considered. */
const_iterator position(int index) const;
/// traversing all stages upto max_depth
/// traversing all stages up to max_depth
bool traverseStages(const ContainerBase::StageCallback &processor,
unsigned int cur_depth, unsigned int max_depth) const;
/// non-const version
bool traverseStages(const NonConstStageCallback &processor,
unsigned int cur_depth, unsigned int max_depth) {
const auto& const_processor = [&processor](const Stage& stage, unsigned int depth) {
return processor(const_cast<Stage&>(stage), depth);
};
return const_cast<const ContainerBasePrivate*>(this)->traverseStages(const_processor, cur_depth, max_depth);
}
// forward these methods to the public interface for containers
bool canCompute() const override;
bool compute() override;
@ -182,6 +192,8 @@ public:
private:
std::list<std::unique_ptr<SolutionBase>> solutions_;
std::list<std::unique_ptr<SolutionBase>> failures_;
std::list<InterfaceState> failure_states_;
};
PIMPL_FUNCTIONS(Wrapper)

View File

@ -82,10 +82,11 @@ public:
/// indicate that this task was reset
void reset();
/// register the given solution, assigning a unique ID
void registerSolution(const SolutionBase &s);
/// publish the given solution
void publishSolution(const SolutionBase &s);
/// operator version for use in stage callbacks
void operator()(const SolutionBase &s) { publishSolution(s); }
/// publish all top-level solutions of task
void publishAllSolutions(bool wait = true);

View File

@ -144,17 +144,20 @@ public:
void setName(const std::string& name);
virtual size_t numSolutions() const = 0;
bool storeFailures() const;
typedef std::function<bool(const SolutionBase&)> SolutionProcessor;
/// process all solutions, calling the callback for each of them
/// process all solutions in cost order, calling the callback for each of them
virtual void processSolutions(const SolutionProcessor &processor) const = 0;
virtual void processFailures(const SolutionProcessor &processor) const {}
/// process all failures, calling the callback for each of them
virtual void processFailures(const SolutionProcessor &processor) const = 0;
typedef std::function<void(const SolutionBase &s)> SolutionCallback;
typedef std::list<SolutionCallback> SolutionCallbackList;
/// add function to be called for every newly found solution
/// add function to be called for every newly found solution or failure
SolutionCallbackList::const_iterator addSolutionCallback(SolutionCallback &&cb);
/// remove function callback
void erase(SolutionCallbackList::const_iterator which);
void removeSolutionCallback(SolutionCallbackList::const_iterator which);
PropertyMap& properties();
const PropertyMap& properties() const {

View File

@ -87,6 +87,7 @@ public:
}
inline void setPrevEnds(const InterfacePtr& prev_ends) { prev_ends_ = prev_ends; }
inline void setNextStarts(const InterfacePtr& next_starts) { next_starts_ = next_starts; }
inline void setIntrospection(Introspection* introspection) { introspection_ = introspection; }
void newSolution(const SolutionBase &current);
@ -108,6 +109,8 @@ private:
InterfaceWeakPtr prev_ends_; // interface to be used for sendBackward()
InterfaceWeakPtr next_starts_; // interface to be used for sendForward()
Introspection* introspection_; // task's introspection instance
};
PIMPL_FUNCTIONS(Stage)
std::ostream& operator<<(std::ostream &os, const StagePrivate& stage);
@ -127,7 +130,8 @@ public:
SubTrajectory& addTrajectory(SubTrajectory&& trajectory);
private:
std::list<SubTrajectory> trajectories_;
std::list<SubTrajectory> solutions_;
std::list<SubTrajectory> failures_;
};
PIMPL_FUNCTIONS(ComputeBase)

View File

@ -179,6 +179,7 @@ public:
inline double cost() const { return cost_; }
void setCost(double cost);
inline bool isFailure() const { return !std::isfinite(cost_); }
const std::string& name() const { return name_; }
void setName(const std::string& name) { name_ = name; }

View File

@ -68,7 +68,7 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
for (auto &stage : children_) {
if (!processor(*stage, cur_depth))
continue;
ContainerBasePrivate *container = dynamic_cast<ContainerBasePrivate*>(stage->pimpl());
const ContainerBasePrivate *container = dynamic_cast<const ContainerBasePrivate*>(stage->pimpl());
if (container)
container->traverseStages(processor, cur_depth+1, max_depth);
}
@ -292,7 +292,7 @@ void SerialContainerPrivate::storeNewSolution(SerialContainer::solution_containe
}
// perform default stage action on new solution
newSolution(solutions_.back());
newSolution(solution);
}
@ -610,6 +610,13 @@ void Wrapper::processSolutions(const Stage::SolutionProcessor &processor) const
break;
}
void Wrapper::processFailures(const Stage::SolutionProcessor &processor) const
{
for(const auto& s : pimpl()->failures_)
if (!processor(*s))
break;
}
// TODO: allow stages to directly execute this code
// This requires that SolutionBase::creator_ is a Stage* (not StagePrivate*)
void Wrapper::spawn(InterfaceState &&state, std::unique_ptr<SolutionBase>&& s)
@ -617,11 +624,16 @@ void Wrapper::spawn(InterfaceState &&state, std::unique_ptr<SolutionBase>&& s)
auto impl = pimpl();
s->setCreator(impl);
SolutionBase* solution = s.get();
impl->solutions_.emplace_back(std::move(s));
impl->prevEnds()->add(InterfaceState(state), NULL, solution);
impl->nextStarts()->add(std::move(state), solution, NULL);
if (s->isFailure()) {
impl->failure_states_.emplace_back(std::move(state));
s->setStartState(impl->failure_states_.back());
s->setEndState(impl->failure_states_.back());
impl->failures_.emplace_back(std::move(s));
} else {
impl->solutions_.emplace_back(std::move(s));
impl->prevEnds()->add(InterfaceState(state), NULL, solution);
impl->nextStarts()->add(std::move(state), solution, NULL);
}
impl->newSolution(*solution);
}

View File

@ -128,6 +128,11 @@ void Introspection::reset()
impl->resetMaps();
}
void Introspection::registerSolution(const SolutionBase &s)
{
solutionId(s);
}
void Introspection::fillSolution(moveit_task_constructor_msgs::Solution &msg,
const SolutionBase &s)
{

View File

@ -37,6 +37,7 @@
#include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h>
#include <iostream>
#include <iomanip>
#include <ros/console.h>
@ -68,7 +69,7 @@ std::ostream& operator<<(std::ostream &os, const InitStageException& e) {
StagePrivate::StagePrivate(Stage *me, const std::string &name)
: me_(me), name_(name), parent_(nullptr)
: me_(me), name_(name), parent_(nullptr), introspection_(nullptr)
{}
InterfaceFlags StagePrivate::interfaceFlags() const
@ -83,8 +84,17 @@ InterfaceFlags StagePrivate::interfaceFlags() const
void StagePrivate::newSolution(const SolutionBase &solution)
{
if (introspection_)
introspection_->registerSolution(solution);
// ignore invalid / failure solutions
if (solution.isFailure())
return;
// call solution callbacks for both, valid solutions and failures
for (const auto& cb : solution_cbs_)
cb(solution);
if (parent())
parent()->onNewSolution(solution);
}
@ -134,14 +144,18 @@ void Stage::setName(const std::string& name)
pimpl_->name_ = name;
}
bool Stage::storeFailures() const
{
return pimpl_->introspection_ != nullptr;
}
Stage::SolutionCallbackList::const_iterator Stage::addSolutionCallback(SolutionCallback &&cb)
{
auto impl = pimpl();
impl->solution_cbs_.emplace_back(std::move(cb));
return --impl->solution_cbs_.cend();
}
void Stage::erase(SolutionCallbackList::const_iterator which)
void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which)
{
pimpl()->solution_cbs_.erase(which);
}
@ -190,10 +204,17 @@ std::ostream& operator<<(std::ostream &os, const Stage& stage) {
return os;
}
SubTrajectory& ComputeBasePrivate::addTrajectory(SubTrajectory&& trajectory){
SubTrajectory& ComputeBasePrivate::addTrajectory(SubTrajectory&& trajectory) {
trajectory.setCreator(this);
trajectories_.emplace_back(trajectory);
return trajectories_.back();
if (!trajectory.isFailure()) {
solutions_.emplace_back(std::move(trajectory));
return solutions_.back();
} else if (me()->storeFailures()) {
// only store failures when introspection is enabled
failures_.emplace_back(std::move(trajectory));
return failures_.back();
} else
return trajectory;
}
@ -203,23 +224,26 @@ ComputeBase::ComputeBase(ComputeBasePrivate *impl)
}
size_t ComputeBase::numSolutions() const {
return pimpl()->trajectories_.size();
return pimpl()->solutions_.size();
}
void ComputeBase::processSolutions(const Stage::SolutionProcessor &processor) const
{
for (const auto& s : pimpl()->trajectories_)
for (const auto& s : pimpl()->solutions_)
if (!processor(s))
return;
}
void ComputeBase::processFailures(const Stage::SolutionProcessor &processor) const
{
// TODO
for (const auto& s : pimpl()->failures_)
if (!processor(s))
return;
}
void ComputeBase::reset() {
pimpl()->trajectories_.clear();
pimpl()->solutions_.clear();
pimpl()->failures_.clear();
Stage::reset();
}

View File

@ -141,8 +141,15 @@ void Task::enableIntrospection(bool enable)
{
if (enable && !introspection_)
introspection_.reset(new Introspection(*this));
else if (!enable && introspection_)
else if (!enable && introspection_) {
// reset introspection instance of all stages
pimpl()->setIntrospection(nullptr);
pimpl()->traverseStages([this](Stage& stage, int) {
stage.pimpl()->setIntrospection(nullptr);
return true;
}, 1, UINT_MAX);
introspection_.reset();
}
}
Introspection &Task::introspection()
@ -174,8 +181,15 @@ void Task::reset()
void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
{
WrapperBase::init(scene);
if (introspection_)
introspection_->publishTaskDescription();
// provide introspection instance to all stages
pimpl()->setIntrospection(introspection_.get());
pimpl()->traverseStages([this](Stage& stage, int) {
stage.pimpl()->setIntrospection(introspection_.get());
return true;
}, 1, UINT_MAX);
// first time publish task
introspection_->publishTaskDescription();
}
bool Task::canCompute() const

View File

@ -4,14 +4,16 @@
## Add gtest based cpp test target and link libraries
if (CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
add_rostest_gtest(${PROJECT_NAME}-test-stage test_stage.launch test_stage.cpp)
target_link_libraries(${PROJECT_NAME}-test-stage ${PROJECT_NAME} gtest_main)
catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp)
target_link_libraries(${PROJECT_NAME}-test-container
${PROJECT_NAME}
gtest_main)
target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} gtest_main)
catkin_add_gtest(${PROJECT_NAME}-test-properties test_properties.cpp)
target_link_libraries(${PROJECT_NAME}-test-properties
${PROJECT_NAME} gtest_main)
target_link_libraries(${PROJECT_NAME}-test-properties ${PROJECT_NAME} gtest_main)
endif()
add_executable(test_plan_current_state test_plan_current_state.cpp)

52
core/test/test_stage.cpp Normal file
View File

@ -0,0 +1,52 @@
#include <moveit/task_constructor/stage_p.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <ros/init.h>
#include <gtest/gtest.h>
#include <initializer_list>
using namespace moveit::task_constructor;
class TestGenerator : public Generator {
planning_scene::PlanningScenePtr ps;
InterfacePtr prev;
InterfacePtr next;
public:
TestGenerator() : Generator("generator") {
robot_model_loader::RobotModelLoader loader;
ps.reset(new planning_scene::PlanningScene(loader.getModel()));
prev.reset(new Interface(Interface::NotifyFunction()));
next.reset(new Interface(Interface::NotifyFunction()));
pimpl()->setPrevEnds(prev);
pimpl()->setNextStarts(next);
}
bool canCompute() const override { return true; }
bool compute() override {
spawn(InterfaceState(ps), 0.0);
return true;
}
};
TEST(Stage, registerCallbacks) {
int argc = 0;
char *argv = nullptr;
ros::init(argc, &argv, "testLocalTaskModel");
TestGenerator g;
uint called = 0;
auto cb = [&called](const SolutionBase &s) {
++called;
return true;
};
auto it = g.addSolutionCallback(cb);
g.compute();
EXPECT_EQ(called, 1);
g.removeSolutionCallback(it);
g.compute();
EXPECT_EQ(called, 1);
}

View File

@ -0,0 +1,7 @@
<launch>
<include file="$(find moveit_resources)/fanuc_moveit_config/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<test pkg="moveit_task_constructor_core"
type="moveit_task_constructor_core-test-stage" test-name="test_stage" />
</launch>

View File

@ -278,8 +278,7 @@ void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs:
}
Node *n = it->second;
bool changed = n->solutions_->processSolutionIDs(s.solved, std::numeric_limits<double>::quiet_NaN()) ||
n->solutions_->processSolutionIDs(s.failed, std::numeric_limits<double>::infinity());
bool changed = n->solutions_->processSolutionIDs(s.solved, s.failed);
// emit notify about model changes when node was already visited
if (changed && (n->node_flags_ & WAS_VISITED)) {
QModelIndex idx = index(n);
@ -354,10 +353,15 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index)
// request solution via service
moveit_task_constructor_msgs::GetSolution srv;
srv.request.solution_id = id;
if (get_solution_client_->call(srv)) {
id_to_solution_[id] = result = processSolutionMessage(srv.response.solution);
} else { // on failure mark remote task as destroyed: don't retrieve more solutions
flags_ |= IS_DESTROYED;
try {
if (get_solution_client_->call(srv)) {
id_to_solution_[id] = result = processSolutionMessage(srv.response.solution);
return result;
} else { // on failure mark remote task as destroyed: don't retrieve more solutions
flags_ |= IS_DESTROYED;
}
} catch (const std::exception& e) {
ROS_ERROR("exception: %s", e.what());
}
}
return result;
@ -502,8 +506,8 @@ void RemoteSolutionModel::sortInternal()
comp = left->name.compare(right->name);
break;
}
if (comp == 0)
comp = (left->creation_rank < right->creation_rank ? -1 : 1);
if (comp == 0) // if still undecided, id decides
comp = (left->id < right->id ? -1 : 1);
return (sort_order_ == Qt::AscendingOrder) ? (comp < 0) : (comp >= 0);
});
}
@ -527,39 +531,49 @@ void RemoteSolutionModel::sortInternal()
}
// process solution ids received in stage statistics
bool RemoteSolutionModel::processSolutionIDs(const std::vector<uint32_t> &ids, double default_cost)
bool RemoteSolutionModel::processSolutionIDs(const std::vector<uint32_t> &successful,
const std::vector<uint32_t> &failed)
{
// ids are originally ordered by cost, order them by creation order here
std::vector<std::pair<uint32_t, uint32_t>> ids_by_creation;
uint32_t rank = 0;
for (uint32_t id : ids)
ids_by_creation.push_back(std::make_pair(id, ++rank));
std::sort(ids_by_creation.begin(), ids_by_creation.end(),
[](const auto& left, const auto& right) { return left.first < right.first; });
// are there any new items?
bool changed = (successful.size() + failed.size() != data_.size());
bool was_empty = data_.empty();
bool size_changed = false;
for (const auto &p : ids_by_creation) {
if (data_.empty() || p.first > data_.back().id) { // new item
data_.emplace_back(Data(p.first, default_cost, 1+data_.size(), p.second));
size_changed |= isVisible(data_.back());
} else {
// find id in available data_
auto data_it = detail::findById(data_, p.first);
Q_ASSERT(data_it != data_.end());
bool was_visible = isVisible(*data_it);
// and update cost rank
data_it->cost_rank = p.second;
size_changed |= (isVisible(*data_it) != was_visible);
}
}
auto last = --data_.end();
// append new items to the end of data_
processSolutionIDs(successful, true);
processSolutionIDs(failed, false);
// assign creation rank to new items
uint32_t rank = was_empty ? 0 : last->creation_rank;
for (auto it = was_empty ? data_.begin() : ++last, end = data_.end(); it != end; ++it)
it->creation_rank = ++rank;
sortInternal();
return size_changed;
return changed;
}
void RemoteSolutionModel::processSolutionIDs(const std::vector<uint32_t> &ids, bool successful)
{
// ids are ordered by cost, insert them into data_ list sorted by id
double default_cost = successful ? std::numeric_limits<double>::quiet_NaN()
: std::numeric_limits<double>::infinity();
uint32_t cost_rank = 0;
for (const uint32_t id : ids) {
Data item(id, default_cost, successful ? ++cost_rank : std::numeric_limits<uint32_t>::max());
// find id in available data_
auto p = std::equal_range(data_.begin(), data_.end(), item);
if (p.first == p.second) { // new item
data_.insert(p.second, std::move(item));
} else { // existing item: update cost rank
Q_ASSERT(p.first->id == id);
p.first->cost_rank = item.cost_rank;
}
}
}
bool RemoteSolutionModel::isVisible(const RemoteSolutionModel::Data &item) const
{
return std::isnan(item.cost) || item.cost < max_cost_;
return std::isnan(item.cost) || item.cost <= max_cost_;
}
}

View File

@ -100,8 +100,12 @@ class RemoteSolutionModel : public QAbstractTableModel {
uint32_t creation_rank; // rank, ordered by creation
uint32_t cost_rank; // rank, ordering by cost
Data(uint32_t id, float cost, uint32_t creation_rank, uint32_t cost_rank)
: id(id), cost(cost), creation_rank(creation_rank), cost_rank(cost_rank) {}
Data(uint32_t id, float cost, uint32_t cost_rank)
: id(id), cost(cost), creation_rank(0), cost_rank(cost_rank) {}
bool operator<(const Data& other) const {
return this->id < other.id;
}
};
// successful and failed solutions ordered by id / creation
typedef std::list<Data> DataList;
@ -111,10 +115,11 @@ class RemoteSolutionModel : public QAbstractTableModel {
// solutions ordered (by default according to cost)
int sort_column_ = -1;
Qt::SortOrder sort_order_ = Qt::AscendingOrder;
float max_cost_ = std::numeric_limits<float>::max();
double max_cost_ = std::numeric_limits<double>::infinity();
std::vector<DataList::iterator> sorted_;
inline bool isVisible (const Data& item) const;
void processSolutionIDs(const std::vector<uint32_t>& ids, bool successful);
void sortInternal();
public:
@ -130,7 +135,7 @@ public:
void sort(int column, Qt::SortOrder order);
void setData(uint32_t id, float cost, const QString &name);
bool processSolutionIDs(const std::vector<uint32_t>& ids, double default_cost);
bool processSolutionIDs(const std::vector<uint32_t> &successful, const std::vector<uint32_t> &failed);
};
}

View File

@ -96,7 +96,7 @@ void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& sta
ROS_ERROR("Solution for model '%s' but model '%s' was expected",
msg.start_scene.robot_model_name .c_str(),
start_scene->getRobotModel()->getName().c_str());
return;
throw std::runtime_error("invalid robot model");
}
// initialize parent scene from solution's start scene