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

View File

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

View File

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

View File

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

View File

@ -103,50 +103,62 @@ void moveit::task_constructor::Task::printState(){
namespace {
bool traverseFullTrajectories(
const moveit::task_constructor::SubTrajectory& start,
moveit::task_constructor::SubTrajectory& start,
int nr_of_trajectories,
moveit::task_constructor::Task::SolutionCallback& cb,
std::vector<robot_trajectory::RobotTrajectoryPtr>& trace)
std::vector<moveit::task_constructor::SubTrajectory*>& trace)
{
bool ret= true;
if(start.trajectory)
trace.push_back(start.trajectory);
trace.push_back(&start);
if(nr_of_trajectories == 1){
ret= cb(trace);
}
else if( start.end ){
for( const moveit::task_constructor::SubTrajectory* successor : start.end->next_trajectory ){
if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) )
for( moveit::task_constructor::SubTrajectory* successor : start.end->next_trajectory ){
if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ){
ret= false;
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 ){
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);
for(const SubTrajectory& subtraj : subtasks_.front()->getTrajectories())
for(SubTrajectory& subtraj : subtasks_.front()->getTrajectories())
if( !traverseFullTrajectories(subtraj, subtasks_.size(), processor, trace) )
return false;
return true;
}
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){
dt.trajectory.emplace_back();
t->getRobotTrajectoryMsg(dt.trajectory.back());
all_flagged= all_flagged && t->flag;
}
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;
pub.publish(dt);
ros::Duration(10.0).sleep();