reworked handling of ids for stages + solutions

- moved fillTaskDescription() + fillTaskStatistics to Introspection class
- replaced global InterfaceState + Solution repositories
  with local maps stored in Introspection class
- ids are set on demand when filling ROS messages
This commit is contained in:
Robert Haschke 2017-11-10 10:23:19 +01:00
parent 622603268f
commit 9dae631a1d
11 changed files with 180 additions and 208 deletions

View File

@ -7,6 +7,7 @@
#include <moveit_task_constructor/TaskStatistics.h>
#include <moveit_task_constructor/Solution.h>
#include <moveit_task_constructor/GetSolution.h>
#include <boost/bimap.hpp>
#define DESCRIPTION_TOPIC "description"
#define STATISTICS_TOPIC "statistics"
@ -14,6 +15,7 @@
namespace moveit { namespace task_constructor {
MOVEIT_CLASS_FORWARD(Stage)
MOVEIT_CLASS_FORWARD(Task)
MOVEIT_CLASS_FORWARD(SolutionBase)
@ -34,13 +36,21 @@ class Introspection {
/// services to provide an individual Solution
ros::ServiceServer get_solution_service_;
/// mapping from stages to their id
std::map<const void*, moveit_task_constructor::StageStatistics::_id_type> stage_to_id_map_;
boost::bimap<uint32_t, const SolutionBase*> id_solution_bimap_;
public:
Introspection(const Task &task);
Introspection(const Introspection &other) = delete;
/// fill task description message for publishing the task configuration
moveit_task_constructor::TaskDescription& fillTaskDescription(moveit_task_constructor::TaskDescription& msg);
/// publish detailed task description
void publishTaskDescription();
/// fill task state message for publishing the current task state
moveit_task_constructor::TaskStatistics& fillTaskStatistics(moveit_task_constructor::TaskStatistics& msg);
/// publish the current state of task
void publishTaskState();
@ -53,11 +63,22 @@ public:
void operator()(const SolutionBase &s) { publishSolution(s); }
/// publish all top-level solutions of task
void publishAllSolutions(bool wait = true);
void publishAllSolutions(bool wait = true) const;
/// get solution
bool getSolution(moveit_task_constructor::GetSolution::Request &req,
moveit_task_constructor::GetSolution::Response &res);
/// retrieve id of given stage
uint32_t stageId(const moveit::task_constructor::Stage * const s) const;
/// retrieve or set id of given solution
uint32_t solutionId(const moveit::task_constructor::SolutionBase &s);
private:
void fillStageStatistics(const Stage &stage, moveit_task_constructor::StageStatistics &s);
/// retrieve or set id of given stage
uint32_t stageId(const moveit::task_constructor::Stage * const s);
};
} }

View File

