specialised SubTask classes

SubTask cannot be instantiated directly, but only its derived classes
that specify the type of operation:

PropagatingAnyWay
PropagatingForward
PropagatingBackward
Connecting
Generator
This commit is contained in:
Robert Haschke 2017-09-29 11:31:40 -07:00
parent f7fe7fc896
commit fcccbae408
18 changed files with 418 additions and 226 deletions

View File

@ -12,7 +12,7 @@ MOVEIT_CLASS_FORWARD(Task)
MOVEIT_CLASS_FORWARD(SubTrajectory) MOVEIT_CLASS_FORWARD(SubTrajectory)
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
std::vector<SubTrajectory*>& solution, bool wait); const std::vector<SubTrajectory*>& solution, bool wait);
void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true); void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true);

View File

@ -8,56 +8,69 @@
#include <cassert> #include <cassert>
namespace planning_scene { namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene); MOVEIT_CLASS_FORWARD(PlanningScene)
} }
namespace robot_trajectory { namespace robot_trajectory {
MOVEIT_CLASS_FORWARD(RobotTrajectory); MOVEIT_CLASS_FORWARD(RobotTrajectory)
} }
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
MOVEIT_CLASS_FORWARD(SubTrajectory); MOVEIT_CLASS_FORWARD(SubTrajectory)
MOVEIT_CLASS_FORWARD(InterfaceState); MOVEIT_CLASS_FORWARD(InterfaceState)
class InterfaceState {
friend class SubTrajectory;
public:
typedef std::vector<SubTrajectory*> SubTrajectoryList;
struct InterfaceState {
InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next) InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next)
: state(ps) : state(ps)
{ {
if( previous ) if( previous )
previous_trajectory.push_back(previous); previous_trajectories_.push_back(previous);
if( next ) if( next )
next_trajectory.push_back(next); next_trajectories_.push_back(next);
} }
std::vector<SubTrajectory*> previous_trajectory; inline const SubTrajectoryList& previousTrajectories() const { return previous_trajectories_; }
std::vector<SubTrajectory*> next_trajectory; inline const SubTrajectoryList& nextTrajectories() const { return next_trajectories_; }
public:
planning_scene::PlanningSceneConstPtr state; planning_scene::PlanningSceneConstPtr state;
double cost; // minimal costs
private:
mutable SubTrajectoryList previous_trajectories_;
mutable SubTrajectoryList next_trajectories_;
}; };
struct SubTrajectory { struct SubTrajectory {
SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj) SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj)
: trajectory(traj), : trajectory(traj),
begin(NULL), begin(NULL),
end(NULL) end(NULL),
cost(0)
{} {}
void hasBeginning(InterfaceState& state){ void setBeginning(const InterfaceState& state){
assert(begin == NULL); assert(begin == NULL);
begin= &state; begin= &state;
state.next_trajectory.push_back(this); state.next_trajectories_.push_back(this);
} }
void hasEnding(InterfaceState& state){ void setEnding(const InterfaceState& state){
assert(end == NULL); assert(end == NULL);
end= &state; end= &state;
state.previous_trajectory.push_back(this); state.previous_trajectories_.push_back(this);
} }
robot_trajectory::RobotTrajectoryPtr trajectory; robot_trajectory::RobotTrajectoryPtr trajectory;
InterfaceState* begin; const InterfaceState* begin;
InterfaceState* end; const InterfaceState* end;
double cost;
}; };
} } } }

View File

