reworked introspection messages

This commit is contained in:
Robert Haschke 2017-10-25 22:39:30 +02:00
parent e686f49d3e
commit 7a1fc21f2e
17 changed files with 163 additions and 83 deletions

View File

@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 2.6.12) cmake_minimum_required(VERSION 2.6.12)
project(moveit_task_constructor) project(moveit_task_constructor)
set(MSG_DEPS trajectory_msgs moveit_msgs) set(MSG_DEPS moveit_msgs visualization_msgs)
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
roscpp roscpp
@ -18,12 +18,14 @@ add_message_files(DIRECTORY msg
FILES FILES
Stage.msg Stage.msg
Task.msg Task.msg
SubSolution.msg
SubTrajectory.msg
Solution.msg Solution.msg
) )
add_service_files(DIRECTORY srv add_service_files(DIRECTORY srv
FILES FILES
GetInterfaceState.srv GetInterfaceState.srv
GetSolutionTrajectory.srv GetSolution.srv
) )
generate_messages(DEPENDENCIES ${MSG_DEPS}) generate_messages(DEPENDENCIES ${MSG_DEPS})

View File

@ -28,11 +28,6 @@ public:
virtual bool canCompute() const = 0; virtual bool canCompute() const = 0;
virtual bool compute() = 0; virtual bool compute() = 0;
size_t numSolutions() const = 0;
typedef std::function<bool(const SolutionBase&)> 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 /// called by a (direct) child when a new solution becomes available
virtual void onNewSolution(SolutionBase& t) = 0; virtual void onNewSolution(SolutionBase& t) = 0;
@ -55,7 +50,7 @@ public:
bool compute() override; bool compute() override;
size_t numSolutions() const 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 /// container used to represent a serial solution
typedef std::vector<const SolutionBase*> solution_container; typedef std::vector<const SolutionBase*> solution_container;
@ -102,7 +97,7 @@ public:
void init(const planning_scene::PlanningSceneConstPtr &scene) override; void init(const planning_scene::PlanningSceneConstPtr &scene) override;
size_t numSolutions() const override; size_t numSolutions() const override;
void processSolutions(const SolutionProcessor &processor) const; void processSolutions(const SolutionProcessor &processor) const override;
protected: protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl); ParallelContainerBase(ParallelContainerBasePrivate* impl);

View File

@ -81,6 +81,11 @@ public:
const std::string& name() const; const std::string& name() const;
virtual size_t numSolutions() const = 0; virtual size_t numSolutions() const = 0;
typedef std::function<bool(const SolutionBase&)> 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: protected:
/// Stage can only be instantiated through derived classes /// Stage can only be instantiated through derived classes
Stage(StagePrivate *impl); Stage(StagePrivate *impl);
@ -97,13 +102,13 @@ public:
PRIVATE_CLASS(ComputeBase) PRIVATE_CLASS(ComputeBase)
void reset() override; void reset() override;
virtual size_t numSolutions() const override; virtual size_t numSolutions() const override;
void processSolutions(const SolutionProcessor &processor) const override;
void processFailures(const SolutionProcessor &processor) const override;
protected: protected:
/// ComputeBase can only be instantiated by derived classes in stage.cpp /// ComputeBase can only be instantiated by derived classes in stage.cpp
ComputeBase(ComputeBasePrivate* impl); ComputeBase(ComputeBasePrivate* impl);
// TODO: Do we really need/want to expose the trajectories?
const std::list<SubTrajectory>& trajectories() const;
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost); SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
}; };

View File

@ -3,6 +3,8 @@
#pragma once #pragma once
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <moveit_task_constructor/Solution.h>
#include <visualization_msgs/MarkerArray.h>
#include <list> #include <list>
#include <vector> #include <vector>
@ -181,8 +183,8 @@ public:
} }
void setCost(double cost); void setCost(double cost);
/// flatten this solution to full solution trajectory, appending to solution /// append this solution to Solution msg
virtual void flattenTo(SolutionTrajectory& solution) const = 0; virtual void fillMessage(::moveit_task_constructor::Solution &solution) const = 0;
protected: protected:
SolutionBase(StagePrivate* creator, double cost) SolutionBase(StagePrivate* creator, double cost)
@ -215,17 +217,20 @@ public:
explicit SubTrajectory(StagePrivate* creator, const robot_trajectory::RobotTrajectoryPtr& traj, double cost); explicit SubTrajectory(StagePrivate* creator, const robot_trajectory::RobotTrajectoryPtr& traj, double cost);
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; } robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
const visualization_msgs::MarkerArray& markers() const { return markers_; }
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 flattenTo(SolutionTrajectory& solution) const override { void fillMessage(::moveit_task_constructor::Solution &msg) const override;
solution.push_back(this);
}
private: private:
const robot_trajectory::RobotTrajectoryPtr trajectory_;
// 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
std::string name_; std::string name_;
// actual trajectory, might be empty
const robot_trajectory::RobotTrajectoryPtr trajectory_;
// additional markers
visualization_msgs::MarkerArray markers_;
}; };

