mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
f7fe7fc896
commit
fcccbae408
@ -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);
|
||||
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
} }
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
|
||||
} }
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||
|
||||
class CurrentState : public SubTask {
|
||||
class CurrentState : public Generator {
|
||||
public:
|
||||
CurrentState(std::string name);
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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);
|
||||
|
||||
273
src/subtask.cpp
273
src/subtask.cpp
@ -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
75
src/subtask_p.h
Normal 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_;
|
||||
};
|
||||
|
||||
} }
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
24
src/task.cpp
24
src/task.cpp
@ -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);
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
Loading…
Reference in New Issue
Block a user