@ -12,72 +12,137 @@
#include <tuple> #include <tuple>
namespace planning_scene { namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene); MOVEIT_CLASS_FORWARD(PlanningScene)
} }
namespace planning_pipeline { namespace planning_pipeline {
MOVEIT_CLASS_FORWARD(PlanningPipeline); MOVEIT_CLASS_FORWARD(PlanningPipeline)
} }
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
MOVEIT_CLASS_FORWARD(SubTask); typedef std::list<InterfaceState> InterfaceStateList;
typedef std::pair<InterfaceState&, InterfaceState&> InterfaceStatePair;
MOVEIT_CLASS_FORWARD(SubTask)
typedef std::weak_ptr<SubTask> SubTaskWeakPtr; typedef std::weak_ptr<SubTask> SubTaskWeakPtr;
class Task;
class SubTaskPrivate;
class SubTask { class SubTask {
friend class SubTaskPrivate; // allow access to impl_
friend class Task; // TODO: remove when we have SubTaskContainers
protected:
SubTaskPrivate* const impl_;
public: public:
SubTask(std::string name); enum PropagationType { FORWARD, BACKWARD, ANYWAY, GENERATOR, CONNECTING };
~SubTask();
const std::string& getName() const;
virtual bool canCompute() = 0; virtual bool canCompute() = 0;
virtual bool compute() = 0; virtual bool compute() = 0;
const std::string& getName();
const std::list<InterfaceState>& getBeginning();
const std::list<InterfaceState>& getEnd();
std::list<SubTrajectory>& getTrajectories();
void setPlanningScene(planning_scene::PlanningSceneConstPtr); void setPlanningScene(planning_scene::PlanningSceneConstPtr);
void setPlanningPipeline(planning_pipeline::PlanningPipelinePtr); void setPlanningPipeline(planning_pipeline::PlanningPipelinePtr);
void addPredecessor(SubTaskPtr);
void addSuccessor(SubTaskPtr);
protected: protected:
InterfaceState& fetchStateBeginning(); /// can only instantiated by derived classes
InterfaceState& fetchStateEnding(); SubTask(SubTaskPrivate *impl);
std::pair<InterfaceState&, InterfaceState&> fetchStatePair();
bool hasBeginning(); /// methods called when a new InterfaceState was spawned
bool hasEnding(); virtual inline void newBeginning(const InterfaceStateList::iterator& it) {}
bool hasStatePair(); virtual inline void newEnd(const InterfaceStateList::iterator& it) {}
SubTrajectory& addTrajectory(robot_trajectory::RobotTrajectoryPtr);
void sendForward(SubTrajectory&, planning_scene::PlanningSceneConstPtr);
void sendBackward(SubTrajectory&, planning_scene::PlanningSceneConstPtr);
void sendBothWays(SubTrajectory&, planning_scene::PlanningSceneConstPtr);
InterfaceState* newBeginning(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
InterfaceState* newEnd(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
const std::string name_;
// avoid shared pointers back
SubTaskWeakPtr predecessor_;
SubTaskPtr successor_;
std::list<SubTrajectory> trajectories_;
std::list<InterfaceState> beginnings_;
std::list<InterfaceState> endings_;
planning_scene::PlanningSceneConstPtr scene_; planning_scene::PlanningSceneConstPtr scene_;
planning_pipeline::PlanningPipelinePtr planner_; planning_pipeline::PlanningPipelinePtr planner_;
std::list<InterfaceState>::iterator it_beginnings_;
std::list<InterfaceState>::iterator it_endings_;
std::pair< std::list<InterfaceState>::iterator, std::list<InterfaceState>::iterator > it_pairs_;
}; };
class PropagatingAnyWayPrivate;
class PropagatingAnyWay : public SubTask {
public:
PropagatingAnyWay(const std::string& name);
static inline constexpr PropagationType type() { return ANYWAY; }
bool hasBeginning() const;
InterfaceState& fetchStateBeginning();
void sendForward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
const InterfaceState& from,
const planning_scene::PlanningSceneConstPtr& to,
double cost = 0);
bool hasEnding() const;
InterfaceState& fetchStateEnding();
void sendBackward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
const planning_scene::PlanningSceneConstPtr& from,
const InterfaceState& to,
double cost = 0);
protected:
// constructor for use in derived classes
inline PropagatingAnyWay(PropagatingAnyWayPrivate* impl);
// get informed when new beginnings and endings become available
void newBeginning(const InterfaceStateList::iterator& it);
void newEnd(const InterfaceStateList::iterator& it);
};
class PropagatingForward : public PropagatingAnyWay {
public:
PropagatingForward(const std::string& name);
static inline constexpr PropagationType type() { return FORWARD; }
private:
// restrict access to backward methods
using PropagatingAnyWay::hasEnding;
using PropagatingAnyWay::fetchStateEnding;
using PropagatingAnyWay::sendBackward;
// don't care about new endings
inline void newEnd(const InterfaceStateList::iterator& it) {}
};
class PropagatingBackward : public PropagatingAnyWay {
public:
PropagatingBackward(const std::string& name);
static inline constexpr PropagationType type() { return BACKWARD; }
private:
// restrict access to forward methods
using PropagatingAnyWay::hasBeginning;
using PropagatingAnyWay::fetchStateBeginning;
using PropagatingAnyWay::sendForward;
// don't care about new beginnings
inline void newBeginning(const InterfaceStateList::iterator& it) {}
};
class Generator : public SubTask {
public:
Generator(const std::string& name);
static inline constexpr PropagationType type() { return GENERATOR; }
void spawn(const planning_scene::PlanningSceneConstPtr &ps, double cost = 0);
};
class Connecting : public SubTask {
public:
Connecting(const std::string& name);
static inline constexpr PropagationType type() { return CONNECTING; }
bool hasStatePair() const;
InterfaceStatePair fetchStatePair();
void connect(const robot_trajectory::RobotTrajectoryPtr&, const InterfaceStatePair&, double cost = 0);
protected:
virtual void newBeginning(const InterfaceStateList::iterator& it);
virtual void newEnd(const InterfaceStateList::iterator& it);
};
} } } }

