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