mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
ca6151cacb
commit
5d9a9c22e4
@ -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;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -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_;
|
||||
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user