View File

@ -7,6 +7,7 @@
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <moveit_task_constructor/Task.h> #include <moveit_task_constructor/Task.h>
#include <moveit_task_constructor/Solution.h>
namespace robot_model_loader { namespace robot_model_loader {
MOVEIT_CLASS_FORWARD(RobotModelLoader) MOVEIT_CLASS_FORWARD(RobotModelLoader)
@ -35,7 +36,7 @@ public:
const std::string &planning_plugin_param_name = "planning_plugin", const std::string &planning_plugin_param_name = "planning_plugin",
const std::string &adapter_plugins_param_name = "request_adapters"); 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 add(Stage::pointer &&stage);
void clear() override; void clear() override;
@ -65,7 +66,7 @@ public:
/// For each solution, composed from several SubTrajectories, /// For each solution, composed from several SubTrajectories,
/// the vector of SubTrajectories as well as the associated costs are provided. /// the vector of SubTrajectories as well as the associated costs are provided.
/// Return true, if traversal should continue, otherwise false. /// Return true, if traversal should continue, otherwise false.
typedef std::function<bool(const SolutionTrajectory& solution, typedef std::function<bool(const ::moveit_task_constructor::Solution& msg,
double accumulated_cost)> SolutionProcessor; double accumulated_cost)> SolutionProcessor;
/// process all solutions /// process all solutions
void processSolutions(const Task::SolutionProcessor &processor) const; void processSolutions(const Task::SolutionProcessor &processor) const;

View File

@ -1,8 +1,8 @@
# unique id within task # id of associated task
uint32 id string task_id
# IDs of subsolutions, empty if ID refers to an actual SubTrajectory # set of all sub solutions involved
uint32[] sub_solutions SubSolution[] sub_solution
# associated cost # (ordered) sequence of actual trajectories
float32 cost SubTrajectory[] sub_trajectory

View File

@ -10,13 +10,16 @@ uint32 flags
# parent id, parent_id == id means root # parent id, parent_id == id means root
uint32 parent_id 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_starts
uint32[] received_ends uint32[] received_ends
# ids of generated start / end states # IDs of generated start / end states
uint32[] generated_starts uint32[] generated_starts
uint32[] generated_ends uint32[] generated_ends
# successful and failed solutions of this stage # successful and failed solution IDs of this stage
Solution[] solutions uint32[] solved
Solution[] failures uint32[] failed

12
msg/SubSolution.msg Normal file
View File

@ -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

22
msg/SubTrajectory.msg Normal file
View File

@ -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?

View File

@ -414,11 +414,19 @@ bool SerialContainer::traverse(const SolutionBase &start, const SolutionProcesso
return result; return result;
} }
void SerialSolution::flattenTo(std::vector<const SubTrajectory *> &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_) 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);
} }

View File

@ -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 flattenTo(SolutionTrajectory& solution) const override; void fillMessage(::moveit_task_constructor::Solution &msg) const override;
private: private:
/// series of sub solutions /// series of sub solutions
@ -123,8 +123,8 @@ 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 flattenTo(SolutionTrajectory& solution) const override { void fillMessage(::moveit_task_constructor::Solution &solution) const override {
wrapped_->flattenTo(solution); wrapped_->fillMessage(solution);
} }
private: private:

View File

@ -3,32 +3,14 @@
#include <moveit_task_constructor/storage.h> #include <moveit_task_constructor/storage.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit_msgs/DisplayTrajectory.h>
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
bool publishSolution(ros::Publisher& pub, const SolutionTrajectory& solution, double cost, bool wait){ bool publishSolution(ros::Publisher& pub,
if (solution.empty()) const moveit_task_constructor::Solution &msg,
return true; double cost, bool wait){
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());
}
}
std::cout << "publishing solution with cost: " << cost << std::endl; std::cout << "publishing solution with cost: " << cost << std::endl;
pub.publish(dt); pub.publish(msg);
if (wait) { if (wait) {
std::cout << "Press <Enter> to continue ..." << std::endl; std::cout << "Press <Enter> to continue ..." << std::endl;
int ch = getchar(); 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) { void publishAllPlans(const Task &task, const std::string &topic, bool wait) {
ros::NodeHandle n; ros::NodeHandle n;
ros::Publisher pub = n.advertise<moveit_msgs::DisplayTrajectory>(topic, 1, true); ros::Publisher pub = n.advertise<::moveit_task_constructor::Solution>(topic, 1, true);
Task::SolutionProcessor processor = std::bind( Task::SolutionProcessor processor = std::bind(
&publishSolution, pub, std::placeholders::_1, std::placeholders::_2, wait); &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) NewSolutionPublisher::NewSolutionPublisher(const std::string &topic)
{ {
ros::NodeHandle n; ros::NodeHandle n;
pub_ = n.advertise<moveit_msgs::DisplayTrajectory>(topic, 1, true); pub_ = n.advertise<::moveit_task_constructor::Solution>(topic, 1, true);
} }
void NewSolutionPublisher::operator()(const SolutionBase &s) void NewSolutionPublisher::operator()(const SolutionBase &s)
{ {
// flatten s into vector of SubTrajectories // flatten s into vector of SubTrajectories
SolutionTrajectory solution; ::moveit_task_constructor::Solution msg;
s.flattenTo(solution); s.fillMessage(msg);
publishSolution(pub_, solution, s.cost(), false); publishSolution(pub_, msg, s.cost(), false);
} }
} } } }

