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)
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})

View File

@ -28,11 +28,6 @@ public:
virtual bool canCompute() const = 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
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<const SolutionBase*> 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);

View File

@ -81,6 +81,11 @@ public:
const std::string& name() const;
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:
/// 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<SubTrajectory>& trajectories() const;
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
};

View File

@ -3,6 +3,8 @@
#pragma once
#include <moveit/macros/class_forward.h>
#include <moveit_task_constructor/Solution.h>
#include <visualization_msgs/MarkerArray.h>
#include <list>
#include <vector>
@ -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_;
};

View File

@ -7,6 +7,7 @@
#include <moveit/macros/class_forward.h>
#include <moveit_task_constructor/Task.h>
#include <moveit_task_constructor/Solution.h>
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<bool(const SolutionTrajectory& solution,
typedef std::function<bool(const ::moveit_task_constructor::Solution& msg,
double accumulated_cost)> SolutionProcessor;
/// process all solutions
void processSolutions(const Task::SolutionProcessor &processor) const;

View File

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

View File

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

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;
}
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_)
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)
{}
/// 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:

View File

@ -3,32 +3,14 @@
#include <moveit_task_constructor/storage.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 {
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 <Enter> 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<moveit_msgs::DisplayTrajectory>(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<moveit_msgs::DisplayTrajectory>(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);
}
} }

View File

@ -150,8 +150,16 @@ size_t ComputeBase::numSolutions() const {
return pimpl()->trajectories_.size();
}
const std::list<SubTrajectory> &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() {

View File

@ -1,4 +1,7 @@
#include "stage_p.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 {
@ -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();
}
} }

View File

@ -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<Task*>(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 <typename Container>
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<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 {
// 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;
}

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