diff --git a/include/moveit_task_constructor/introspection.h b/include/moveit_task_constructor/introspection.h index 972d76d1..9dcd7101 100644 --- a/include/moveit_task_constructor/introspection.h +++ b/include/moveit_task_constructor/introspection.h @@ -7,6 +7,7 @@ #include #include #include +#include #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 stage_to_id_map_; + boost::bimap 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); }; } } diff --git a/include/moveit_task_constructor/storage.h b/include/moveit_task_constructor/storage.h index d5145556..011c0b61 100644 --- a/include/moveit_task_constructor/storage.h +++ b/include/moveit_task_constructor/storage.h @@ -27,43 +27,7 @@ MOVEIT_CLASS_FORWARD(InterfaceState) MOVEIT_CLASS_FORWARD(Interface) typedef std::weak_ptr 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 -class Repository : private std::deque { - typedef std::deque 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 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 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 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::instance().add(this); - } - ~SolutionBase() { - Repository::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 diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 286c3e5d..fd9600f3 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -6,9 +6,6 @@ #include "container.h" #include - -#include -#include #include #include @@ -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; diff --git a/msg/StageStatistics.msg b/msg/StageStatistics.msg index 36652bf7..a498365e 100644 --- a/msg/StageStatistics.msg +++ b/msg/StageStatistics.msg @@ -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 diff --git a/msg/SubSolution.msg b/msg/SubSolution.msg index 148b1314..88264af3 100644 --- a/msg/SubSolution.msg +++ b/msg/SubSolution.msg @@ -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 diff --git a/msg/SubTrajectory.msg b/msg/SubTrajectory.msg index b6db3a02..95ba52b0 100644 --- a/msg/SubTrajectory.msg +++ b/msg/SubTrajectory.msg @@ -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 diff --git a/src/container.cpp b/src/container.cpp index 17f9cdc3..4d6a18ee 100644 --- a/src/container.cpp +++ b/src/container.cpp @@ -1,5 +1,6 @@ #include "container_p.h" +#include #include #include @@ -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){ diff --git a/src/container_p.h b/src/container_p.h index 3897aaa9..f6bd2e82 100644 --- a/src/container_p.h +++ b/src/container_p.h @@ -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: diff --git a/src/introspection.cpp b/src/introspection.cpp index 83058188..2246f30f 100644 --- a/src/introspection.cpp +++ b/src/introspection.cpp @@ -1,3 +1,4 @@ +#include "stage_p.h" #include #include #include @@ -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(DESCRIPTION_TOPIC, 1, true); task_statistics_publisher_ = nh_.advertise(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::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; +} + } } diff --git a/src/storage.cpp b/src/storage.cpp index 90f2a3cd..a9a0a5ef 100644 --- a/src/storage.cpp +++ b/src/storage.cpp @@ -1,34 +1,22 @@ #include "stage_p.h" #include +#include #include #include +#include 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::instance().remove(this); - id_ = 0; -} - -void InterfaceState::registerID() -{ - id_ = Repository::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); } diff --git a/src/task.cpp b/src/task.cpp index 5406aedb..be04059d 100644 --- a/src/task.cpp +++ b/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 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 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; -} - } }