moveit_task_constructor/src/subtask.cpp
v4hn 5d9a9c22e4 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.
2017-03-06 20:47:56 +01:00

160 lines
4.6 KiB
C++

#include <moveit_task_constructor/subtask.h>
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){
predecessor_= prev_task.get();
}
void moveit::task_constructor::SubTask::addSuccessor(SubTaskPtr next_task){
successor_= next_task;
}
const std::string&
moveit::task_constructor::SubTask::getName(){
return name_;
}
const std::list<moveit::task_constructor::InterfaceState>&
moveit::task_constructor::SubTask::getBegin(){
return beginnings_;
}
const std::list<moveit::task_constructor::InterfaceState>&
moveit::task_constructor::SubTask::getEnd(){
return endings_;
}
const std::list<moveit::task_constructor::SubTrajectory>&
moveit::task_constructor::SubTask::getTrajectories(){
return trajectories_;
}
void
moveit::task_constructor::SubTask::setPlanningScene(planning_scene::PlanningSceneConstPtr scene){
scene_= scene;
}
void
moveit::task_constructor::SubTask::setPlanningPipeline(planning_pipeline::PlanningPipelinePtr planner){
planner_= planner;
}
moveit::task_constructor::InterfaceState&
moveit::task_constructor::SubTask::fetchStateBeginning(){
if(it_beginnings_ == beginnings_.end())
throw std::runtime_error("no new state for beginning available");
moveit::task_constructor::InterfaceState& state= *it_beginnings_;
++it_beginnings_;
return state;
}
moveit::task_constructor::InterfaceState&
moveit::task_constructor::SubTask::fetchStateEnding(){
if(it_endings_ == endings_.end())
throw std::runtime_error("no new state for ending available");
moveit::task_constructor::InterfaceState& state= *it_endings_;
++it_endings_;
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::SubTask::addTrajectory(robot_trajectory::RobotTrajectoryPtr trajectory){
trajectories_.emplace_back(trajectory);
return trajectories_.back();
}
void
moveit::task_constructor::SubTask::sendForward(moveit::task_constructor::SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){
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){
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( predecessor_ != NULL )
sendBackward(traj, ps);
if( successor_ )
sendForward(traj, ps);
}
moveit::task_constructor::InterfaceState*
moveit::task_constructor::SubTask::newBegin(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_end){
assert( bool(ps) );
beginnings_.push_back( InterfaceState(ps, old_end, NULL) );
// we just appended a state to the list, but the iterator doesn't see it anymore
// so let's point it at the new one
if( it_beginnings_ == beginnings_.end() )
--it_beginnings_;
// TODO: need to handle the pairs iterator
if( it_pairs_.first == beginnings_.end() )
--it_pairs_.first;
return &beginnings_.back();
}
moveit::task_constructor::InterfaceState*
moveit::task_constructor::SubTask::newEnd(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_beginning){
assert( bool(ps) );
endings_.push_back( InterfaceState(ps, NULL, old_beginning) );
// we just appended a state to the list, but the iterator doesn't see it anymore
// so let's point it at the new one
if( it_endings_ == endings_.end() )
--it_endings_;
//TODO: need to handle the pairs iterator properly
if( it_pairs_.second == endings_.end() )
--it_pairs_.second;
return &endings_.back();
}
bool
moveit::task_constructor::SubTask::hasBeginning(){
return it_beginnings_ != beginnings_.end();
}
bool
moveit::task_constructor::SubTask::hasEnding(){
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();
}