add stub for pair iteration

This is not too hard, but takes a bit of time to get right,
so I'll add a stub for the moment.
This commit is contained in:
v4hn 2017-03-06 19:34:11 +01:00
parent 895d7842a3
commit a9c9a61710
2 changed files with 27 additions and 1 deletions

View File

@ -48,6 +48,7 @@ protected:
bool hasBeginning(); bool hasBeginning();
bool hasEnding(); bool hasEnding();
bool hasStatePair();
SubTrajectory& addTrajectory(robot_trajectory::RobotTrajectoryPtr); SubTrajectory& addTrajectory(robot_trajectory::RobotTrajectoryPtr);
@ -74,6 +75,8 @@ protected:
std::list<InterfaceState>::iterator it_beginnings_; std::list<InterfaceState>::iterator it_beginnings_;
std::list<InterfaceState>::iterator it_endings_; std::list<InterfaceState>::iterator it_endings_;
std::pair< std::list<InterfaceState>::iterator, std::list<InterfaceState>::iterator > it_pairs_;
}; };
} }

View File

@ -3,7 +3,8 @@
moveit::task_constructor::SubTask::SubTask(std::string name) moveit::task_constructor::SubTask::SubTask(std::string name)
: name_(name), : name_(name),
it_beginnings_(beginnings_.begin()), it_beginnings_(beginnings_.begin()),
it_endings_(endings_.begin()) it_endings_(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){
@ -66,6 +67,14 @@ moveit::task_constructor::SubTask::fetchStateEnding(){
return state; return state;
} }
std::pair<moveit::task_constructor::InterfaceState&, moveit::task_constructor::InterfaceState&>
moveit::task_constructor::SubTask::fetchStatePair(){
// TODO: implement this properly
return std::pair<moveit::task_constructor::InterfaceState&, moveit::task_constructor::InterfaceState&>(
*it_pairs_.first,
*(it_pairs_.second++));
}
moveit::task_constructor::SubTrajectory& moveit::task_constructor::SubTrajectory&
moveit::task_constructor::SubTask::addTrajectory(robot_trajectory::RobotTrajectoryPtr trajectory){ moveit::task_constructor::SubTask::addTrajectory(robot_trajectory::RobotTrajectoryPtr trajectory){
trajectories_.emplace_back(trajectory); trajectories_.emplace_back(trajectory);
@ -108,6 +117,10 @@ moveit::task_constructor::SubTask::newBegin(planning_scene::PlanningSceneConstPt
if( it_beginnings_ == beginnings_.end() ) if( it_beginnings_ == beginnings_.end() )
--it_beginnings_; --it_beginnings_;
// TODO: need to handle the pairs iterator
if( it_pairs_.first == beginnings_.end() )
--it_pairs_.first;
return &beginnings_.back(); return &beginnings_.back();
} }
@ -121,6 +134,10 @@ moveit::task_constructor::SubTask::newEnd(planning_scene::PlanningSceneConstPtr
if( it_endings_ == endings_.end() ) if( it_endings_ == endings_.end() )
--it_endings_; --it_endings_;
//TODO: need to handle the pairs iterator properly
if( it_pairs_.second == endings_.end() )
--it_pairs_.second;
return &endings_.back(); return &endings_.back();
} }
@ -133,3 +150,9 @@ bool
moveit::task_constructor::SubTask::hasEnding(){ moveit::task_constructor::SubTask::hasEnding(){
return it_endings_ != endings_.end(); return it_endings_ != endings_.end();
} }
bool
moveit::task_constructor::SubTask::hasStatePair(){
// TODO: implement this properly
return it_pairs_.first != beginnings_.end() && it_pairs_.second != endings_.end();
}