mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
57cd11e3a9
commit
97bb6ef2c6
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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 {
|
||||
|
||||
@ -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 ¤t);
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
@ -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; }
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
@ -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)
|
||||
{
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
52
core/test/test_stage.cpp
Normal 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);
|
||||
}
|
||||
7
core/test/test_stage.launch
Normal file
7
core/test/test_stage.launch
Normal 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>
|
||||
@ -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_;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user