invert 1:n relationship between InterfaceState and SubTrajectory

Starting with the Move Subtask, a single InterfaceState might have
multiple trajectories associated.

On the other hand, a graph structure of nodes is arbitrary difficult to handle,
debug and even specify. Because all examples considered up to now are
linear, I decided to stay with a linear ordering for now.
This should also allow much cleaner visualizations.
This commit is contained in:
v4hn 2017-03-06 20:47:56 +01:00
parent ca6151cacb
commit 5d9a9c22e4
3 changed files with 30 additions and 27 deletions

View File

@ -23,36 +23,38 @@ MOVEIT_CLASS_FORWARD(InterfaceState);
struct InterfaceState {
InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next)
: state(ps),
previous_trajectory(previous),
next_trajectory(next)
previous_trajectory(1, previous),
next_trajectory(1, next)
{}
SubTrajectory* previous_trajectory;
SubTrajectory* next_trajectory;
std::vector<SubTrajectory*> previous_trajectory;
std::vector<SubTrajectory*> next_trajectory;
planning_scene::PlanningSceneConstPtr state;
};
struct SubTrajectory {
SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj)
: trajectory(traj)
: trajectory(traj),
begin(NULL),
end(NULL)
{}
void connectToBeginning(InterfaceState& state){
begin.push_back(&state);
assert(state.next_trajectory == NULL);
state.next_trajectory= this;
assert(begin == NULL);
begin= &state;
state.next_trajectory.push_back(this);
}
void connectToEnding(InterfaceState& state){
end.push_back(&state);
assert(state.previous_trajectory == NULL);
state.previous_trajectory= this;
assert(end == NULL);
end= &state;
state.previous_trajectory.push_back(this);
}
robot_trajectory::RobotTrajectoryPtr trajectory;
std::vector<InterfaceState*> begin;
std::vector<InterfaceState*> end;
InterfaceState* begin;
InterfaceState* end;
};
}

View File

@ -62,8 +62,8 @@ protected:
const std::string name_;
// maintain raw pointers to predecessors to avoid pointer circles
std::vector<SubTask*> predecessors_;
std::vector<SubTaskPtr> successors_;
SubTask* predecessor_;
SubTaskPtr successor_;
std::list<SubTrajectory> trajectories_;

View File

@ -2,17 +2,18 @@
moveit::task_constructor::SubTask::SubTask(std::string name)
: name_(name),
predecessor_(NULL),
it_beginnings_(beginnings_.begin()),
it_endings_(endings_.begin()),
it_pairs_(beginnings_.begin(), endings_.begin())
{};
void moveit::task_constructor::SubTask::addPredecessor(SubTaskPtr prev_task){
predecessors_.push_back( prev_task.get() );
predecessor_= prev_task.get();
}
void moveit::task_constructor::SubTask::addSuccessor(SubTaskPtr next_task){
successors_.push_back( next_task );
successor_= next_task;
}
const std::string&
@ -83,26 +84,26 @@ moveit::task_constructor::SubTask::addTrajectory(robot_trajectory::RobotTrajecto
void
moveit::task_constructor::SubTask::sendForward(moveit::task_constructor::SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){
std::cout << "sending state forward to " << successors_.size() << " successors" << std::endl;
traj.end.reserve(successors_.size());
for( SubTaskPtr succ : successors_ )
traj.end.push_back( succ->newBegin(ps, &traj) );
if( successor_ ){
std::cout << "sending state forward to successor" << std::endl;
traj.end= successor_->newBegin(ps, &traj);
}
}
void
moveit::task_constructor::SubTask::sendBackward(moveit::task_constructor::SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){
std::cout << "sending state backward to " << predecessors_.size() << " successors" << std::endl;
traj.begin.reserve(successors_.size());
for( SubTask* pred : predecessors_ )
traj.begin.push_back( pred->newEnd(ps, &traj) );
if( predecessor_ != NULL ){
std::cout << "sending state backward to predecessor" << std::endl;
traj.begin= predecessor_->newEnd(ps, &traj);
}
}
void
moveit::task_constructor::SubTask::sendBothWays(moveit::task_constructor::SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){
std::cout << "sending state both ways" << std::endl;
if( predecessors_.size() > 0 )
if( predecessor_ != NULL )
sendBackward(traj, ps);
if( successors_.size() > 0 )
if( successor_ )
sendForward(traj, ps);
}