mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
344713536d
commit
25fcc39a33
@ -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;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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_;
|
||||
}
|
||||
|
||||
38
src/task.cpp
38
src/task.cpp
@ -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();
|
||||
|
||||
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();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user