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

View File

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

View File

@ -12,72 +12,137 @@
#include <tuple>
namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene);
MOVEIT_CLASS_FORWARD(PlanningScene)
}
namespace planning_pipeline {
MOVEIT_CLASS_FORWARD(PlanningPipeline);
MOVEIT_CLASS_FORWARD(PlanningPipeline)
}
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;
class Task;
class SubTaskPrivate;
class SubTask {
friend class SubTaskPrivate; // allow access to impl_
friend class Task; // TODO: remove when we have SubTaskContainers
protected:
SubTaskPrivate* const impl_;
public:
SubTask(std::string name);
enum PropagationType { FORWARD, BACKWARD, ANYWAY, GENERATOR, CONNECTING };
~SubTask();
const std::string& getName() const;
virtual bool canCompute() = 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 setPlanningPipeline(planning_pipeline::PlanningPipelinePtr);
void addPredecessor(SubTaskPtr);
void addSuccessor(SubTaskPtr);
protected:
InterfaceState& fetchStateBeginning();
InterfaceState& fetchStateEnding();
std::pair<InterfaceState&, InterfaceState&> fetchStatePair();
/// can only instantiated by derived classes
SubTask(SubTaskPrivate *impl);
bool hasBeginning();
bool hasEnding();
bool hasStatePair();
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_;
/// methods called when a new InterfaceState was spawned
virtual inline void newBeginning(const InterfaceStateList::iterator& it) {}
virtual inline void newEnd(const InterfaceStateList::iterator& it) {}
planning_scene::PlanningSceneConstPtr scene_;
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 {
class CartesianPositionMotion : public SubTask {
class CartesianPositionMotion : public PropagatingAnyWay {
public:
CartesianPositionMotion(std::string name);

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -8,7 +8,7 @@
namespace moveit { namespace task_constructor {
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();
for(const SubTrajectory* t : solution){
if(t->trajectory){
@ -26,14 +26,14 @@ bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
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::Publisher pub = n.advertise<moveit_msgs::DisplayTrajectory>(topic, 1, true);
moveit_msgs::DisplayTrajectory dt;
robot_state::robotStateToRobotStateMsg(task.getCurrentRobotState(), dt.trajectory_start);
dt.model_id= task.getCurrentRobotState().getRobotModel()->getName();
Task::SolutionCallback processor= std::bind(
Task::SolutionCallback processor = std::bind(
&publishSolution, pub, dt, std::placeholders::_1, wait);
task.processSolutions(processor);
@ -53,7 +53,7 @@ void NewSolutionPublisher::publish()
robot_state::robotStateToRobotStateMsg(task_.getCurrentRobotState(), dt.trajectory_start);
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;
for(const SubTrajectory* t : solution){
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 {
SubTask::SubTask(std::string name)
: name_(name),
it_beginnings_(beginnings_.begin()),
it_endings_(endings_.begin()),
it_pairs_(beginnings_.begin(), endings_.begin())
{}
void SubTask::addPredecessor(SubTaskPtr prev_task){
predecessor_= SubTaskWeakPtr(prev_task);
SubTask::SubTask(SubTaskPrivate *impl)
: impl_(impl)
{
}
void SubTask::addSuccessor(SubTaskPtr next_task){
successor_= next_task;
SubTask::~SubTask()
{
delete impl_;
}
const std::string&
SubTask::getName(){
return name_;
const std::string& SubTask::getName() const {
return impl_->name_;
}
const std::list<InterfaceState>&
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){
void SubTask::setPlanningScene(planning_scene::PlanningSceneConstPtr scene){
scene_= scene;
}
void
SubTask::setPlanningPipeline(planning_pipeline::PlanningPipelinePtr planner){
void SubTask::setPlanningPipeline(planning_pipeline::PlanningPipelinePtr 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_;
++it_beginnings_;
return state;
void SubTaskPrivate::addPredecessor(SubTaskPtr prev_task){
predecessor_= SubTaskWeakPtr(prev_task);
}
InterfaceState&
SubTask::fetchStateEnding(){
if(it_endings_ == endings_.end())
throw std::runtime_error("no new state for ending available");
InterfaceState& state= *it_endings_;
++it_endings_;
return state;
void SubTaskPrivate::addSuccessor(SubTaskPtr next_task){
successor_= next_task;
}
std::pair<InterfaceState&, InterfaceState&>
SubTask::fetchStatePair(){
// TODO: implement this properly
return std::pair<InterfaceState&, InterfaceState&>(
*it_pairs_.first,
*(it_pairs_.second++));
}
SubTrajectory&
SubTask::addTrajectory(robot_trajectory::RobotTrajectoryPtr trajectory){
SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
trajectories_.emplace_back(trajectory);
return trajectories_.back();
}
void
SubTask::sendForward(SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){
void SubTaskPrivate::sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps){
if( successor_ ){
std::cout << "sending state forward to successor" << std::endl;
traj.end= successor_->newBeginning(ps, &traj);
traj.end= successor_->impl_->newBeginning(ps, &traj);
}
}
void
SubTask::sendBackward(SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){
void SubTaskPrivate::sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps){
if( !predecessor_.expired() ){
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
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){
InterfaceState* SubTaskPrivate::newBeginning(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_end){
assert( bool(ps) );
beginnings_.push_back( InterfaceState(ps, old_end, NULL) );
// 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( it_beginnings_ == beginnings_.end() )
--it_beginnings_;
// TODO: need to handle the pairs iterator
if( it_pairs_.first == beginnings_.end() )
--it_pairs_.first;
me_->newBeginning(--beginnings_.end());
return &beginnings_.back();
}
InterfaceState*
SubTask::newEnd(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_beginning){
InterfaceState* SubTaskPrivate::newEnd(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_beginning){
assert( bool(ps) );
endings_.push_back( InterfaceState(ps, NULL, old_beginning) );
// 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( it_endings_ == endings_.end() )
--it_endings_;
//TODO: need to handle the pairs iterator properly
if( it_pairs_.second == endings_.end() )
--it_pairs_.second;
me_->newEnd(--endings_.end());
return &endings_.back();
}
bool
SubTask::hasBeginning(){
return it_beginnings_ != beginnings_.end();
PropagatingAnyWay::PropagatingAnyWay(const std::string &name)
: 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
SubTask::hasEnding(){
return it_endings_ != endings_.end();
InterfaceState& PropagatingAnyWay::fetchStateBeginning(){
I(PropagatingAnyWay);
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
SubTask::hasStatePair(){
void PropagatingAnyWay::sendForward(const robot_trajectory::RobotTrajectoryPtr& t,
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
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 {
CartesianPositionMotion::CartesianPositionMotion(std::string name)
: SubTask(name),
: PropagatingAnyWay(name),
step_size_(0.005)
{
ros::NodeHandle nh;
@ -163,12 +163,7 @@ bool CartesianPositionMotion::_computeFromBeginning(){
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
robot_state= *result_state;
moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj);
trajectory.hasBeginning(beginning);
sendForward(trajectory, result_scene);
sendForward(traj, beginning, result_scene);
_publishTrajectory(*traj, *result_state);
}
@ -238,12 +233,7 @@ bool CartesianPositionMotion::_computeFromEnding(){
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
robot_state= *result_state;
moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj);
trajectory.hasEnding(ending);
sendBackward(trajectory, result_scene);
sendBackward(traj, result_scene, ending);
_publishTrajectory(*traj, *result_state);
return true;

View File

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

View File

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

View File

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

View File

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

View File

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