diff --git a/CMakeLists.txt b/CMakeLists.txt index a69ac809..7df517c6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.6.12) project(moveit_task_constructor) -set(MSG_DEPS trajectory_msgs moveit_msgs) +set(MSG_DEPS moveit_msgs visualization_msgs) find_package(catkin REQUIRED COMPONENTS roscpp @@ -18,12 +18,14 @@ add_message_files(DIRECTORY msg FILES Stage.msg Task.msg + SubSolution.msg + SubTrajectory.msg Solution.msg ) add_service_files(DIRECTORY srv FILES GetInterfaceState.srv - GetSolutionTrajectory.srv + GetSolution.srv ) generate_messages(DEPENDENCIES ${MSG_DEPS}) diff --git a/include/moveit_task_constructor/container.h b/include/moveit_task_constructor/container.h index 5ab1f8de..5f712906 100644 --- a/include/moveit_task_constructor/container.h +++ b/include/moveit_task_constructor/container.h @@ -28,11 +28,6 @@ public: virtual bool canCompute() const = 0; virtual bool compute() = 0; - size_t numSolutions() const = 0; - typedef std::function SolutionProcessor; - /// process all solutions, calling the callback for each of them - virtual void processSolutions(const SolutionProcessor &processor) const = 0; - /// called by a (direct) child when a new solution becomes available virtual void onNewSolution(SolutionBase& t) = 0; @@ -55,7 +50,7 @@ public: bool compute() override; size_t numSolutions() const override; - void processSolutions(const SolutionProcessor &processor) const; + void processSolutions(const SolutionProcessor &processor) const override; /// container used to represent a serial solution typedef std::vector solution_container; @@ -102,7 +97,7 @@ public: void init(const planning_scene::PlanningSceneConstPtr &scene) override; size_t numSolutions() const override; - void processSolutions(const SolutionProcessor &processor) const; + void processSolutions(const SolutionProcessor &processor) const override; protected: ParallelContainerBase(ParallelContainerBasePrivate* impl); diff --git a/include/moveit_task_constructor/stage.h b/include/moveit_task_constructor/stage.h index a11a1ecc..cb2496f6 100644 --- a/include/moveit_task_constructor/stage.h +++ b/include/moveit_task_constructor/stage.h @@ -81,6 +81,11 @@ public: const std::string& name() const; virtual size_t numSolutions() const = 0; + typedef std::function SolutionProcessor; + /// process all solutions, calling the callback for each of them + virtual void processSolutions(const SolutionProcessor &processor) const = 0; + virtual void processFailures(const SolutionProcessor &processor) const {} + protected: /// Stage can only be instantiated through derived classes Stage(StagePrivate *impl); @@ -97,13 +102,13 @@ public: PRIVATE_CLASS(ComputeBase) void reset() override; virtual size_t numSolutions() const override; + void processSolutions(const SolutionProcessor &processor) const override; + void processFailures(const SolutionProcessor &processor) const override; protected: /// ComputeBase can only be instantiated by derived classes in stage.cpp ComputeBase(ComputeBasePrivate* impl); - // TODO: Do we really need/want to expose the trajectories? - const std::list& trajectories() const; SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost); }; diff --git a/include/moveit_task_constructor/storage.h b/include/moveit_task_constructor/storage.h index bf03cbf2..c7d6c4ec 100644 --- a/include/moveit_task_constructor/storage.h +++ b/include/moveit_task_constructor/storage.h @@ -3,6 +3,8 @@ #pragma once #include +#include +#include #include #include @@ -181,8 +183,8 @@ public: } void setCost(double cost); - /// flatten this solution to full solution trajectory, appending to solution - virtual void flattenTo(SolutionTrajectory& solution) const = 0; + /// append this solution to Solution msg + virtual void fillMessage(::moveit_task_constructor::Solution &solution) const = 0; protected: SolutionBase(StagePrivate* creator, double cost) @@ -215,17 +217,20 @@ public: explicit SubTrajectory(StagePrivate* creator, const robot_trajectory::RobotTrajectoryPtr& traj, double cost); robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; } + const visualization_msgs::MarkerArray& markers() const { return markers_; } + const std::string& name() const { return name_; } void setName(const std::string& name) { name_ = name; } - void flattenTo(SolutionTrajectory& solution) const override { - solution.push_back(this); - } + void fillMessage(::moveit_task_constructor::Solution &msg) const override; private: - const robot_trajectory::RobotTrajectoryPtr trajectory_; // trajectories could have a name, e.g. a generator could name its solutions std::string name_; + // actual trajectory, might be empty + const robot_trajectory::RobotTrajectoryPtr trajectory_; + // additional markers + visualization_msgs::MarkerArray markers_; }; diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 574922c3..43d702db 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -7,6 +7,7 @@ #include #include +#include namespace robot_model_loader { MOVEIT_CLASS_FORWARD(RobotModelLoader) @@ -35,7 +36,7 @@ public: const std::string &planning_plugin_param_name = "planning_plugin", const std::string &adapter_plugins_param_name = "request_adapters"); - size_t id() const { return id_; } + std::string id() const; void add(Stage::pointer &&stage); void clear() override; @@ -65,7 +66,7 @@ public: /// For each solution, composed from several SubTrajectories, /// the vector of SubTrajectories as well as the associated costs are provided. /// Return true, if traversal should continue, otherwise false. - typedef std::function SolutionProcessor; /// process all solutions void processSolutions(const Task::SolutionProcessor &processor) const; diff --git a/msg/Solution.msg b/msg/Solution.msg index e948b630..f2a5e1af 100644 --- a/msg/Solution.msg +++ b/msg/Solution.msg @@ -1,8 +1,8 @@ -# unique id within task -uint32 id +# id of associated task +string task_id -# IDs of subsolutions, empty if ID refers to an actual SubTrajectory -uint32[] sub_solutions +# set of all sub solutions involved +SubSolution[] sub_solution -# associated cost -float32 cost +# (ordered) sequence of actual trajectories +SubTrajectory[] sub_trajectory diff --git a/msg/Stage.msg b/msg/Stage.msg index 3aa73d2d..001e2457 100644 --- a/msg/Stage.msg +++ b/msg/Stage.msg @@ -10,13 +10,16 @@ uint32 flags # parent id, parent_id == id means root uint32 parent_id -# ids of received start / end states +# 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 +# IDs of generated start / end states uint32[] generated_starts uint32[] generated_ends -# successful and failed solutions of this stage -Solution[] solutions -Solution[] failures +# successful and failed solution IDs of this stage +uint32[] solved +uint32[] failed diff --git a/msg/SubSolution.msg b/msg/SubSolution.msg new file mode 100644 index 00000000..db733bb5 --- /dev/null +++ b/msg/SubSolution.msg @@ -0,0 +1,12 @@ +# unique id within task +uint32 id + +# associated cost +float32 cost + +# IDs of start and end state? +#uint32 start_id +#uint32 end_id + +# IDs of subsolutions +uint32[] sub_solution_id diff --git a/msg/SubTrajectory.msg b/msg/SubTrajectory.msg new file mode 100644 index 00000000..8d66b149 --- /dev/null +++ b/msg/SubTrajectory.msg @@ -0,0 +1,22 @@ +# unique id within task +uint32 id + +# associated names +string name + +# associated cost +float32 cost + +# name of task stage +string stage + +# IDs of start and end state? +# Or have an array of them in Solution.msg? +#uint32 start_id +#uint32 end_id + +# trajectory +moveit_msgs/RobotTrajectory trajectory +visualization_msgs/MarkerArray markers + +# TODO: add PlanningScene diff w.r.t. previous end state? diff --git a/src/container.cpp b/src/container.cpp index 0d82dd85..d40f2b16 100644 --- a/src/container.cpp +++ b/src/container.cpp @@ -414,11 +414,19 @@ bool SerialContainer::traverse(const SolutionBase &start, const SolutionProcesso return result; } -void SerialSolution::flattenTo(std::vector &solution) const +void SerialSolution::fillMessage(::moveit_task_constructor::Solution &msg) const { - solution.reserve(solution.size() + subsolutions_.size()); + ::moveit_task_constructor::SubSolution sub_msg; + sub_msg.id = this->id(); + sub_msg.cost = this->cost(); + sub_msg.sub_solution_id.reserve(subsolutions_.size()); for (const SolutionBase* s : subsolutions_) - s->flattenTo(solution); + sub_msg.sub_solution_id.push_back(s->id()); + 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); } diff --git a/src/container_p.h b/src/container_p.h index a69a8422..3897aaa9 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 flattenTo(SolutionTrajectory& solution) const override; + void fillMessage(::moveit_task_constructor::Solution &msg) const override; private: /// series of sub solutions @@ -123,8 +123,8 @@ public: explicit WrappedSolution(StagePrivate* creator, SolutionBase* wrapped) : SolutionBase(creator, wrapped->cost()), wrapped_(wrapped) {} - void flattenTo(SolutionTrajectory& solution) const override { - wrapped_->flattenTo(solution); + void fillMessage(::moveit_task_constructor::Solution &solution) const override { + wrapped_->fillMessage(solution); } private: diff --git a/src/debug.cpp b/src/debug.cpp index 3d0c2543..d1a3c47b 100644 --- a/src/debug.cpp +++ b/src/debug.cpp @@ -3,32 +3,14 @@ #include #include -#include -#include -#include -#include namespace moveit { namespace task_constructor { -bool publishSolution(ros::Publisher& pub, const SolutionTrajectory& solution, double cost, bool wait){ - if (solution.empty()) - return true; - - moveit_msgs::DisplayTrajectory dt; - - const robot_state::RobotState& state = solution.front()->start()->scene()->getCurrentState(); - robot_state::robotStateToRobotStateMsg(state, dt.trajectory_start); - dt.model_id = state.getRobotModel()->getName(); - - for(const SubTrajectory* t : solution){ - if(t->trajectory()){ - dt.trajectory.emplace_back(); - t->trajectory()->getRobotTrajectoryMsg(dt.trajectory.back()); - } - } - +bool publishSolution(ros::Publisher& pub, + const moveit_task_constructor::Solution &msg, + double cost, bool wait){ std::cout << "publishing solution with cost: " << cost << std::endl; - pub.publish(dt); + pub.publish(msg); if (wait) { std::cout << "Press to continue ..." << std::endl; int ch = getchar(); @@ -40,7 +22,7 @@ bool publishSolution(ros::Publisher& pub, const SolutionTrajectory& solution, do void publishAllPlans(const Task &task, const std::string &topic, bool wait) { ros::NodeHandle n; - ros::Publisher pub = n.advertise(topic, 1, true); + ros::Publisher pub = n.advertise<::moveit_task_constructor::Solution>(topic, 1, true); Task::SolutionProcessor processor = std::bind( &publishSolution, pub, std::placeholders::_1, std::placeholders::_2, wait); @@ -51,16 +33,16 @@ void publishAllPlans(const Task &task, const std::string &topic, bool wait) { NewSolutionPublisher::NewSolutionPublisher(const std::string &topic) { ros::NodeHandle n; - pub_ = n.advertise(topic, 1, true); + pub_ = n.advertise<::moveit_task_constructor::Solution>(topic, 1, true); } void NewSolutionPublisher::operator()(const SolutionBase &s) { // flatten s into vector of SubTrajectories - SolutionTrajectory solution; - s.flattenTo(solution); + ::moveit_task_constructor::Solution msg; + s.fillMessage(msg); - publishSolution(pub_, solution, s.cost(), false); + publishSolution(pub_, msg, s.cost(), false); } } } diff --git a/src/stage.cpp b/src/stage.cpp index 1d4db6ae..1915a0e6 100644 --- a/src/stage.cpp +++ b/src/stage.cpp @@ -150,8 +150,16 @@ size_t ComputeBase::numSolutions() const { return pimpl()->trajectories_.size(); } -const std::list &ComputeBase::trajectories() const { - return pimpl()->trajectories_; +void ComputeBase::processSolutions(const Stage::SolutionProcessor &processor) const +{ + for (const auto& s : pimpl()->trajectories_) + if (!processor(s)) + return; +} + +void ComputeBase::processFailures(const Stage::SolutionProcessor &processor) const +{ + // TODO } void ComputeBase::reset() { diff --git a/src/storage.cpp b/src/storage.cpp index 76b0954b..a20da89d 100644 --- a/src/storage.cpp +++ b/src/storage.cpp @@ -1,4 +1,7 @@ +#include "stage_p.h" #include +#include +#include namespace moveit { namespace task_constructor { @@ -60,4 +63,18 @@ void SolutionBase::setCost(double cost) { cost_ = cost; } + +void SubTrajectory::fillMessage(moveit_task_constructor::Solution &msg) const { + msg.sub_trajectory.emplace_back(); + moveit_task_constructor::SubTrajectory& t = msg.sub_trajectory.back(); + t.id = this->id(); + t.name = this->name(); + t.cost = this->cost(); + t.stage = this->creator()->name(); + if (trajectory()) + trajectory()->getRobotTrajectoryMsg(t.trajectory); + t.markers = this->markers(); +} + + } } diff --git a/src/task.cpp b/src/task.cpp index b236d239..83d7666e 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -177,11 +177,13 @@ void Task::processSolutions(const ContainerBase::SolutionProcessor &processor) c } void Task::processSolutions(const Task::SolutionProcessor& processor) const { - SolutionTrajectory solution; - processSolutions([&solution, &processor](const SolutionBase& s) { - solution.clear(); - s.flattenTo(solution); - return processor(solution, s.cost()); + ::moveit_task_constructor::Solution msg; + msg.task_id = id(); + processSolutions([&msg, &processor](const SolutionBase& s) { + msg.sub_solution.clear(); + msg.sub_trajectory.clear(); + s.fillMessage(msg); + return processor(msg, s.cost()); }); } @@ -201,6 +203,12 @@ inline const ContainerBase* Task::wrapped() const return const_cast(this)->wrapped(); } +std::string Task::id() const +{ + ros::NodeHandle n; + return std::to_string(id_) + '@' + n.getNamespace(); +} + void Task::printState(const Task &t){ ContainerBase::StageCallback processor = [](const Stage& stage, int depth) -> bool { std::cout << std::string(2*depth, ' ') << stage << std::endl; @@ -209,8 +217,8 @@ void Task::printState(const Task &t){ t.wrapped()->traverseRecursively(processor); } -template -void fillStateList(Container &c, const InterfaceConstPtr& interface) { +void fillStateList(moveit_task_constructor::Stage::_received_starts_type &c, + const InterfaceConstPtr& interface) { c.clear(); if (!interface) return; for (const InterfaceState& state : *interface) @@ -220,7 +228,7 @@ void fillStateList(Container &c, const InterfaceConstPtr& interface) { moveit_task_constructor::Task& Task::fillMessage(moveit_task_constructor::Task &msg) const { std::map stage_to_id_map; - ContainerBase::StageCallback processor = + 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 const StagePrivate *simpl = stage.pimpl(); @@ -239,15 +247,28 @@ moveit_task_constructor::Task& Task::fillMessage(moveit_task_constructor::Task & fillStateList(s.generated_starts, simpl->nextStarts()); fillStateList(s.generated_ends, simpl->prevEnds()); - // TODO: insert solutions via processSolutions() + // 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); + + // finally store in msg.stages msg.stages.push_back(std::move(s)); return true; }; - msg.id = id_; + msg.id = id(); + msg.stages.clear(); stage_to_id_map[this] = 0; // ID for root - wrapped()->traverseRecursively(processor); + wrapped()->traverseRecursively(stageProcessor); return msg; } diff --git a/srv/GetSolution.srv b/srv/GetSolution.srv new file mode 100644 index 00000000..9ff91660 --- /dev/null +++ b/srv/GetSolution.srv @@ -0,0 +1,6 @@ +# ID of solution (as published in Task msg) +uint32 solution_id + +--- + +Solution solution diff --git a/srv/GetSolutionTrajectory.srv b/srv/GetSolutionTrajectory.srv deleted file mode 100644 index a5099f3e..00000000 --- a/srv/GetSolutionTrajectory.srv +++ /dev/null @@ -1,7 +0,0 @@ -# ID of solution (as published in Task msg) -uint32 solution_id - ---- - -# solution trajectory composed from multiple sub trajectories -moveit_msgs/DisplayTrajectory trajectory