diff --git a/include/moveit_task_constructor/storage.h b/include/moveit_task_constructor/storage.h index 2b9528e5..2958b712 100644 --- a/include/moveit_task_constructor/storage.h +++ b/include/moveit_task_constructor/storage.h @@ -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 previous_trajectory; + std::vector 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 begin; - std::vector end; + InterfaceState* begin; + InterfaceState* end; }; } diff --git a/include/moveit_task_constructor/subtask.h b/include/moveit_task_constructor/subtask.h index 9a356983..c8061e69 100644 --- a/include/moveit_task_constructor/subtask.h +++ b/include/moveit_task_constructor/subtask.h @@ -62,8 +62,8 @@ protected: const std::string name_; // maintain raw pointers to predecessors to avoid pointer circles - std::vector predecessors_; - std::vector successors_; + SubTask* predecessor_; + SubTaskPtr successor_; std::list trajectories_; diff --git a/src/subtask.cpp b/src/subtask.cpp index 3e399d21..6d83359e 100644 --- a/src/subtask.cpp +++ b/src/subtask.cpp @@ -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); }