mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
reworked introspection messages
This commit is contained in:
parent
e686f49d3e
commit
7a1fc21f2e
@ -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})
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
|
||||
@ -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_;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
12
msg/SubSolution.msg
Normal 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
22
msg/SubTrajectory.msg
Normal 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?
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
@ -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() {
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
|
||||
} }
|
||||
|
||||
43
src/task.cpp
43
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<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
6
srv/GetSolution.srv
Normal file
@ -0,0 +1,6 @@
|
||||
# ID of solution (as published in Task msg)
|
||||
uint32 solution_id
|
||||
|
||||
---
|
||||
|
||||
Solution solution
|
||||
@ -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
|
||||
Loading…
Reference in New Issue
Block a user