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 { struct InterfaceState {
InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next) InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next)
: state(ps), : state(ps),
previous_trajectory(previous), previous_trajectory(1, previous),
next_trajectory(next) next_trajectory(1, next)
{} {}
SubTrajectory* previous_trajectory; std::vector<SubTrajectory*> previous_trajectory;
SubTrajectory* next_trajectory; std::vector<SubTrajectory*> next_trajectory;
planning_scene::PlanningSceneConstPtr state; planning_scene::PlanningSceneConstPtr state;
}; };
struct SubTrajectory { struct SubTrajectory {
SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj) SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj)
: trajectory(traj) : trajectory(traj),
begin(NULL),
end(NULL)
{} {}
void connectToBeginning(InterfaceState& state){ void connectToBeginning(InterfaceState& state){
begin.push_back(&state); assert(begin == NULL);
assert(state.next_trajectory == NULL); begin= &state;
state.next_trajectory= this; state.next_trajectory.push_back(this);
} }
void connectToEnding(InterfaceState& state){ void connectToEnding(InterfaceState& state){
end.push_back(&state); assert(end == NULL);
assert(state.previous_trajectory == NULL); end= &state;
state.previous_trajectory= this; state.previous_trajectory.push_back(this);
} }
robot_trajectory::RobotTrajectoryPtr trajectory; robot_trajectory::RobotTrajectoryPtr trajectory;
std::vector<InterfaceState*> begin; InterfaceState* begin;
std::vector<InterfaceState*> end; InterfaceState* end;
}; };
} }

View File

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

View File

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