mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
622603268f
commit
9dae631a1d
@ -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);
|
||||
};
|
||||
|
||||
} }
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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());
|
||||
for (const SolutionBase* s : subsolutions_)
|
||||
sub_msg.sub_solution_id.push_back(s->id());
|
||||
msg.sub_solution.push_back(sub_msg);
|
||||
if (introspection) {
|
||||
for (const SolutionBase* s : subsolutions_)
|
||||
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){
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
@ -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 ¬ify)
|
||||
: 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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
99
src/task.cpp
99
src/task.cpp
@ -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;
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
Loading…
Reference in New Issue
Block a user