View File

@ -15,7 +15,7 @@ MOVEIT_CLASS_FORWARD(MoveGroupInterface);
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {
class CartesianPositionMotion : public SubTask { class CartesianPositionMotion : public PropagatingAnyWay {
public: public:
CartesianPositionMotion(std::string name); CartesianPositionMotion(std::string name);

View File

@ -6,7 +6,7 @@
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {
class CurrentState : public SubTask { class CurrentState : public Generator {
public: public:
CurrentState(std::string name); CurrentState(std::string name);

View File

@ -14,7 +14,7 @@ MOVEIT_CLASS_FORWARD(MoveGroupInterface);
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {
class GenerateGraspPose : public SubTask { class GenerateGraspPose : public Generator {
public: public:
GenerateGraspPose(std::string name); GenerateGraspPose(std::string name);

View File

@ -5,12 +5,12 @@
#include <moveit_task_constructor/subtask.h> #include <moveit_task_constructor/subtask.h>
namespace moveit { namespace planning_interface { namespace moveit { namespace planning_interface {
MOVEIT_CLASS_FORWARD(MoveGroupInterface); MOVEIT_CLASS_FORWARD(MoveGroupInterface)
} } } }
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {
class Gripper : public SubTask { class Gripper : public PropagatingForward {
public: public:
Gripper(std::string name); Gripper(std::string name);

View File

@ -10,7 +10,7 @@ MOVEIT_CLASS_FORWARD(MoveGroupInterface)
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {
class Move : public SubTask { class Move : public Connecting {
public: public:
Move(std::string name); Move(std::string name);

View File

@ -33,7 +33,7 @@ MOVEIT_CLASS_FORWARD(Task)
class Task { class Task {
public: public:
typedef std::function<bool(std::vector<SubTrajectory*>&)> SolutionCallback; typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback;
Task(); Task();
~Task(); ~Task();
@ -42,6 +42,7 @@ public:
bool plan(); bool plan();
bool processSolutions(const SolutionCallback &processor);
bool processSolutions(const SolutionCallback &processor) const; bool processSolutions(const SolutionCallback &processor) const;
const moveit::core::RobotState &getCurrentRobotState() const; const moveit::core::RobotState &getCurrentRobotState() const;
@ -50,7 +51,7 @@ public:
void clear(); void clear();
protected: protected:
std::vector<SubTaskPtr> subtasks_; std::vector<SubTaskPtr> subtasks_;
planning_scene::PlanningScenePtr scene_; planning_scene::PlanningScenePtr scene_;

View File

@ -8,7 +8,7 @@
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
std::vector<SubTrajectory*>& solution, bool wait){ const std::vector<SubTrajectory*>& solution, bool wait){
dt.trajectory.clear(); dt.trajectory.clear();
for(const SubTrajectory* t : solution){ for(const SubTrajectory* t : solution){
if(t->trajectory){ if(t->trajectory){
@ -26,14 +26,14 @@ bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
return true; return true;
} }
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_msgs::DisplayTrajectory>(topic, 1, true);
moveit_msgs::DisplayTrajectory dt; moveit_msgs::DisplayTrajectory dt;
robot_state::robotStateToRobotStateMsg(task.getCurrentRobotState(), dt.trajectory_start); robot_state::robotStateToRobotStateMsg(task.getCurrentRobotState(), dt.trajectory_start);
dt.model_id= task.getCurrentRobotState().getRobotModel()->getName(); dt.model_id= task.getCurrentRobotState().getRobotModel()->getName();
Task::SolutionCallback processor= std::bind( Task::SolutionCallback processor = std::bind(
&publishSolution, pub, dt, std::placeholders::_1, wait); &publishSolution, pub, dt, std::placeholders::_1, wait);
task.processSolutions(processor); task.processSolutions(processor);
@ -53,7 +53,7 @@ void NewSolutionPublisher::publish()
robot_state::robotStateToRobotStateMsg(task_.getCurrentRobotState(), dt.trajectory_start); robot_state::robotStateToRobotStateMsg(task_.getCurrentRobotState(), dt.trajectory_start);
dt.model_id = task_.getCurrentRobotState().getRobotModel()->getName(); dt.model_id = task_.getCurrentRobotState().getRobotModel()->getName();
Task::SolutionCallback processor = [this,&dt](std::vector<SubTrajectory*>& solution) { Task::SolutionCallback processor = [this,&dt](const std::vector<SubTrajectory*>& solution) {
bool all_published = true; bool all_published = true;
for(const SubTrajectory* t : solution){ for(const SubTrajectory* t : solution){
auto result = published_.insert(t); auto result = published_.insert(t);

View File

@ -1,162 +1,219 @@
#include <moveit_task_constructor/subtask.h> #include "subtask_p.h"
// get correctly casted private impl pointer
#define I(Class) Class##Private * const impl = reinterpret_cast<Class##Private*>(impl_)
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
SubTask::SubTask(std::string name) SubTask::SubTask(SubTaskPrivate *impl)
: name_(name), : impl_(impl)
it_beginnings_(beginnings_.begin()), {
it_endings_(endings_.begin()),
it_pairs_(beginnings_.begin(), endings_.begin())
{}
void SubTask::addPredecessor(SubTaskPtr prev_task){
predecessor_= SubTaskWeakPtr(prev_task);
} }
void SubTask::addSuccessor(SubTaskPtr next_task){ SubTask::~SubTask()
successor_= next_task; {
delete impl_;
} }
const std::string& const std::string& SubTask::getName() const {
SubTask::getName(){ return impl_->name_;
return name_;
} }
const std::list<InterfaceState>& void SubTask::setPlanningScene(planning_scene::PlanningSceneConstPtr scene){
SubTask::getBeginning(){
return beginnings_;
}
const std::list<InterfaceState>&
SubTask::getEnd(){
return endings_;
}
std::list<SubTrajectory>&
SubTask::getTrajectories(){
return trajectories_;
}
void
SubTask::setPlanningScene(planning_scene::PlanningSceneConstPtr scene){
scene_= scene; scene_= scene;
} }
void void SubTask::setPlanningPipeline(planning_pipeline::PlanningPipelinePtr planner){
SubTask::setPlanningPipeline(planning_pipeline::PlanningPipelinePtr planner){
planner_= planner; planner_= planner;
} }
InterfaceState&
SubTask::fetchStateBeginning(){
if(it_beginnings_ == beginnings_.end())
throw std::runtime_error("no new state for beginning available");
InterfaceState& state= *it_beginnings_; void SubTaskPrivate::addPredecessor(SubTaskPtr prev_task){
++it_beginnings_; predecessor_= SubTaskWeakPtr(prev_task);
return state;
} }
InterfaceState& void SubTaskPrivate::addSuccessor(SubTaskPtr next_task){
SubTask::fetchStateEnding(){ successor_= next_task;
if(it_endings_ == endings_.end())
throw std::runtime_error("no new state for ending available");
InterfaceState& state= *it_endings_;
++it_endings_;
return state;
} }
std::pair<InterfaceState&, InterfaceState&> SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
SubTask::fetchStatePair(){
// TODO: implement this properly
return std::pair<InterfaceState&, InterfaceState&>(
*it_pairs_.first,
*(it_pairs_.second++));
}
SubTrajectory&
SubTask::addTrajectory(robot_trajectory::RobotTrajectoryPtr trajectory){
trajectories_.emplace_back(trajectory); trajectories_.emplace_back(trajectory);
return trajectories_.back(); return trajectories_.back();
} }
void void SubTaskPrivate::sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps){
SubTask::sendForward(SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){
if( successor_ ){ if( successor_ ){
std::cout << "sending state forward to successor" << std::endl; std::cout << "sending state forward to successor" << std::endl;
traj.end= successor_->newBeginning(ps, &traj); traj.end= successor_->impl_->newBeginning(ps, &traj);
} }
} }
void void SubTaskPrivate::sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps){
SubTask::sendBackward(SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){
if( !predecessor_.expired() ){ if( !predecessor_.expired() ){
std::cout << "sending state backward to predecessor" << std::endl; std::cout << "sending state backward to predecessor" << std::endl;
traj.begin= predecessor_.lock()->newEnd(ps, &traj); traj.begin= predecessor_.lock()->impl_->newEnd(ps, &traj);
} }
} }
void InterfaceState* SubTaskPrivate::newBeginning(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_end){
SubTask::sendBothWays(SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){
std::cout << "sending state both ways" << std::endl;
if( !predecessor_.expired() )
sendBackward(traj, ps);
if( successor_ )
sendForward(traj, ps);
}
InterfaceState*
SubTask::newBeginning(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_end){
assert( bool(ps) ); assert( bool(ps) );
beginnings_.push_back( InterfaceState(ps, old_end, NULL) ); beginnings_.push_back( InterfaceState(ps, old_end, NULL) );
// we just appended a state to the list, but the iterator doesn't see it anymore me_->newBeginning(--beginnings_.end());
// so let's point it at the new one
if( it_beginnings_ == beginnings_.end() )
--it_beginnings_;
// TODO: need to handle the pairs iterator
if( it_pairs_.first == beginnings_.end() )
--it_pairs_.first;
return &beginnings_.back(); return &beginnings_.back();
} }
InterfaceState* InterfaceState* SubTaskPrivate::newEnd(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_beginning){
SubTask::newEnd(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_beginning){
assert( bool(ps) ); assert( bool(ps) );
endings_.push_back( InterfaceState(ps, NULL, old_beginning) ); endings_.push_back( InterfaceState(ps, NULL, old_beginning) );
// we just appended a state to the list, but the iterator doesn't see it anymore me_->newEnd(--endings_.end());
// so let's point it at the new one
if( it_endings_ == endings_.end() )
--it_endings_;
//TODO: need to handle the pairs iterator properly
if( it_pairs_.second == endings_.end() )
--it_pairs_.second;
return &endings_.back(); return &endings_.back();
} }
bool
SubTask::hasBeginning(){ PropagatingAnyWay::PropagatingAnyWay(const std::string &name)
return it_beginnings_ != beginnings_.end(); : SubTask(new PropagatingAnyWayPrivate(this, name))
{}
PropagatingAnyWay::PropagatingAnyWay(PropagatingAnyWayPrivate *impl) : SubTask(impl) {}
bool PropagatingAnyWay::hasBeginning() const{
I(PropagatingAnyWay);
return impl->it_beginnings_ != impl->beginnings_.end();
} }
bool InterfaceState& PropagatingAnyWay::fetchStateBeginning(){
SubTask::hasEnding(){ I(PropagatingAnyWay);
return it_endings_ != endings_.end(); if(impl->it_beginnings_ == impl->beginnings_.end())
throw std::runtime_error("no new state for beginning available");
InterfaceState& state= *impl->it_beginnings_;
++impl->it_beginnings_;
return state;
} }
bool void PropagatingAnyWay::sendForward(const robot_trajectory::RobotTrajectoryPtr& t,
SubTask::hasStatePair(){ const InterfaceState& from,
const planning_scene::PlanningSceneConstPtr& to,
double cost){
SubTrajectory &trajectory = impl_->addTrajectory(t, cost);
trajectory.setBeginning(from);
impl_->sendForward(trajectory, to);
}
void PropagatingAnyWay::newBeginning(const InterfaceStateList::iterator &it)
{
I(PropagatingAnyWay);
// we just appended a state to the list, but the iterator doesn't see it anymore
// so let's point it at the new one
if( impl->it_beginnings_ == impl->beginnings_.end() )
--impl->it_beginnings_;
}
bool PropagatingAnyWay::hasEnding() const{
I(PropagatingAnyWay);
return impl->it_endings_ != impl->endings_.end();
}
InterfaceState& PropagatingAnyWay::fetchStateEnding(){
I(PropagatingAnyWay);
if(impl->it_endings_ == impl->endings_.end())
throw std::runtime_error("no new state for ending available");
InterfaceState& state= *impl->it_endings_;
++impl->it_endings_;
return state;
}
void PropagatingAnyWay::sendBackward(const robot_trajectory::RobotTrajectoryPtr& t,
const planning_scene::PlanningSceneConstPtr& from,
const InterfaceState& to,
double cost){
SubTrajectory& trajectory = impl_->addTrajectory(t, cost);
trajectory.setEnding(to);
impl_->sendBackward(trajectory, from);
}
void PropagatingAnyWay::newEnd(const InterfaceStateList::iterator &it)
{
I(PropagatingAnyWay);
// we just appended a state to the list, but the iterator doesn't see it anymore
// so let's point it at the new one
if( impl->it_endings_ == impl->endings_.end() )
--impl->it_endings_;
}
PropagatingForward::PropagatingForward(const std::string& name)
: PropagatingAnyWay(new PropagatingForwardPrivate(this, name))
{}
PropagatingBackward::PropagatingBackward(const std::string &name)
: PropagatingAnyWay(new PropagatingBackwardPrivate(this, name))
{}
Generator::Generator(const std::string &name)
: SubTask(new SubTaskPrivate(this, name))
{}
void Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost)
{
// empty trajectory ref -> this node only produces states
robot_trajectory::RobotTrajectoryPtr dummy;
moveit::task_constructor::SubTrajectory& trajectory = impl_->addTrajectory(dummy, cost);
std::cout << "spawning state" << std::endl;
impl_->sendBackward(trajectory, ps);
impl_->sendForward(trajectory, ps);
}
Connecting::Connecting(const std::string &name)
: SubTask(new ConnectingPrivate(this, name))
{
}
void Connecting::newBeginning(const InterfaceStateList::iterator& it)
{
I(Connecting);
// TODO: need to handle the pairs iterator
if( impl->it_pairs_.first == impl->beginnings_.end() )
--impl->it_pairs_.first;
}
void Connecting::newEnd(const InterfaceStateList::iterator& it)
{
I(Connecting);
// TODO: need to handle the pairs iterator properly
if( impl->it_pairs_.second == impl->endings_.end() )
--impl->it_pairs_.second;
}
bool Connecting::hasStatePair() const{
I(Connecting);
// TODO: implement this properly // TODO: implement this properly
return it_pairs_.first != beginnings_.end() && it_pairs_.second != endings_.end(); return impl->it_pairs_.first != impl->beginnings_.end() &&
impl->it_pairs_.second != impl->endings_.end();
} }
std::pair<InterfaceState&, InterfaceState&> Connecting::fetchStatePair(){
I(Connecting);
// TODO: implement this properly
return std::pair<InterfaceState&, InterfaceState&>
(*impl->it_pairs_.first, *(impl->it_pairs_.second++));
}
void Connecting::connect(const robot_trajectory::RobotTrajectoryPtr& t, const InterfaceStatePair& state_pair, double cost)
{
moveit::task_constructor::SubTrajectory& trajectory = impl_->addTrajectory(t, cost);
trajectory.setBeginning(state_pair.first);
trajectory.setEnding(state_pair.second);
}
} } } }

75
src/subtask_p.h Normal file
View File

@ -0,0 +1,75 @@
#pragma once
#include <moveit_task_constructor/subtask.h>
namespace moveit { namespace task_constructor {
class SubTaskPrivate {
public:
inline SubTaskPrivate(SubTask* me, const std::string& name)
: me_(me), name_(name)
{}
inline const InterfaceStateList& getBeginning() const { return beginnings_; }
inline const InterfaceStateList& getEnd() const { return endings_; }
inline const std::list<SubTrajectory>& getTrajectories() const { return trajectories_; }
void addPredecessor(SubTaskPtr);
void addSuccessor(SubTaskPtr);
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
void sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
void sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
InterfaceState* newBeginning(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
InterfaceState* newEnd(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
public:
SubTask* const me_; // pointer to owning instance
const std::string name_;
// avoid shared pointers back: why? to allow proper deallocation?
SubTaskWeakPtr predecessor_;
SubTaskPtr successor_;
InterfaceStateList beginnings_;
InterfaceStateList endings_;
std::list<SubTrajectory> trajectories_;
};
class PropagatingAnyWayPrivate : public SubTaskPrivate {
friend class PropagatingAnyWay;
public:
inline PropagatingAnyWayPrivate(PropagatingAnyWay *me, const std::string &name)
: SubTaskPrivate(me, name)
, it_beginnings_ (beginnings_.begin())
, it_endings_ (endings_.begin())
{}
protected:
InterfaceStateList::iterator it_beginnings_;
InterfaceStateList::iterator it_endings_;
};
// for now, we use the same implementation for the reduced versions too
typedef PropagatingAnyWayPrivate PropagatingForwardPrivate;
typedef PropagatingAnyWayPrivate PropagatingBackwardPrivate;
class ConnectingPrivate : public SubTaskPrivate {
friend class Connecting;
public:
inline ConnectingPrivate(Connecting *me, const std::string &name)
: SubTaskPrivate(me, name)
, it_pairs_(beginnings_.begin(), endings_.begin())
{}
private:
std::pair<InterfaceStateList::iterator, InterfaceStateList::iterator> it_pairs_;
};
} }

View File

@ -15,7 +15,7 @@
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {
CartesianPositionMotion::CartesianPositionMotion(std::string name) CartesianPositionMotion::CartesianPositionMotion(std::string name)
: SubTask(name), : PropagatingAnyWay(name),
step_size_(0.005) step_size_(0.005)
{ {
ros::NodeHandle nh; ros::NodeHandle nh;
@ -163,12 +163,7 @@ bool CartesianPositionMotion::_computeFromBeginning(){
moveit::core::RobotStatePtr result_state= trajectory_steps.back(); moveit::core::RobotStatePtr result_state= trajectory_steps.back();
robot_state= *result_state; robot_state= *result_state;
moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj); sendForward(traj, beginning, result_scene);
trajectory.hasBeginning(beginning);
sendForward(trajectory, result_scene);
_publishTrajectory(*traj, *result_state); _publishTrajectory(*traj, *result_state);
} }
@ -238,12 +233,7 @@ bool CartesianPositionMotion::_computeFromEnding(){
moveit::core::RobotStatePtr result_state= trajectory_steps.back(); moveit::core::RobotStatePtr result_state= trajectory_steps.back();
robot_state= *result_state; robot_state= *result_state;
moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj); sendBackward(traj, result_scene, ending);
trajectory.hasEnding(ending);
sendBackward(trajectory, result_scene);
_publishTrajectory(*traj, *result_state); _publishTrajectory(*traj, *result_state);
return true; return true;

View File

@ -3,7 +3,7 @@
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {
CurrentState::CurrentState(std::string name) CurrentState::CurrentState(std::string name)
: SubTask(name) : Generator(name)
{ {
ran_= false; ran_= false;
} }
@ -14,14 +14,8 @@ bool CurrentState::canCompute(){
bool CurrentState::compute(){ bool CurrentState::compute(){
ran_= true; ran_= true;
assert( scene_ ); assert( scene_ );
spawn(scene_);
// empty trajectory ref -> this node only produces states
robot_trajectory::RobotTrajectoryPtr traj;
moveit::task_constructor::SubTrajectory& trajectory= addTrajectory(traj);
sendBothWays(trajectory, scene_);
return true; return true;
} }

View File

@ -18,7 +18,7 @@
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {
GenerateGraspPose::GenerateGraspPose(std::string name) GenerateGraspPose::GenerateGraspPose(std::string name)
: SubTask(name), : Generator(name),
timeout_(0.1), timeout_(0.1),
angle_delta_(0.1), angle_delta_(0.1),
max_ik_solutions_(0), max_ik_solutions_(0),
@ -115,9 +115,6 @@ bool GenerateGraspPose::compute(){
const std::string ik_link= ik_link_.empty() ? jmg_eef->getEndEffectorParentGroup().second : ik_link_; const std::string ik_link= ik_link_.empty() ? jmg_eef->getEndEffectorParentGroup().second : ik_link_;
// empty trajectory -> this subtask only produces states
const robot_trajectory::RobotTrajectoryPtr traj;
if(!gripper_grasp_pose_.empty()){ if(!gripper_grasp_pose_.empty()){
grasp_state.setToDefaultValues(jmg_eef, gripper_grasp_pose_); grasp_state.setToDefaultValues(jmg_eef, gripper_grasp_pose_);
} }
@ -173,8 +170,7 @@ bool GenerateGraspPose::compute(){
previous_solutions_.emplace_back(); previous_solutions_.emplace_back();
grasp_state.copyJointGroupPositions(jmg_active, previous_solutions_.back()); grasp_state.copyJointGroupPositions(jmg_active, previous_solutions_.back());
moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj); spawn(grasp_scene);
sendBothWays(trajectory, grasp_scene);
return true; return true;
} }
} }

View File

@ -12,7 +12,7 @@
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {
Gripper::Gripper(std::string name) Gripper::Gripper(std::string name)
: SubTask(name) : PropagatingForward(name)
{} {}
void Gripper::setEndEffector(std::string eef){ void Gripper::setEndEffector(std::string eef){
@ -24,6 +24,7 @@ void Gripper::setAttachLink(std::string link){
} }
void Gripper::setFrom(std::string named_target){ void Gripper::setFrom(std::string named_target){
// TODO: this direction isn't handled yet
from_named_target_= named_target; from_named_target_= named_target;
} }
@ -53,7 +54,7 @@ bool Gripper::compute(){
} }
} }
InterfaceState& start= fetchStateBeginning(); const InterfaceState& start= fetchStateBeginning();
planning_scene::PlanningScenePtr scene(start.state->diff()); planning_scene::PlanningScenePtr scene(start.state->diff());
@ -83,9 +84,7 @@ bool Gripper::compute(){
} }
// finish subtask // finish subtask
SubTrajectory& trajectory= addTrajectory(res.trajectory_); sendForward(res.trajectory_, start, scene);
trajectory.hasBeginning(start);
sendForward(trajectory, scene);
return true; return true;
} }

View File

@ -12,7 +12,7 @@
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {
Move::Move(std::string name) Move::Move(std::string name)
: SubTask(name), : Connecting(name),
timeout_(5.0) timeout_(5.0)
{} {}
@ -66,9 +66,7 @@ bool Move::compute(){
return false; return false;
// finish subtask // finish subtask
moveit::task_constructor::SubTrajectory& trajectory= addTrajectory(res.trajectory_); connect(res.trajectory_, state_pair);
trajectory.hasBeginning(state_pair.first);
trajectory.hasEnding(state_pair.second);
return true; return true;
} }

View File

@ -1,5 +1,5 @@
#include "subtask_p.h"
#include <moveit_task_constructor/task.h> #include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/subtask.h>
#include <moveit_task_constructor/debug.h> #include <moveit_task_constructor/debug.h>
#include <ros/ros.h> #include <ros/ros.h>
@ -83,8 +83,8 @@ void Task::add( SubTaskPtr subtask ){
subtask->setPlanningPipeline( planner_ ); subtask->setPlanningPipeline( planner_ );
if( !subtasks_.empty() ){ if( !subtasks_.empty() ){
subtask->addPredecessor( subtasks_.back() ); subtask->impl_->addPredecessor( subtasks_.back() );
subtasks_.back()->addSuccessor( subtask ); subtasks_.back()->impl_->addSuccessor( subtask );
} }
subtasks_.push_back( subtask ); subtasks_.push_back( subtask );
@ -97,9 +97,9 @@ const robot_state::RobotState& Task::getCurrentRobotState() const {
void Task::printState(){ void Task::printState(){
for( auto& st : subtasks_ ){ for( auto& st : subtasks_ ){
std::cout std::cout
<< st->getBeginning().size() << " -> " << st->impl_->getBeginning().size() << " -> "
<< st->getTrajectories().size() << st->impl_->getTrajectories().size()
<< " <- " << st->getEnd().size() << " <- " << st->impl_->getEnd().size()
<< " / " << st->getName() << " / " << st->getName()
<< std::endl; << std::endl;
} }
@ -120,7 +120,7 @@ bool traverseFullTrajectories(
ret= cb(trace); ret= cb(trace);
} }
else if( start.end ){ else if( start.end ){
for( SubTrajectory* successor : start.end->next_trajectory ){ for( SubTrajectory* successor : start.end->nextTrajectories() ){
if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ){ if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ){
ret= false; ret= false;
break; break;
@ -134,14 +134,18 @@ bool traverseFullTrajectories(
} }
} }
bool Task::processSolutions(const Task::SolutionCallback& processor) const { bool Task::processSolutions(const Task::SolutionCallback& processor) {
const size_t nr_of_trajectories= subtasks_.size(); const size_t nr_of_trajectories= subtasks_.size();
std::vector<SubTrajectory*> trace; std::vector<SubTrajectory*> trace;
trace.reserve(nr_of_trajectories); trace.reserve(nr_of_trajectories);
for(SubTrajectory& subtraj : subtasks_.front()->getTrajectories()) for(SubTrajectory& st : subtasks_.front()->impl_->trajectories_)
if( !traverseFullTrajectories(subtraj, subtasks_.size(), processor, trace) ) if( !traverseFullTrajectories(st, subtasks_.size(), processor, trace) )
return false; return false;
return true; return true;
} }
bool Task::processSolutions(const Task::SolutionCallback& processor) const {
return const_cast<Task*>(this)->processSolutions(processor);
}
} } } }