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/TaskStatistics.h>
|
||||||
#include <moveit_task_constructor/Solution.h>
|
#include <moveit_task_constructor/Solution.h>
|
||||||
#include <moveit_task_constructor/GetSolution.h>
|
#include <moveit_task_constructor/GetSolution.h>
|
||||||
|
#include <boost/bimap.hpp>
|
||||||
|
|
||||||
#define DESCRIPTION_TOPIC "description"
|
#define DESCRIPTION_TOPIC "description"
|
||||||
#define STATISTICS_TOPIC "statistics"
|
#define STATISTICS_TOPIC "statistics"
|
||||||
@ -14,6 +15,7 @@
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
|
MOVEIT_CLASS_FORWARD(Stage)
|
||||||
MOVEIT_CLASS_FORWARD(Task)
|
MOVEIT_CLASS_FORWARD(Task)
|
||||||
MOVEIT_CLASS_FORWARD(SolutionBase)
|
MOVEIT_CLASS_FORWARD(SolutionBase)
|
||||||
|
|
||||||
@ -34,13 +36,21 @@ class Introspection {
|
|||||||
/// services to provide an individual Solution
|
/// services to provide an individual Solution
|
||||||
ros::ServiceServer get_solution_service_;
|
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:
|
public:
|
||||||
Introspection(const Task &task);
|
Introspection(const Task &task);
|
||||||
Introspection(const Introspection &other) = delete;
|
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
|
/// publish detailed task description
|
||||||
void publishTaskDescription();
|
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
|
/// publish the current state of task
|
||||||
void publishTaskState();
|
void publishTaskState();
|
||||||
|
|
||||||
@ -53,11 +63,22 @@ public:
|
|||||||
void operator()(const SolutionBase &s) { publishSolution(s); }
|
void operator()(const SolutionBase &s) { publishSolution(s); }
|
||||||
|
|
||||||
/// publish all top-level solutions of task
|
/// publish all top-level solutions of task
|
||||||
void publishAllSolutions(bool wait = true);
|
void publishAllSolutions(bool wait = true) const;
|
||||||
|
|
||||||
/// get solution
|
/// get solution
|
||||||
bool getSolution(moveit_task_constructor::GetSolution::Request &req,
|
bool getSolution(moveit_task_constructor::GetSolution::Request &req,
|
||||||
moveit_task_constructor::GetSolution::Response &res);
|
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)
|
MOVEIT_CLASS_FORWARD(Interface)
|
||||||
typedef std::weak_ptr<Interface> InterfaceWeakPtr;
|
typedef std::weak_ptr<Interface> InterfaceWeakPtr;
|
||||||
MOVEIT_CLASS_FORWARD(Stage)
|
MOVEIT_CLASS_FORWARD(Stage)
|
||||||
|
MOVEIT_CLASS_FORWARD(Introspection)
|
||||||
|
|
||||||
/** 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);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
/** InterfaceState describes a potential start or goal state for a planning stage.
|
/** InterfaceState describes a potential start or goal state for a planning stage.
|
||||||
@ -73,8 +37,6 @@ public:
|
|||||||
class InterfaceState {
|
class InterfaceState {
|
||||||
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
|
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
|
||||||
public:
|
public:
|
||||||
/// get ID of this state
|
|
||||||
size_t id() const { return id_; }
|
|
||||||
// TODO turn this into priority queue
|
// TODO turn this into priority queue
|
||||||
typedef std::deque<SolutionBase*> Solutions;
|
typedef std::deque<SolutionBase*> Solutions;
|
||||||
|
|
||||||
@ -84,8 +46,6 @@ public:
|
|||||||
/// copy an existing InterfaceState, but not including incoming/outgoing trajectories
|
/// copy an existing InterfaceState, but not including incoming/outgoing trajectories
|
||||||
InterfaceState(const InterfaceState& existing);
|
InterfaceState(const InterfaceState& existing);
|
||||||
|
|
||||||
~InterfaceState();
|
|
||||||
|
|
||||||
inline const planning_scene::PlanningSceneConstPtr& scene() const { return scene_; }
|
inline const planning_scene::PlanningSceneConstPtr& scene() const { return scene_; }
|
||||||
inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; }
|
inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; }
|
||||||
inline const Solutions& outgoingTrajectories() const { return outgoing_trajectories_; }
|
inline const Solutions& outgoingTrajectories() const { return outgoing_trajectories_; }
|
||||||
@ -96,9 +56,6 @@ private:
|
|||||||
inline void addOutgoing(SolutionBase* t) { outgoing_trajectories_.push_back(t); }
|
inline void addOutgoing(SolutionBase* t) { outgoing_trajectories_.push_back(t); }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
inline void registerID();
|
|
||||||
|
|
||||||
size_t id_;
|
|
||||||
planning_scene::PlanningSceneConstPtr scene_;
|
planning_scene::PlanningSceneConstPtr scene_;
|
||||||
// TODO: add PropertyMap: std::map<std::string, std::any> to allow passing of parameters or attributes
|
// 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
|
// 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 {
|
class SolutionBase {
|
||||||
public:
|
public:
|
||||||
// retrieve ID of this solution in repository
|
|
||||||
inline size_t id() const { return id_; }
|
|
||||||
// TODO: get rid of creator (only used in SerialContainer)
|
// TODO: get rid of creator (only used in SerialContainer)
|
||||||
inline const StagePrivate* creator() const { return creator_; }
|
inline const StagePrivate* creator() const { return creator_; }
|
||||||
inline double cost() const { return cost_; }
|
inline double cost() const { return cost_; }
|
||||||
@ -185,22 +140,16 @@ public:
|
|||||||
void setCost(double cost);
|
void setCost(double cost);
|
||||||
|
|
||||||
/// append this solution to Solution msg
|
/// 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:
|
protected:
|
||||||
SolutionBase(StagePrivate* creator, double cost)
|
SolutionBase(StagePrivate* creator, double cost)
|
||||||
: creator_(creator), cost_(cost)
|
: creator_(creator), cost_(cost)
|
||||||
{
|
{
|
||||||
id_ = Repository<SolutionBase>::instance().add(this);
|
|
||||||
}
|
|
||||||
~SolutionBase() {
|
|
||||||
Repository<SolutionBase>::instance().remove(this);
|
|
||||||
id_ = 0; // invalidate ID
|
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// unique ID of this solution
|
|
||||||
size_t id_;
|
|
||||||
// back-pointer to creating stage, allows to access sub-solutions
|
// back-pointer to creating stage, allows to access sub-solutions
|
||||||
StagePrivate *creator_;
|
StagePrivate *creator_;
|
||||||
// associated cost
|
// associated cost
|
||||||
@ -223,7 +172,8 @@ public:
|
|||||||
const std::string& name() const { return name_; }
|
const std::string& name() const { return name_; }
|
||||||
void setName(const std::string& name) { name_ = 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:
|
private:
|
||||||
// trajectories could have a name, e.g. a generator could name its solutions
|
// trajectories could have a name, e.g. a generator could name its solutions
|
||||||
|
|||||||
@ -6,9 +6,6 @@
|
|||||||
#include "container.h"
|
#include "container.h"
|
||||||
|
|
||||||
#include <moveit_task_constructor/introspection.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_task_constructor/Solution.h>
|
||||||
|
|
||||||
#include <moveit/macros/class_forward.h>
|
#include <moveit/macros/class_forward.h>
|
||||||
@ -64,10 +61,6 @@ public:
|
|||||||
bool plan();
|
bool plan();
|
||||||
/// print current state std::cout
|
/// print current state std::cout
|
||||||
static void printState(const Task &t);
|
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;
|
size_t numSolutions() const override;
|
||||||
|
|
||||||
|
|||||||
@ -6,16 +6,6 @@ uint32 id
|
|||||||
# parent id, parent_id == id means root
|
# parent id, parent_id == id means root
|
||||||
uint32 parent_id
|
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
|
# successful and failed solution IDs of this stage
|
||||||
uint32[] solved
|
uint32[] solved
|
||||||
uint32[] failed
|
uint32[] failed
|
||||||
|
|||||||
@ -4,5 +4,8 @@ uint32 id
|
|||||||
# associated cost
|
# associated cost
|
||||||
float32 cost
|
float32 cost
|
||||||
|
|
||||||
|
# id of stage that created this trajectory
|
||||||
|
uint32 stage_id
|
||||||
|
|
||||||
# IDs of subsolutions
|
# IDs of subsolutions
|
||||||
uint32[] sub_solution_id
|
uint32[] sub_solution_id
|
||||||
|
|||||||
@ -7,12 +7,13 @@ float32 cost
|
|||||||
# optional associated name of sub trajectory
|
# optional associated name of sub trajectory
|
||||||
string name
|
string name
|
||||||
|
|
||||||
|
# id of stage that created this trajectory
|
||||||
|
uint32 stage_id
|
||||||
|
|
||||||
# trajectory
|
# trajectory
|
||||||
moveit_msgs/RobotTrajectory trajectory
|
moveit_msgs/RobotTrajectory trajectory
|
||||||
# markers, e.g. providing additional hints or illustrating failure
|
# markers, e.g. providing additional hints or illustrating failure
|
||||||
visualization_msgs/MarkerArray markers
|
visualization_msgs/MarkerArray markers
|
||||||
|
|
||||||
# ID of end state
|
|
||||||
uint32 end_id
|
|
||||||
# planning scene of end state as diff w.r.t. start state
|
# planning scene of end state as diff w.r.t. start state
|
||||||
moveit_msgs/PlanningScene scene_diff
|
moveit_msgs/PlanningScene scene_diff
|
||||||
|
|||||||
@ -1,5 +1,6 @@
|
|||||||
#include "container_p.h"
|
#include "container_p.h"
|
||||||
|
|
||||||
|
#include <moveit_task_constructor/introspection.h>
|
||||||
#include <ros/console.h>
|
#include <ros/console.h>
|
||||||
|
|
||||||
#include <memory>
|
#include <memory>
|
||||||
@ -414,23 +415,30 @@ bool SerialContainer::traverse(const SolutionBase &start, const SolutionProcesso
|
|||||||
return result;
|
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;
|
::moveit_task_constructor::SubSolution sub_msg;
|
||||||
sub_msg.id = this->id();
|
sub_msg.id = introspection ? introspection->solutionId(*this) : 0;
|
||||||
sub_msg.cost = this->cost();
|
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());
|
sub_msg.sub_solution_id.reserve(subsolutions_.size());
|
||||||
for (const SolutionBase* s : subsolutions_)
|
if (introspection) {
|
||||||
sub_msg.sub_solution_id.push_back(s->id());
|
for (const SolutionBase* s : subsolutions_)
|
||||||
msg.sub_solution.push_back(sub_msg);
|
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());
|
msg.sub_trajectory.reserve(msg.sub_trajectory.size() + subsolutions_.size());
|
||||||
for (const SolutionBase* s : subsolutions_)
|
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)
|
: ContainerBasePrivate(me, name)
|
||||||
{
|
{
|
||||||
starts_.reset(new Interface([me](const Interface::iterator& external){
|
starts_.reset(new Interface([me](const Interface::iterator& external){
|
||||||
|
|||||||
@ -71,7 +71,7 @@ public:
|
|||||||
: SolutionBase(creator, cost), subsolutions_(subsolutions)
|
: SolutionBase(creator, cost), subsolutions_(subsolutions)
|
||||||
{}
|
{}
|
||||||
/// append all subsolutions to solution
|
/// 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:
|
private:
|
||||||
/// series of sub solutions
|
/// series of sub solutions
|
||||||
@ -123,8 +123,9 @@ public:
|
|||||||
explicit WrappedSolution(StagePrivate* creator, SolutionBase* wrapped)
|
explicit WrappedSolution(StagePrivate* creator, SolutionBase* wrapped)
|
||||||
: SolutionBase(creator, wrapped->cost()), wrapped_(wrapped)
|
: SolutionBase(creator, wrapped->cost()), wrapped_(wrapped)
|
||||||
{}
|
{}
|
||||||
void fillMessage(::moveit_task_constructor::Solution &solution) const override {
|
void fillMessage(::moveit_task_constructor::Solution &solution,
|
||||||
wrapped_->fillMessage(solution);
|
Introspection* introspection = nullptr) const override {
|
||||||
|
wrapped_->fillMessage(solution, introspection);
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
|
#include "stage_p.h"
|
||||||
#include <moveit_task_constructor/introspection.h>
|
#include <moveit_task_constructor/introspection.h>
|
||||||
#include <moveit_task_constructor/task.h>
|
#include <moveit_task_constructor/task.h>
|
||||||
#include <moveit_task_constructor/storage.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
|
: nh_(std::string("~/") + task.id()) // topics + services are advertised in private namespace
|
||||||
, task_(task)
|
, 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_description_publisher_ = nh_.advertise<moveit_task_constructor::TaskDescription>(DESCRIPTION_TOPIC, 1, true);
|
||||||
task_statistics_publisher_ = nh_.advertise<moveit_task_constructor::TaskStatistics>(STATISTICS_TOPIC, 1);
|
task_statistics_publisher_ = nh_.advertise<moveit_task_constructor::TaskStatistics>(STATISTICS_TOPIC, 1);
|
||||||
solution_publisher_ = nh_.advertise<::moveit_task_constructor::Solution>(SOLUTION_TOPIC, 1, true);
|
solution_publisher_ = nh_.advertise<::moveit_task_constructor::Solution>(SOLUTION_TOPIC, 1, true);
|
||||||
@ -21,29 +24,39 @@ Introspection::Introspection(const Task &task)
|
|||||||
void Introspection::publishTaskDescription()
|
void Introspection::publishTaskDescription()
|
||||||
{
|
{
|
||||||
::moveit_task_constructor::TaskDescription msg;
|
::moveit_task_constructor::TaskDescription msg;
|
||||||
task_description_publisher_.publish(task_.fillTaskDescription(msg));
|
task_description_publisher_.publish(fillTaskDescription(msg));
|
||||||
}
|
}
|
||||||
|
|
||||||
void Introspection::publishTaskState()
|
void Introspection::publishTaskState()
|
||||||
{
|
{
|
||||||
::moveit_task_constructor::TaskStatistics msg;
|
::moveit_task_constructor::TaskStatistics msg;
|
||||||
task_statistics_publisher_.publish(task_.fillTaskStatistics(msg));
|
task_statistics_publisher_.publish(fillTaskStatistics(msg));
|
||||||
}
|
}
|
||||||
|
|
||||||
void Introspection::reset()
|
void Introspection::reset()
|
||||||
{
|
{
|
||||||
|
// send empty task description message to indicate reset
|
||||||
::moveit_task_constructor::TaskDescription msg;
|
::moveit_task_constructor::TaskDescription msg;
|
||||||
|
msg.id = task_.id();
|
||||||
task_description_publisher_.publish(msg);
|
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)
|
void Introspection::publishSolution(const SolutionBase &s)
|
||||||
{
|
{
|
||||||
::moveit_task_constructor::Solution msg;
|
::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);
|
solution_publisher_.publish(msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Introspection::publishAllSolutions(bool wait)
|
void Introspection::publishAllSolutions(bool wait) const
|
||||||
{
|
{
|
||||||
Task::SolutionProcessor processor
|
Task::SolutionProcessor processor
|
||||||
= [this, wait](const ::moveit_task_constructor::Solution& msg, double cost) {
|
= [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,
|
bool Introspection::getSolution(moveit_task_constructor::GetSolution::Request &req,
|
||||||
moveit_task_constructor::GetSolution::Response &res)
|
moveit_task_constructor::GetSolution::Response &res)
|
||||||
{
|
{
|
||||||
::moveit_task_constructor::Solution msg;
|
auto it = id_solution_bimap_.left.find(req.solution_id);
|
||||||
const SolutionBase& solution = Repository<SolutionBase>::instance()[req.solution_id];
|
if (it == id_solution_bimap_.left.end())
|
||||||
solution.fillMessage(msg);
|
return false;
|
||||||
res.solution = msg;
|
|
||||||
|
const SolutionBase& solution = *it->second;
|
||||||
|
solution.fillMessage(res.solution, this);
|
||||||
return true;
|
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 "stage_p.h"
|
||||||
#include <moveit_task_constructor/storage.h>
|
#include <moveit_task_constructor/storage.h>
|
||||||
|
#include <moveit_task_constructor/introspection.h>
|
||||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||||
#include <moveit/robot_state/conversions.h>
|
#include <moveit/robot_state/conversions.h>
|
||||||
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr &ps)
|
InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr &ps)
|
||||||
: scene_(ps)
|
: scene_(ps)
|
||||||
{
|
{
|
||||||
registerID();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
InterfaceState::InterfaceState(const InterfaceState &existing)
|
InterfaceState::InterfaceState(const InterfaceState &existing)
|
||||||
: scene_(existing.scene())
|
: 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)
|
Interface::Interface(const Interface::NotifyFunction ¬ify)
|
||||||
: notify_(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();
|
msg.sub_trajectory.emplace_back();
|
||||||
moveit_task_constructor::SubTrajectory& t = msg.sub_trajectory.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.cost = this->cost();
|
||||||
t.name = this->name();
|
t.name = this->name();
|
||||||
|
|
||||||
|
const Introspection *ci = introspection;
|
||||||
|
t.stage_id = ci ? ci->stageId(this->creator()->me()) : 0;
|
||||||
|
|
||||||
if (trajectory())
|
if (trajectory())
|
||||||
trajectory()->getRobotTrajectoryMsg(t.trajectory);
|
trajectory()->getRobotTrajectoryMsg(t.trajectory);
|
||||||
t.markers = this->markers();
|
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);
|
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