@ -27,43 +27,7 @@ MOVEIT_CLASS_FORWARD(InterfaceState)
MOVEIT_CLASS_FORWARD(Interface)
typedef std::weak_ptr<Interface> InterfaceWeakPtr;
MOVEIT_CLASS_FORWARD(Stage)
/** singleton repository for pointers of T
*
* Serves as an efficient mapping from IDs to the associated T instance.
* size() serves as the new ID after insertion of a new item,
* i.e. we have item IDs from 1 onwards. ID == 0 is invalid.
*/
template <typename T>
class Repository : private std::deque<const T*> {
typedef std::deque<const T*> base_type;
// non-copyable singleton
Repository() {}
Repository(const Repository&) = delete;
void operator=(const Repository&) = delete;
public:
static Repository& instance() {
static Repository instance_;
return instance_;
}
size_t add(const T* item) {
this->push_back(item);
return this->size();
}
void remove(const T* item) {
// invalidate entry
this->base_type::at(item->id()-1) = nullptr;
}
const T& operator[](size_t id) const {
return *this->base_type::at(id-1);
}
};
MOVEIT_CLASS_FORWARD(Introspection)
/** InterfaceState describes a potential start or goal state for a planning stage.
@ -73,8 +37,6 @@ public:
class InterfaceState {
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
public:
/// get ID of this state
size_t id() const { return id_; }
// TODO turn this into priority queue
typedef std::deque<SolutionBase*> Solutions;
@ -84,8 +46,6 @@ public:
/// copy an existing InterfaceState, but not including incoming/outgoing trajectories
InterfaceState(const InterfaceState& existing);
~InterfaceState();
inline const planning_scene::PlanningSceneConstPtr& scene() const { return scene_; }
inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; }
inline const Solutions& outgoingTrajectories() const { return outgoing_trajectories_; }
@ -96,9 +56,6 @@ private:
inline void addOutgoing(SolutionBase* t) { outgoing_trajectories_.push_back(t); }
private:
inline void registerID();
size_t id_;
planning_scene::PlanningSceneConstPtr scene_;
// TODO: add PropertyMap: std::map<std::string, std::any> to allow passing of parameters or attributes
// TODO: add visualization_msgs::MarkerArray to allow for any complex visualization of the state
@ -162,8 +119,6 @@ typedef std::vector<const SubTrajectory*> SolutionTrajectory;
class SolutionBase {
public:
// retrieve ID of this solution in repository
inline size_t id() const { return id_; }
// TODO: get rid of creator (only used in SerialContainer)
inline const StagePrivate* creator() const { return creator_; }
inline double cost() const { return cost_; }
@ -185,22 +140,16 @@ public:
void setCost(double cost);
/// append this solution to Solution msg
virtual void fillMessage(::moveit_task_constructor::Solution &solution) const = 0;
virtual void fillMessage(::moveit_task_constructor::Solution &solution,
Introspection* introspection = nullptr) const = 0;
protected:
SolutionBase(StagePrivate* creator, double cost)
: creator_(creator), cost_(cost)
{
id_ = Repository<SolutionBase>::instance().add(this);
}
~SolutionBase() {
Repository<SolutionBase>::instance().remove(this);
id_ = 0; // invalidate ID
}
private:
// unique ID of this solution
size_t id_;
// back-pointer to creating stage, allows to access sub-solutions
StagePrivate *creator_;
// associated cost
@ -223,7 +172,8 @@ public:
const std::string& name() const { return name_; }
void setName(const std::string& name) { name_ = name; }
void fillMessage(::moveit_task_constructor::Solution &msg) const override;
void fillMessage(::moveit_task_constructor::Solution &msg,
Introspection* introspection = nullptr) const override;
private:
// trajectories could have a name, e.g. a generator could name its solutions

View File

@ -6,9 +6,6 @@
#include "container.h"
#include <moveit_task_constructor/introspection.h>
#include <moveit_task_constructor/TaskDescription.h>
#include <moveit_task_constructor/TaskStatistics.h>
#include <moveit_task_constructor/Solution.h>
#include <moveit/macros/class_forward.h>
@ -64,10 +61,6 @@ public:
bool plan();
/// print current state std::cout
static void printState(const Task &t);
/// fill task description message for publishing the task configuration
moveit_task_constructor::TaskDescription& fillTaskDescription(moveit_task_constructor::TaskDescription& msg) const;
/// fill task state message for publishing the current task state
moveit_task_constructor::TaskStatistics& fillTaskStatistics(moveit_task_constructor::TaskStatistics& msg) const;
size_t numSolutions() const override;

View File

@ -6,16 +6,6 @@ uint32 id
# parent id, parent_id == id means root
uint32 parent_id
# The order of the following IDs will change depending on their associated cost.
# However, the IDs themselves will always refer to the same underlying entity.
# IDs of received start / end states
uint32[] received_starts
uint32[] received_ends
# IDs of generated start / end states
uint32[] generated_starts
uint32[] generated_ends
# successful and failed solution IDs of this stage
uint32[] solved
uint32[] failed

View File

@ -4,5 +4,8 @@ uint32 id
# associated cost
float32 cost
# id of stage that created this trajectory
uint32 stage_id
# IDs of subsolutions
uint32[] sub_solution_id

View File

@ -7,12 +7,13 @@ float32 cost
# optional associated name of sub trajectory
string name
# id of stage that created this trajectory
uint32 stage_id
# trajectory
moveit_msgs/RobotTrajectory trajectory
# markers, e.g. providing additional hints or illustrating failure
visualization_msgs/MarkerArray markers
# ID of end state
uint32 end_id
# planning scene of end state as diff w.r.t. start state
moveit_msgs/PlanningScene scene_diff

View File

@ -1,5 +1,6 @@
#include "container_p.h"
#include <moveit_task_constructor/introspection.h>
#include <ros/console.h>
#include <memory>
@ -414,23 +415,30 @@ bool SerialContainer::traverse(const SolutionBase &start, const SolutionProcesso
return result;
}
void SerialSolution::fillMessage(::moveit_task_constructor::Solution &msg) const
void SerialSolution::fillMessage(::moveit_task_constructor::Solution &msg,
Introspection* introspection = nullptr) const
{
::moveit_task_constructor::SubSolution sub_msg;
sub_msg.id = this->id();
sub_msg.id = introspection ? introspection->solutionId(*this) : 0;
sub_msg.cost = this->cost();
const Introspection *ci = introspection;
sub_msg.stage_id = ci ? ci->stageId(this->creator()->me()) : 0;
sub_msg.sub_solution_id.reserve(subsolutions_.size());
if (introspection) {
for (const SolutionBase* s : subsolutions_)
sub_msg.sub_solution_id.push_back(s->id());
sub_msg.sub_solution_id.push_back(introspection->solutionId(*s));
msg.sub_solution.push_back(sub_msg);
}
msg.sub_trajectory.reserve(msg.sub_trajectory.size() + subsolutions_.size());
for (const SolutionBase* s : subsolutions_)
s->fillMessage(msg);
s->fillMessage(msg, introspection);
}
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::__cxx11::string &name)
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::string &name)
: ContainerBasePrivate(me, name)
{
starts_.reset(new Interface([me](const Interface::iterator& external){

View File

@ -71,7 +71,7 @@ public:
: SolutionBase(creator, cost), subsolutions_(subsolutions)
{}
/// append all subsolutions to solution
void fillMessage(::moveit_task_constructor::Solution &msg) const override;
void fillMessage(::moveit_task_constructor::Solution &msg, Introspection *introspection) const override;
private:
/// series of sub solutions
@ -123,8 +123,9 @@ public:
explicit WrappedSolution(StagePrivate* creator, SolutionBase* wrapped)
: SolutionBase(creator, wrapped->cost()), wrapped_(wrapped)
{}
void fillMessage(::moveit_task_constructor::Solution &solution) const override {
wrapped_->fillMessage(solution);
void fillMessage(::moveit_task_constructor::Solution &solution,
Introspection* introspection = nullptr) const override {
wrapped_->fillMessage(solution, introspection);
}
private:

View File

@ -1,3 +1,4 @@
#include "stage_p.h"
#include <moveit_task_constructor/introspection.h>
#include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/storage.h>
@ -11,6 +12,8 @@ Introspection::Introspection(const Task &task)
: nh_(std::string("~/") + task.id()) // topics + services are advertised in private namespace
, task_(task)
{
stage_to_id_map_[&task_] = 0; // root is task having ID = 0
task_description_publisher_ = nh_.advertise<moveit_task_constructor::TaskDescription>(DESCRIPTION_TOPIC, 1, true);
task_statistics_publisher_ = nh_.advertise<moveit_task_constructor::TaskStatistics>(STATISTICS_TOPIC, 1);
solution_publisher_ = nh_.advertise<::moveit_task_constructor::Solution>(SOLUTION_TOPIC, 1, true);
@ -21,29 +24,39 @@ Introspection::Introspection(const Task &task)
void Introspection::publishTaskDescription()
{
::moveit_task_constructor::TaskDescription msg;
task_description_publisher_.publish(task_.fillTaskDescription(msg));
task_description_publisher_.publish(fillTaskDescription(msg));
}
void Introspection::publishTaskState()
{
::moveit_task_constructor::TaskStatistics msg;
task_statistics_publisher_.publish(task_.fillTaskStatistics(msg));
task_statistics_publisher_.publish(fillTaskStatistics(msg));
}
void Introspection::reset()
{
// send empty task description message to indicate reset
::moveit_task_constructor::TaskDescription msg;
msg.id = task_.id();
task_description_publisher_.publish(msg);
// reset maps
stage_to_id_map_.clear();
stage_to_id_map_[&task_] = 0; // root is task having ID = 0
id_solution_bimap_.clear();
}
void Introspection::publishSolution(const SolutionBase &s)
{
::moveit_task_constructor::Solution msg;
s.fillMessage(msg);
s.fillMessage(msg, this);
msg.task_id = task_.id();
s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
solution_publisher_.publish(msg);
}
void Introspection::publishAllSolutions(bool wait)
void Introspection::publishAllSolutions(bool wait) const
{
Task::SolutionProcessor processor
= [this, wait](const ::moveit_task_constructor::Solution& msg, double cost) {
@ -64,11 +77,107 @@ void Introspection::publishAllSolutions(bool wait)
bool Introspection::getSolution(moveit_task_constructor::GetSolution::Request &req,
moveit_task_constructor::GetSolution::Response &res)
{
::moveit_task_constructor::Solution msg;
const SolutionBase& solution = Repository<SolutionBase>::instance()[req.solution_id];
solution.fillMessage(msg);
res.solution = msg;
auto it = id_solution_bimap_.left.find(req.solution_id);
if (it == id_solution_bimap_.left.end())
return false;
const SolutionBase& solution = *it->second;
solution.fillMessage(res.solution, this);
return true;
}
uint32_t Introspection::stageId(const Stage* const s)
{
return stage_to_id_map_.insert(std::make_pair(s, stage_to_id_map_.size())).first->second;
}
uint32_t Introspection::stageId(const Stage* const s) const
{
auto it = stage_to_id_map_.find(s);
if (it == stage_to_id_map_.end())
throw std::runtime_error("unknown stage");
return it->second;
}
uint32_t Introspection::solutionId(const SolutionBase& s)
{
auto result = id_solution_bimap_.left.insert(std::make_pair(id_solution_bimap_.size(), &s));
return result.first->first;
}
void Introspection::fillStageStatistics(const Stage& stage, moveit_task_constructor::StageStatistics& s)
{
// successfull solutions
Stage::SolutionProcessor solutionProcessor = [this, &s](const SolutionBase& solution) {
s.solved.push_back(solutionId(solution));
return true;
};
stage.processSolutions(solutionProcessor);
// failed solution attempts
solutionProcessor = [this, &s](const SolutionBase& solution) {
s.failed.push_back(solutionId(solution));
return true;
};
stage.processFailures(solutionProcessor);
}
moveit_task_constructor::TaskDescription& Introspection::fillTaskDescription(moveit_task_constructor::TaskDescription &msg)
{
ContainerBase::StageCallback stageProcessor =
[this, &msg](const Stage& stage, int) -> bool {
// this method is called for each child stage of a given parent
moveit_task_constructor::StageDescription desc;
moveit_task_constructor::StageStatistics stat;
desc.id = stat.id = stageId(&stage);
desc.name = stage.name();
desc.flags = stage.pimpl()->interfaceFlags();
// TODO fill stage properties
auto it = stage_to_id_map_.find(stage.pimpl()->parent());
assert (it != stage_to_id_map_.cend());
desc.parent_id = stat.parent_id = it->second;
fillStageStatistics(stage, stat);
// finally store in msg
msg.description.push_back(std::move(desc));
msg.statistics.push_back(std::move(stat));
return true;
};
msg.description.clear();
msg.statistics.clear();
task_.stages()->traverseRecursively(stageProcessor);
msg.id = task_.id();
return msg;
}
moveit_task_constructor::TaskStatistics& Introspection::fillTaskStatistics(moveit_task_constructor::TaskStatistics &msg)
{
ContainerBase::StageCallback stageProcessor =
[this, &msg](const Stage& stage, int) -> bool {
// this method is called for each child stage of a given parent
moveit_task_constructor::StageStatistics stat; // create new Stage msg
stat.id = stageId(&stage);
auto it = stage_to_id_map_.find(stage.pimpl()->parent());
assert (it != stage_to_id_map_.cend());
stat.parent_id = it->second;
fillStageStatistics(stage, stat);
// finally store in msg.stages
msg.stages.push_back(std::move(stat));
return true;
};
msg.stages.clear();
task_.stages()->traverseRecursively(stageProcessor);
msg.id = task_.id();
return msg;
}
} }

View File

@ -1,34 +1,22 @@
#include "stage_p.h"
#include <moveit_task_constructor/storage.h>
#include <moveit_task_constructor/introspection.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit { namespace task_constructor {
InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr &ps)
: scene_(ps)
{
registerID();
}
InterfaceState::InterfaceState(const InterfaceState &existing)
: scene_(existing.scene())
{
registerID();
}
InterfaceState::~InterfaceState()
{
Repository<InterfaceState>::instance().remove(this);
id_ = 0;
}
void InterfaceState::registerID()
{
id_ = Repository<InterfaceState>::instance().add(this);
}
Interface::Interface(const Interface::NotifyFunction &notify)
: notify_(notify)
{}
@ -64,15 +52,22 @@ void SolutionBase::setCost(double cost) {
}
void SubTrajectory::fillMessage(moveit_task_constructor::Solution &msg) const {
void SubTrajectory::fillMessage(moveit_task_constructor::Solution &msg,
Introspection *introspection) const {
msg.sub_trajectory.emplace_back();
moveit_task_constructor::SubTrajectory& t = msg.sub_trajectory.back();
t.id = this->id();
t.id = introspection ? introspection->solutionId(*this) : 0;
t.cost = this->cost();
t.name = this->name();
const Introspection *ci = introspection;
t.stage_id = ci ? ci->stageId(this->creator()->me()) : 0;
if (trajectory())
trajectory()->getRobotTrajectoryMsg(t.trajectory);
t.markers = this->markers();
this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
}

View File

@ -239,103 +239,4 @@ void Task::printState(const Task &t){
t.stages()->traverseRecursively(processor);
}
void fillInterfaceList(moveit_task_constructor::StageStatistics::_received_starts_type &c,
const InterfaceConstPtr& interface) {
c.clear();
if (!interface) return;
for (const InterfaceState& state : *interface)
c.push_back(state.id());
}
void fillStageStatistics(const Stage& stage, moveit_task_constructor::StageStatistics& s)
{
const StagePrivate *simpl = stage.pimpl();
fillInterfaceList(s.received_starts, simpl->starts());
fillInterfaceList(s.received_ends, simpl->ends());
fillInterfaceList(s.generated_starts, simpl->nextStarts());
fillInterfaceList(s.generated_ends, simpl->prevEnds());
// insert solution IDs
Stage::SolutionProcessor solutionProcessor = [&s](const SolutionBase& solution) {
s.solved.push_back(solution.id());
return true;
};
stage.processSolutions(solutionProcessor);
solutionProcessor = [&s](const SolutionBase& solution) {
s.failed.push_back(solution.id());
return true;
};
stage.processFailures(solutionProcessor);
}
moveit_task_constructor::TaskDescription& Task::fillTaskDescription(moveit_task_constructor::TaskDescription &msg) const
{
std::map<const Stage*, moveit_task_constructor::StageStatistics::_id_type> stage_to_id_map;
stage_to_id_map[this] = 0; // ID for root
ContainerBase::StageCallback stageProcessor =
[&stage_to_id_map, &msg](const Stage& stage, int) -> bool {
// this method is called for each child stage of a given parent
moveit_task_constructor::StageDescription desc;
moveit_task_constructor::StageStatistics stat;
desc.id = stat.id = stage_to_id_map.size();
stage_to_id_map[&stage] = stat.id;
desc.name = stage.name();
desc.flags = stage.pimpl()->interfaceFlags();
// TODO fill stage properties
auto it = stage_to_id_map.find(stage.pimpl()->parent());
assert (it != stage_to_id_map.cend());
desc.parent_id = stat.parent_id = it->second;
fillStageStatistics(stage, stat);
// finally store in msg
msg.description.push_back(std::move(desc));
msg.statistics.push_back(std::move(stat));
return true;
};
msg.description.clear();
msg.statistics.clear();
stages()->traverseRecursively(stageProcessor);
msg.id = id();
return msg;
}
moveit_task_constructor::TaskStatistics& Task::fillTaskStatistics(moveit_task_constructor::TaskStatistics &msg) const
{
std::map<const Stage*, moveit_task_constructor::StageStatistics::_id_type> stage_to_id_map;
stage_to_id_map[this] = 0; // ID for root
ContainerBase::StageCallback stageProcessor =
[&stage_to_id_map, &msg](const Stage& stage, int) -> bool {
// this method is called for each child stage of a given parent
moveit_task_constructor::StageStatistics stat; // create new Stage msg
stat.id = stage_to_id_map.size();
stage_to_id_map[&stage] = stat.id;
auto it = stage_to_id_map.find(stage.pimpl()->parent());
assert (it != stage_to_id_map.cend());
stat.parent_id = it->second;
fillStageStatistics(stage, stat);
// finally store in msg.stages
msg.stages.push_back(std::move(stat));
return true;
};
msg.stages.clear();
stages()->traverseRecursively(stageProcessor);
msg.id = id();
return msg;
}
} }