task: flag previously returned trajectories to return them only once

This required to move from RobotTrajectorys to SubTrajectorys in more
interfaces, as well as fewer const-changes to handle the flags.
This commit is contained in:
v4hn 2017-03-24 12:46:59 +01:00
parent 344713536d
commit 25fcc39a33
5 changed files with 35 additions and 18 deletions

View File

@ -40,7 +40,8 @@ struct SubTrajectory {
SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj) SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj)
: trajectory(traj), : trajectory(traj),
begin(NULL), begin(NULL),
end(NULL) end(NULL),
flag(false)
{} {}
void hasBeginning(InterfaceState& state){ void hasBeginning(InterfaceState& state){
@ -58,6 +59,8 @@ struct SubTrajectory {
robot_trajectory::RobotTrajectoryPtr trajectory; robot_trajectory::RobotTrajectoryPtr trajectory;
InterfaceState* begin; InterfaceState* begin;
InterfaceState* end; InterfaceState* end;
bool flag;
}; };
} }

View File

@ -33,7 +33,7 @@ public:
const std::string& getName(); const std::string& getName();
const std::list<InterfaceState>& getBegin(); const std::list<InterfaceState>& getBegin();
const std::list<InterfaceState>& getEnd(); const std::list<InterfaceState>& getEnd();
const std::list<SubTrajectory>& getTrajectories(); std::list<SubTrajectory>& getTrajectories();
void setPlanningScene(planning_scene::PlanningSceneConstPtr); void setPlanningScene(planning_scene::PlanningSceneConstPtr);
void setPlanningPipeline(planning_pipeline::PlanningPipelinePtr); void setPlanningPipeline(planning_pipeline::PlanningPipelinePtr);

View File

@ -2,6 +2,8 @@
#pragma once #pragma once
#include <moveit_task_constructor/storage.h>
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <ros/ros.h> #include <ros/ros.h>
@ -33,7 +35,7 @@ MOVEIT_CLASS_FORWARD(Task);
class Task { class Task {
public: public:
typedef std::function<bool(std::vector<robot_trajectory::RobotTrajectoryPtr>&)> SolutionCallback; typedef std::function<bool(std::vector<SubTrajectory*>&)> SolutionCallback;
Task(); Task();
~Task(); ~Task();

View File

@ -31,7 +31,7 @@ moveit::task_constructor::SubTask::getEnd(){
return endings_; return endings_;
} }
const std::list<moveit::task_constructor::SubTrajectory>& std::list<moveit::task_constructor::SubTrajectory>&
moveit::task_constructor::SubTask::getTrajectories(){ moveit::task_constructor::SubTask::getTrajectories(){
return trajectories_; return trajectories_;
} }

View File

@ -103,50 +103,62 @@ void moveit::task_constructor::Task::printState(){
namespace { namespace {
bool traverseFullTrajectories( bool traverseFullTrajectories(
const moveit::task_constructor::SubTrajectory& start, moveit::task_constructor::SubTrajectory& start,
int nr_of_trajectories, int nr_of_trajectories,
moveit::task_constructor::Task::SolutionCallback& cb, moveit::task_constructor::Task::SolutionCallback& cb,
std::vector<robot_trajectory::RobotTrajectoryPtr>& trace) std::vector<moveit::task_constructor::SubTrajectory*>& trace)
{ {
bool ret= true; bool ret= true;
if(start.trajectory) trace.push_back(&start);
trace.push_back(start.trajectory);
if(nr_of_trajectories == 1){ if(nr_of_trajectories == 1){
ret= cb(trace); ret= cb(trace);
} }
else if( start.end ){ else if( start.end ){
for( const moveit::task_constructor::SubTrajectory* successor : start.end->next_trajectory ){ for( moveit::task_constructor::SubTrajectory* successor : start.end->next_trajectory ){
if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ) if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ){
ret= false; ret= false;
break; break;
}
} }
} }
if(start.trajectory) trace.pop_back();
trace.pop_back();
return true; return ret;
} }
} }
bool moveit::task_constructor::Task::processSolutions(moveit::task_constructor::Task::SolutionCallback& processor ){ bool moveit::task_constructor::Task::processSolutions(moveit::task_constructor::Task::SolutionCallback& processor ){
const size_t nr_of_trajectories= subtasks_.size(); const size_t nr_of_trajectories= subtasks_.size();
std::vector<robot_trajectory::RobotTrajectoryPtr> trace; std::vector<moveit::task_constructor::SubTrajectory*> trace;
trace.reserve(nr_of_trajectories); trace.reserve(nr_of_trajectories);
for(const SubTrajectory& subtraj : subtasks_.front()->getTrajectories()) for(SubTrajectory& subtraj : subtasks_.front()->getTrajectories())
if( !traverseFullTrajectories(subtraj, subtasks_.size(), processor, trace) ) if( !traverseFullTrajectories(subtraj, subtasks_.size(), processor, trace) )
return false; return false;
return true; return true;
} }
namespace { namespace {
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, std::vector<robot_trajectory::RobotTrajectoryPtr>& solution){ bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, std::vector<moveit::task_constructor::SubTrajectory*>& solution){
bool all_flagged= true;
for(auto& t : solution){ for(auto& t : solution){
dt.trajectory.emplace_back(); all_flagged= all_flagged && t->flag;
t->getRobotTrajectoryMsg(dt.trajectory.back());
} }
if( all_flagged ){
return true;
}
for(auto& t : solution){
if(t->trajectory){
dt.trajectory.emplace_back();
t->trajectory->getRobotTrajectoryMsg(dt.trajectory.back());
}
t->flag= true;
}
std::cout << "publishing solution" << std::endl; std::cout << "publishing solution" << std::endl;
pub.publish(dt); pub.publish(dt);
ros::Duration(10.0).sleep(); ros::Duration(10.0).sleep();