View File

@ -150,8 +150,16 @@ size_t ComputeBase::numSolutions() const {
return pimpl()->trajectories_.size(); return pimpl()->trajectories_.size();
} }
const std::list<SubTrajectory> &ComputeBase::trajectories() const { void ComputeBase::processSolutions(const Stage::SolutionProcessor &processor) const
return pimpl()->trajectories_; {
for (const auto& s : pimpl()->trajectories_)
if (!processor(s))
return;
}
void ComputeBase::processFailures(const Stage::SolutionProcessor &processor) const
{
// TODO
} }
void ComputeBase::reset() { void ComputeBase::reset() {

View File

@ -1,4 +1,7 @@
#include "stage_p.h"
#include <moveit_task_constructor/storage.h> #include <moveit_task_constructor/storage.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/robot_state/conversions.h>
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
@ -60,4 +63,18 @@ void SolutionBase::setCost(double cost) {
cost_ = 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();
}
} } } }

View File

@ -177,11 +177,13 @@ void Task::processSolutions(const ContainerBase::SolutionProcessor &processor) c
} }
void Task::processSolutions(const Task::SolutionProcessor& processor) const { void Task::processSolutions(const Task::SolutionProcessor& processor) const {
SolutionTrajectory solution; ::moveit_task_constructor::Solution msg;
processSolutions([&solution, &processor](const SolutionBase& s) { msg.task_id = id();
solution.clear(); processSolutions([&msg, &processor](const SolutionBase& s) {
s.flattenTo(solution); msg.sub_solution.clear();
return processor(solution, s.cost()); 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<Task*>(this)->wrapped(); return const_cast<Task*>(this)->wrapped();
} }
std::string Task::id() const
{
ros::NodeHandle n;
return std::to_string(id_) + '@' + n.getNamespace();
}
void Task::printState(const Task &t){ void Task::printState(const Task &t){
ContainerBase::StageCallback processor = [](const Stage& stage, int depth) -> bool { ContainerBase::StageCallback processor = [](const Stage& stage, int depth) -> bool {
std::cout << std::string(2*depth, ' ') << stage << std::endl; std::cout << std::string(2*depth, ' ') << stage << std::endl;
@ -209,8 +217,8 @@ void Task::printState(const Task &t){
t.wrapped()->traverseRecursively(processor); t.wrapped()->traverseRecursively(processor);
} }
template <typename Container> void fillStateList(moveit_task_constructor::Stage::_received_starts_type &c,
void fillStateList(Container &c, const InterfaceConstPtr& interface) { const InterfaceConstPtr& interface) {
c.clear(); c.clear();
if (!interface) return; if (!interface) return;
for (const InterfaceState& state : *interface) 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 moveit_task_constructor::Task& Task::fillMessage(moveit_task_constructor::Task &msg) const
{ {
std::map<const Stage*, moveit_task_constructor::Stage::_id_type> stage_to_id_map; std::map<const Stage*, moveit_task_constructor::Stage::_id_type> stage_to_id_map;
ContainerBase::StageCallback processor = ContainerBase::StageCallback stageProcessor =
[&stage_to_id_map, &msg](const Stage& stage, int) -> bool { [&stage_to_id_map, &msg](const Stage& stage, int) -> bool {
// this method is called for each child stage of a given parent // this method is called for each child stage of a given parent
const StagePrivate *simpl = stage.pimpl(); 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_starts, simpl->nextStarts());
fillStateList(s.generated_ends, simpl->prevEnds()); 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)); msg.stages.push_back(std::move(s));
return true; return true;
}; };
msg.id = id_; msg.id = id();
msg.stages.clear();
stage_to_id_map[this] = 0; // ID for root stage_to_id_map[this] = 0; // ID for root
wrapped()->traverseRecursively(processor); wrapped()->traverseRecursively(stageProcessor);
return msg; return msg;
} }

6
srv/GetSolution.srv Normal file
View File

@ -0,0 +1,6 @@
# ID of solution (as published in Task msg)
uint32 solution_id
---
Solution solution

View File

@ -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