mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
specialised SubTask classes
SubTask cannot be instantiated directly, but only its derived classes that specify the type of operation: PropagatingAnyWay PropagatingForward PropagatingBackward Connecting Generator
This commit is contained in:
parent
f7fe7fc896
commit
fcccbae408
@ -12,7 +12,7 @@ MOVEIT_CLASS_FORWARD(Task)
|
|||||||
MOVEIT_CLASS_FORWARD(SubTrajectory)
|
MOVEIT_CLASS_FORWARD(SubTrajectory)
|
||||||
|
|
||||||
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
|
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
|
||||||
std::vector<SubTrajectory*>& solution, bool wait);
|
const std::vector<SubTrajectory*>& solution, bool wait);
|
||||||
|
|
||||||
void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true);
|
void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true);
|
||||||
|
|
||||||
|
|||||||
@ -8,56 +8,69 @@
|
|||||||
#include <cassert>
|
#include <cassert>
|
||||||
|
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene);
|
MOVEIT_CLASS_FORWARD(PlanningScene)
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace robot_trajectory {
|
namespace robot_trajectory {
|
||||||
MOVEIT_CLASS_FORWARD(RobotTrajectory);
|
MOVEIT_CLASS_FORWARD(RobotTrajectory)
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(SubTrajectory);
|
MOVEIT_CLASS_FORWARD(SubTrajectory)
|
||||||
MOVEIT_CLASS_FORWARD(InterfaceState);
|
MOVEIT_CLASS_FORWARD(InterfaceState)
|
||||||
|
|
||||||
|
class InterfaceState {
|
||||||
|
friend class SubTrajectory;
|
||||||
|
|
||||||
|
public:
|
||||||
|
typedef std::vector<SubTrajectory*> SubTrajectoryList;
|
||||||
|
|
||||||
struct InterfaceState {
|
|
||||||
InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next)
|
InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next)
|
||||||
: state(ps)
|
: state(ps)
|
||||||
{
|
{
|
||||||
if( previous )
|
if( previous )
|
||||||
previous_trajectory.push_back(previous);
|
previous_trajectories_.push_back(previous);
|
||||||
if( next )
|
if( next )
|
||||||
next_trajectory.push_back(next);
|
next_trajectories_.push_back(next);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<SubTrajectory*> previous_trajectory;
|
inline const SubTrajectoryList& previousTrajectories() const { return previous_trajectories_; }
|
||||||
std::vector<SubTrajectory*> next_trajectory;
|
inline const SubTrajectoryList& nextTrajectories() const { return next_trajectories_; }
|
||||||
|
|
||||||
|
public:
|
||||||
planning_scene::PlanningSceneConstPtr state;
|
planning_scene::PlanningSceneConstPtr state;
|
||||||
|
double cost; // minimal costs
|
||||||
|
|
||||||
|
private:
|
||||||
|
mutable SubTrajectoryList previous_trajectories_;
|
||||||
|
mutable SubTrajectoryList next_trajectories_;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct SubTrajectory {
|
struct SubTrajectory {
|
||||||
SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj)
|
SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj)
|
||||||
: trajectory(traj),
|
: trajectory(traj),
|
||||||
begin(NULL),
|
begin(NULL),
|
||||||
end(NULL)
|
end(NULL),
|
||||||
|
cost(0)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
void hasBeginning(InterfaceState& state){
|
void setBeginning(const InterfaceState& state){
|
||||||
assert(begin == NULL);
|
assert(begin == NULL);
|
||||||
begin= &state;
|
begin= &state;
|
||||||
state.next_trajectory.push_back(this);
|
state.next_trajectories_.push_back(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
void hasEnding(InterfaceState& state){
|
void setEnding(const InterfaceState& state){
|
||||||
assert(end == NULL);
|
assert(end == NULL);
|
||||||
end= &state;
|
end= &state;
|
||||||
state.previous_trajectory.push_back(this);
|
state.previous_trajectories_.push_back(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryPtr trajectory;
|
robot_trajectory::RobotTrajectoryPtr trajectory;
|
||||||
InterfaceState* begin;
|
const InterfaceState* begin;
|
||||||
InterfaceState* end;
|
const InterfaceState* end;
|
||||||
|
double cost;
|
||||||
};
|
};
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -12,72 +12,137 @@
|
|||||||
#include <tuple>
|
#include <tuple>
|
||||||
|
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene);
|
MOVEIT_CLASS_FORWARD(PlanningScene)
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace planning_pipeline {
|
namespace planning_pipeline {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningPipeline);
|
MOVEIT_CLASS_FORWARD(PlanningPipeline)
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(SubTask);
|
typedef std::list<InterfaceState> InterfaceStateList;
|
||||||
|
typedef std::pair<InterfaceState&, InterfaceState&> InterfaceStatePair;
|
||||||
|
|
||||||
|
MOVEIT_CLASS_FORWARD(SubTask)
|
||||||
typedef std::weak_ptr<SubTask> SubTaskWeakPtr;
|
typedef std::weak_ptr<SubTask> SubTaskWeakPtr;
|
||||||
|
|
||||||
|
class Task;
|
||||||
|
class SubTaskPrivate;
|
||||||
class SubTask {
|
class SubTask {
|
||||||
|
friend class SubTaskPrivate; // allow access to impl_
|
||||||
|
friend class Task; // TODO: remove when we have SubTaskContainers
|
||||||
|
|
||||||
|
protected:
|
||||||
|
SubTaskPrivate* const impl_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
SubTask(std::string name);
|
enum PropagationType { FORWARD, BACKWARD, ANYWAY, GENERATOR, CONNECTING };
|
||||||
|
|
||||||
|
~SubTask();
|
||||||
|
const std::string& getName() const;
|
||||||
|
|
||||||
virtual bool canCompute() = 0;
|
virtual bool canCompute() = 0;
|
||||||
virtual bool compute() = 0;
|
virtual bool compute() = 0;
|
||||||
|
|
||||||
const std::string& getName();
|
|
||||||
const std::list<InterfaceState>& getBeginning();
|
|
||||||
const std::list<InterfaceState>& getEnd();
|
|
||||||
std::list<SubTrajectory>& getTrajectories();
|
|
||||||
|
|
||||||
void setPlanningScene(planning_scene::PlanningSceneConstPtr);
|
void setPlanningScene(planning_scene::PlanningSceneConstPtr);
|
||||||
void setPlanningPipeline(planning_pipeline::PlanningPipelinePtr);
|
void setPlanningPipeline(planning_pipeline::PlanningPipelinePtr);
|
||||||
|
|
||||||
void addPredecessor(SubTaskPtr);
|
|
||||||
void addSuccessor(SubTaskPtr);
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
InterfaceState& fetchStateBeginning();
|
/// can only instantiated by derived classes
|
||||||
InterfaceState& fetchStateEnding();
|
SubTask(SubTaskPrivate *impl);
|
||||||
std::pair<InterfaceState&, InterfaceState&> fetchStatePair();
|
|
||||||
|
|
||||||
bool hasBeginning();
|
/// methods called when a new InterfaceState was spawned
|
||||||
bool hasEnding();
|
virtual inline void newBeginning(const InterfaceStateList::iterator& it) {}
|
||||||
bool hasStatePair();
|
virtual inline void newEnd(const InterfaceStateList::iterator& it) {}
|
||||||
|
|
||||||
SubTrajectory& addTrajectory(robot_trajectory::RobotTrajectoryPtr);
|
|
||||||
|
|
||||||
void sendForward(SubTrajectory&, planning_scene::PlanningSceneConstPtr);
|
|
||||||
void sendBackward(SubTrajectory&, planning_scene::PlanningSceneConstPtr);
|
|
||||||
void sendBothWays(SubTrajectory&, planning_scene::PlanningSceneConstPtr);
|
|
||||||
|
|
||||||
InterfaceState* newBeginning(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
|
|
||||||
InterfaceState* newEnd(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
|
|
||||||
|
|
||||||
const std::string name_;
|
|
||||||
|
|
||||||
// avoid shared pointers back
|
|
||||||
SubTaskWeakPtr predecessor_;
|
|
||||||
SubTaskPtr successor_;
|
|
||||||
|
|
||||||
std::list<SubTrajectory> trajectories_;
|
|
||||||
|
|
||||||
std::list<InterfaceState> beginnings_;
|
|
||||||
std::list<InterfaceState> endings_;
|
|
||||||
|
|
||||||
planning_scene::PlanningSceneConstPtr scene_;
|
planning_scene::PlanningSceneConstPtr scene_;
|
||||||
planning_pipeline::PlanningPipelinePtr planner_;
|
planning_pipeline::PlanningPipelinePtr planner_;
|
||||||
|
|
||||||
std::list<InterfaceState>::iterator it_beginnings_;
|
|
||||||
std::list<InterfaceState>::iterator it_endings_;
|
|
||||||
|
|
||||||
std::pair< std::list<InterfaceState>::iterator, std::list<InterfaceState>::iterator > it_pairs_;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class PropagatingAnyWayPrivate;
|
||||||
|
class PropagatingAnyWay : public SubTask {
|
||||||
|
public:
|
||||||
|
PropagatingAnyWay(const std::string& name);
|
||||||
|
static inline constexpr PropagationType type() { return ANYWAY; }
|
||||||
|
|
||||||
|
bool hasBeginning() const;
|
||||||
|
InterfaceState& fetchStateBeginning();
|
||||||
|
void sendForward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
|
||||||
|
const InterfaceState& from,
|
||||||
|
const planning_scene::PlanningSceneConstPtr& to,
|
||||||
|
double cost = 0);
|
||||||
|
|
||||||
|
bool hasEnding() const;
|
||||||
|
InterfaceState& fetchStateEnding();
|
||||||
|
void sendBackward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
|
||||||
|
const planning_scene::PlanningSceneConstPtr& from,
|
||||||
|
const InterfaceState& to,
|
||||||
|
double cost = 0);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
// constructor for use in derived classes
|
||||||
|
inline PropagatingAnyWay(PropagatingAnyWayPrivate* impl);
|
||||||
|
|
||||||
|
// get informed when new beginnings and endings become available
|
||||||
|
void newBeginning(const InterfaceStateList::iterator& it);
|
||||||
|
void newEnd(const InterfaceStateList::iterator& it);
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class PropagatingForward : public PropagatingAnyWay {
|
||||||
|
public:
|
||||||
|
PropagatingForward(const std::string& name);
|
||||||
|
static inline constexpr PropagationType type() { return FORWARD; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
// restrict access to backward methods
|
||||||
|
using PropagatingAnyWay::hasEnding;
|
||||||
|
using PropagatingAnyWay::fetchStateEnding;
|
||||||
|
using PropagatingAnyWay::sendBackward;
|
||||||
|
// don't care about new endings
|
||||||
|
inline void newEnd(const InterfaceStateList::iterator& it) {}
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class PropagatingBackward : public PropagatingAnyWay {
|
||||||
|
public:
|
||||||
|
PropagatingBackward(const std::string& name);
|
||||||
|
static inline constexpr PropagationType type() { return BACKWARD; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
// restrict access to forward methods
|
||||||
|
using PropagatingAnyWay::hasBeginning;
|
||||||
|
using PropagatingAnyWay::fetchStateBeginning;
|
||||||
|
using PropagatingAnyWay::sendForward;
|
||||||
|
// don't care about new beginnings
|
||||||
|
inline void newBeginning(const InterfaceStateList::iterator& it) {}
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class Generator : public SubTask {
|
||||||
|
public:
|
||||||
|
Generator(const std::string& name);
|
||||||
|
static inline constexpr PropagationType type() { return GENERATOR; }
|
||||||
|
|
||||||
|
void spawn(const planning_scene::PlanningSceneConstPtr &ps, double cost = 0);
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class Connecting : public SubTask {
|
||||||
|
public:
|
||||||
|
Connecting(const std::string& name);
|
||||||
|
static inline constexpr PropagationType type() { return CONNECTING; }
|
||||||
|
|
||||||
|
bool hasStatePair() const;
|
||||||
|
InterfaceStatePair fetchStatePair();
|
||||||
|
void connect(const robot_trajectory::RobotTrajectoryPtr&, const InterfaceStatePair&, double cost = 0);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
virtual void newBeginning(const InterfaceStateList::iterator& it);
|
||||||
|
virtual void newEnd(const InterfaceStateList::iterator& it);
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -15,7 +15,7 @@ MOVEIT_CLASS_FORWARD(MoveGroupInterface);
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
class CartesianPositionMotion : public SubTask {
|
class CartesianPositionMotion : public PropagatingAnyWay {
|
||||||
public:
|
public:
|
||||||
CartesianPositionMotion(std::string name);
|
CartesianPositionMotion(std::string name);
|
||||||
|
|
||||||
|
|||||||
@ -6,7 +6,7 @@
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
class CurrentState : public SubTask {
|
class CurrentState : public Generator {
|
||||||
public:
|
public:
|
||||||
CurrentState(std::string name);
|
CurrentState(std::string name);
|
||||||
|
|
||||||
|
|||||||
@ -14,7 +14,7 @@ MOVEIT_CLASS_FORWARD(MoveGroupInterface);
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
class GenerateGraspPose : public SubTask {
|
class GenerateGraspPose : public Generator {
|
||||||
public:
|
public:
|
||||||
GenerateGraspPose(std::string name);
|
GenerateGraspPose(std::string name);
|
||||||
|
|
||||||
|
|||||||
@ -5,12 +5,12 @@
|
|||||||
#include <moveit_task_constructor/subtask.h>
|
#include <moveit_task_constructor/subtask.h>
|
||||||
|
|
||||||
namespace moveit { namespace planning_interface {
|
namespace moveit { namespace planning_interface {
|
||||||
MOVEIT_CLASS_FORWARD(MoveGroupInterface);
|
MOVEIT_CLASS_FORWARD(MoveGroupInterface)
|
||||||
} }
|
} }
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
class Gripper : public SubTask {
|
class Gripper : public PropagatingForward {
|
||||||
public:
|
public:
|
||||||
Gripper(std::string name);
|
Gripper(std::string name);
|
||||||
|
|
||||||
|
|||||||
@ -10,7 +10,7 @@ MOVEIT_CLASS_FORWARD(MoveGroupInterface)
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
class Move : public SubTask {
|
class Move : public Connecting {
|
||||||
public:
|
public:
|
||||||
Move(std::string name);
|
Move(std::string name);
|
||||||
|
|
||||||
|
|||||||
@ -33,7 +33,7 @@ MOVEIT_CLASS_FORWARD(Task)
|
|||||||
|
|
||||||
class Task {
|
class Task {
|
||||||
public:
|
public:
|
||||||
typedef std::function<bool(std::vector<SubTrajectory*>&)> SolutionCallback;
|
typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback;
|
||||||
|
|
||||||
Task();
|
Task();
|
||||||
~Task();
|
~Task();
|
||||||
@ -42,6 +42,7 @@ public:
|
|||||||
|
|
||||||
bool plan();
|
bool plan();
|
||||||
|
|
||||||
|
bool processSolutions(const SolutionCallback &processor);
|
||||||
bool processSolutions(const SolutionCallback &processor) const;
|
bool processSolutions(const SolutionCallback &processor) const;
|
||||||
|
|
||||||
const moveit::core::RobotState &getCurrentRobotState() const;
|
const moveit::core::RobotState &getCurrentRobotState() const;
|
||||||
|
|||||||
@ -8,7 +8,7 @@
|
|||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
|
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
|
||||||
std::vector<SubTrajectory*>& solution, bool wait){
|
const std::vector<SubTrajectory*>& solution, bool wait){
|
||||||
dt.trajectory.clear();
|
dt.trajectory.clear();
|
||||||
for(const SubTrajectory* t : solution){
|
for(const SubTrajectory* t : solution){
|
||||||
if(t->trajectory){
|
if(t->trajectory){
|
||||||
@ -53,7 +53,7 @@ void NewSolutionPublisher::publish()
|
|||||||
robot_state::robotStateToRobotStateMsg(task_.getCurrentRobotState(), dt.trajectory_start);
|
robot_state::robotStateToRobotStateMsg(task_.getCurrentRobotState(), dt.trajectory_start);
|
||||||
dt.model_id = task_.getCurrentRobotState().getRobotModel()->getName();
|
dt.model_id = task_.getCurrentRobotState().getRobotModel()->getName();
|
||||||
|
|
||||||
Task::SolutionCallback processor = [this,&dt](std::vector<SubTrajectory*>& solution) {
|
Task::SolutionCallback processor = [this,&dt](const std::vector<SubTrajectory*>& solution) {
|
||||||
bool all_published = true;
|
bool all_published = true;
|
||||||
for(const SubTrajectory* t : solution){
|
for(const SubTrajectory* t : solution){
|
||||||
auto result = published_.insert(t);
|
auto result = published_.insert(t);
|
||||||
|
|||||||
273
src/subtask.cpp
273
src/subtask.cpp
@ -1,162 +1,219 @@
|
|||||||
#include <moveit_task_constructor/subtask.h>
|
#include "subtask_p.h"
|
||||||
|
|
||||||
|
// get correctly casted private impl pointer
|
||||||
|
#define I(Class) Class##Private * const impl = reinterpret_cast<Class##Private*>(impl_)
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
SubTask::SubTask(std::string name)
|
SubTask::SubTask(SubTaskPrivate *impl)
|
||||||
: name_(name),
|
: impl_(impl)
|
||||||
it_beginnings_(beginnings_.begin()),
|
{
|
||||||
it_endings_(endings_.begin()),
|
|
||||||
it_pairs_(beginnings_.begin(), endings_.begin())
|
|
||||||
{}
|
|
||||||
|
|
||||||
void SubTask::addPredecessor(SubTaskPtr prev_task){
|
|
||||||
predecessor_= SubTaskWeakPtr(prev_task);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void SubTask::addSuccessor(SubTaskPtr next_task){
|
SubTask::~SubTask()
|
||||||
successor_= next_task;
|
{
|
||||||
|
delete impl_;
|
||||||
}
|
}
|
||||||
|
|
||||||
const std::string&
|
const std::string& SubTask::getName() const {
|
||||||
SubTask::getName(){
|
return impl_->name_;
|
||||||
return name_;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
const std::list<InterfaceState>&
|
void SubTask::setPlanningScene(planning_scene::PlanningSceneConstPtr scene){
|
||||||
SubTask::getBeginning(){
|
|
||||||
return beginnings_;
|
|
||||||
}
|
|
||||||
|
|
||||||
const std::list<InterfaceState>&
|
|
||||||
SubTask::getEnd(){
|
|
||||||
return endings_;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::list<SubTrajectory>&
|
|
||||||
SubTask::getTrajectories(){
|
|
||||||
return trajectories_;
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
SubTask::setPlanningScene(planning_scene::PlanningSceneConstPtr scene){
|
|
||||||
scene_= scene;
|
scene_= scene;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void SubTask::setPlanningPipeline(planning_pipeline::PlanningPipelinePtr planner){
|
||||||
SubTask::setPlanningPipeline(planning_pipeline::PlanningPipelinePtr planner){
|
|
||||||
planner_= planner;
|
planner_= planner;
|
||||||
}
|
}
|
||||||
|
|
||||||
InterfaceState&
|
|
||||||
SubTask::fetchStateBeginning(){
|
|
||||||
if(it_beginnings_ == beginnings_.end())
|
|
||||||
throw std::runtime_error("no new state for beginning available");
|
|
||||||
|
|
||||||
InterfaceState& state= *it_beginnings_;
|
void SubTaskPrivate::addPredecessor(SubTaskPtr prev_task){
|
||||||
++it_beginnings_;
|
predecessor_= SubTaskWeakPtr(prev_task);
|
||||||
|
|
||||||
return state;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
InterfaceState&
|
void SubTaskPrivate::addSuccessor(SubTaskPtr next_task){
|
||||||
SubTask::fetchStateEnding(){
|
successor_= next_task;
|
||||||
if(it_endings_ == endings_.end())
|
|
||||||
throw std::runtime_error("no new state for ending available");
|
|
||||||
|
|
||||||
InterfaceState& state= *it_endings_;
|
|
||||||
++it_endings_;
|
|
||||||
|
|
||||||
return state;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
std::pair<InterfaceState&, InterfaceState&>
|
SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
|
||||||
SubTask::fetchStatePair(){
|
|
||||||
// TODO: implement this properly
|
|
||||||
return std::pair<InterfaceState&, InterfaceState&>(
|
|
||||||
*it_pairs_.first,
|
|
||||||
*(it_pairs_.second++));
|
|
||||||
}
|
|
||||||
|
|
||||||
SubTrajectory&
|
|
||||||
SubTask::addTrajectory(robot_trajectory::RobotTrajectoryPtr trajectory){
|
|
||||||
trajectories_.emplace_back(trajectory);
|
trajectories_.emplace_back(trajectory);
|
||||||
return trajectories_.back();
|
return trajectories_.back();
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void SubTaskPrivate::sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps){
|
||||||
SubTask::sendForward(SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){
|
|
||||||
if( successor_ ){
|
if( successor_ ){
|
||||||
std::cout << "sending state forward to successor" << std::endl;
|
std::cout << "sending state forward to successor" << std::endl;
|
||||||
traj.end= successor_->newBeginning(ps, &traj);
|
traj.end= successor_->impl_->newBeginning(ps, &traj);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void SubTaskPrivate::sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps){
|
||||||
SubTask::sendBackward(SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){
|
|
||||||
if( !predecessor_.expired() ){
|
if( !predecessor_.expired() ){
|
||||||
std::cout << "sending state backward to predecessor" << std::endl;
|
std::cout << "sending state backward to predecessor" << std::endl;
|
||||||
traj.begin= predecessor_.lock()->newEnd(ps, &traj);
|
traj.begin= predecessor_.lock()->impl_->newEnd(ps, &traj);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
InterfaceState* SubTaskPrivate::newBeginning(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_end){
|
||||||
SubTask::sendBothWays(SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){
|
|
||||||
std::cout << "sending state both ways" << std::endl;
|
|
||||||
if( !predecessor_.expired() )
|
|
||||||
sendBackward(traj, ps);
|
|
||||||
if( successor_ )
|
|
||||||
sendForward(traj, ps);
|
|
||||||
}
|
|
||||||
|
|
||||||
InterfaceState*
|
|
||||||
SubTask::newBeginning(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_end){
|
|
||||||
assert( bool(ps) );
|
assert( bool(ps) );
|
||||||
|
|
||||||
beginnings_.push_back( InterfaceState(ps, old_end, NULL) );
|
beginnings_.push_back( InterfaceState(ps, old_end, NULL) );
|
||||||
|
|
||||||
// we just appended a state to the list, but the iterator doesn't see it anymore
|
me_->newBeginning(--beginnings_.end());
|
||||||
// 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();
|
return &beginnings_.back();
|
||||||
}
|
}
|
||||||
|
|
||||||
InterfaceState*
|
InterfaceState* SubTaskPrivate::newEnd(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_beginning){
|
||||||
SubTask::newEnd(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_beginning){
|
|
||||||
assert( bool(ps) );
|
assert( bool(ps) );
|
||||||
endings_.push_back( InterfaceState(ps, NULL, old_beginning) );
|
endings_.push_back( InterfaceState(ps, NULL, old_beginning) );
|
||||||
|
|
||||||
// we just appended a state to the list, but the iterator doesn't see it anymore
|
me_->newEnd(--endings_.end());
|
||||||
// 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();
|
return &endings_.back();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
|
||||||
SubTask::hasBeginning(){
|
PropagatingAnyWay::PropagatingAnyWay(const std::string &name)
|
||||||
return it_beginnings_ != beginnings_.end();
|
: SubTask(new PropagatingAnyWayPrivate(this, name))
|
||||||
|
{}
|
||||||
|
|
||||||
|
PropagatingAnyWay::PropagatingAnyWay(PropagatingAnyWayPrivate *impl) : SubTask(impl) {}
|
||||||
|
|
||||||
|
bool PropagatingAnyWay::hasBeginning() const{
|
||||||
|
I(PropagatingAnyWay);
|
||||||
|
return impl->it_beginnings_ != impl->beginnings_.end();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
InterfaceState& PropagatingAnyWay::fetchStateBeginning(){
|
||||||
SubTask::hasEnding(){
|
I(PropagatingAnyWay);
|
||||||
return it_endings_ != endings_.end();
|
if(impl->it_beginnings_ == impl->beginnings_.end())
|
||||||
|
throw std::runtime_error("no new state for beginning available");
|
||||||
|
|
||||||
|
InterfaceState& state= *impl->it_beginnings_;
|
||||||
|
++impl->it_beginnings_;
|
||||||
|
|
||||||
|
return state;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
void PropagatingAnyWay::sendForward(const robot_trajectory::RobotTrajectoryPtr& t,
|
||||||
SubTask::hasStatePair(){
|
const InterfaceState& from,
|
||||||
|
const planning_scene::PlanningSceneConstPtr& to,
|
||||||
|
double cost){
|
||||||
|
SubTrajectory &trajectory = impl_->addTrajectory(t, cost);
|
||||||
|
trajectory.setBeginning(from);
|
||||||
|
impl_->sendForward(trajectory, to);
|
||||||
|
}
|
||||||
|
|
||||||
|
void PropagatingAnyWay::newBeginning(const InterfaceStateList::iterator &it)
|
||||||
|
{
|
||||||
|
I(PropagatingAnyWay);
|
||||||
|
// 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( impl->it_beginnings_ == impl->beginnings_.end() )
|
||||||
|
--impl->it_beginnings_;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool PropagatingAnyWay::hasEnding() const{
|
||||||
|
I(PropagatingAnyWay);
|
||||||
|
return impl->it_endings_ != impl->endings_.end();
|
||||||
|
}
|
||||||
|
|
||||||
|
InterfaceState& PropagatingAnyWay::fetchStateEnding(){
|
||||||
|
I(PropagatingAnyWay);
|
||||||
|
if(impl->it_endings_ == impl->endings_.end())
|
||||||
|
throw std::runtime_error("no new state for ending available");
|
||||||
|
|
||||||
|
InterfaceState& state= *impl->it_endings_;
|
||||||
|
++impl->it_endings_;
|
||||||
|
|
||||||
|
return state;
|
||||||
|
}
|
||||||
|
|
||||||
|
void PropagatingAnyWay::sendBackward(const robot_trajectory::RobotTrajectoryPtr& t,
|
||||||
|
const planning_scene::PlanningSceneConstPtr& from,
|
||||||
|
const InterfaceState& to,
|
||||||
|
double cost){
|
||||||
|
SubTrajectory& trajectory = impl_->addTrajectory(t, cost);
|
||||||
|
trajectory.setEnding(to);
|
||||||
|
impl_->sendBackward(trajectory, from);
|
||||||
|
}
|
||||||
|
|
||||||
|
void PropagatingAnyWay::newEnd(const InterfaceStateList::iterator &it)
|
||||||
|
{
|
||||||
|
I(PropagatingAnyWay);
|
||||||
|
// 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( impl->it_endings_ == impl->endings_.end() )
|
||||||
|
--impl->it_endings_;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
PropagatingForward::PropagatingForward(const std::string& name)
|
||||||
|
: PropagatingAnyWay(new PropagatingForwardPrivate(this, name))
|
||||||
|
{}
|
||||||
|
|
||||||
|
|
||||||
|
PropagatingBackward::PropagatingBackward(const std::string &name)
|
||||||
|
: PropagatingAnyWay(new PropagatingBackwardPrivate(this, name))
|
||||||
|
{}
|
||||||
|
|
||||||
|
|
||||||
|
Generator::Generator(const std::string &name)
|
||||||
|
: SubTask(new SubTaskPrivate(this, name))
|
||||||
|
{}
|
||||||
|
|
||||||
|
void Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost)
|
||||||
|
{
|
||||||
|
// empty trajectory ref -> this node only produces states
|
||||||
|
robot_trajectory::RobotTrajectoryPtr dummy;
|
||||||
|
moveit::task_constructor::SubTrajectory& trajectory = impl_->addTrajectory(dummy, cost);
|
||||||
|
|
||||||
|
std::cout << "spawning state" << std::endl;
|
||||||
|
impl_->sendBackward(trajectory, ps);
|
||||||
|
impl_->sendForward(trajectory, ps);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
Connecting::Connecting(const std::string &name)
|
||||||
|
: SubTask(new ConnectingPrivate(this, name))
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void Connecting::newBeginning(const InterfaceStateList::iterator& it)
|
||||||
|
{
|
||||||
|
I(Connecting);
|
||||||
|
// TODO: need to handle the pairs iterator
|
||||||
|
if( impl->it_pairs_.first == impl->beginnings_.end() )
|
||||||
|
--impl->it_pairs_.first;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Connecting::newEnd(const InterfaceStateList::iterator& it)
|
||||||
|
{
|
||||||
|
I(Connecting);
|
||||||
|
// TODO: need to handle the pairs iterator properly
|
||||||
|
if( impl->it_pairs_.second == impl->endings_.end() )
|
||||||
|
--impl->it_pairs_.second;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Connecting::hasStatePair() const{
|
||||||
|
I(Connecting);
|
||||||
// TODO: implement this properly
|
// TODO: implement this properly
|
||||||
return it_pairs_.first != beginnings_.end() && it_pairs_.second != endings_.end();
|
return impl->it_pairs_.first != impl->beginnings_.end() &&
|
||||||
|
impl->it_pairs_.second != impl->endings_.end();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
std::pair<InterfaceState&, InterfaceState&> Connecting::fetchStatePair(){
|
||||||
|
I(Connecting);
|
||||||
|
// TODO: implement this properly
|
||||||
|
return std::pair<InterfaceState&, InterfaceState&>
|
||||||
|
(*impl->it_pairs_.first, *(impl->it_pairs_.second++));
|
||||||
|
}
|
||||||
|
|
||||||
|
void Connecting::connect(const robot_trajectory::RobotTrajectoryPtr& t, const InterfaceStatePair& state_pair, double cost)
|
||||||
|
{
|
||||||
|
moveit::task_constructor::SubTrajectory& trajectory = impl_->addTrajectory(t, cost);
|
||||||
|
trajectory.setBeginning(state_pair.first);
|
||||||
|
trajectory.setEnding(state_pair.second);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
75
src/subtask_p.h
Normal file
75
src/subtask_p.h
Normal file
@ -0,0 +1,75 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <moveit_task_constructor/subtask.h>
|
||||||
|
|
||||||
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
|
class SubTaskPrivate {
|
||||||
|
public:
|
||||||
|
inline SubTaskPrivate(SubTask* me, const std::string& name)
|
||||||
|
: me_(me), name_(name)
|
||||||
|
{}
|
||||||
|
|
||||||
|
inline const InterfaceStateList& getBeginning() const { return beginnings_; }
|
||||||
|
inline const InterfaceStateList& getEnd() const { return endings_; }
|
||||||
|
inline const std::list<SubTrajectory>& getTrajectories() const { return trajectories_; }
|
||||||
|
|
||||||
|
void addPredecessor(SubTaskPtr);
|
||||||
|
void addSuccessor(SubTaskPtr);
|
||||||
|
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
|
||||||
|
|
||||||
|
void sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
|
||||||
|
void sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
|
||||||
|
|
||||||
|
InterfaceState* newBeginning(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
|
||||||
|
InterfaceState* newEnd(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
|
||||||
|
|
||||||
|
public:
|
||||||
|
SubTask* const me_; // pointer to owning instance
|
||||||
|
const std::string name_;
|
||||||
|
|
||||||
|
// avoid shared pointers back: why? to allow proper deallocation?
|
||||||
|
SubTaskWeakPtr predecessor_;
|
||||||
|
SubTaskPtr successor_;
|
||||||
|
|
||||||
|
InterfaceStateList beginnings_;
|
||||||
|
InterfaceStateList endings_;
|
||||||
|
std::list<SubTrajectory> trajectories_;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class PropagatingAnyWayPrivate : public SubTaskPrivate {
|
||||||
|
friend class PropagatingAnyWay;
|
||||||
|
|
||||||
|
public:
|
||||||
|
inline PropagatingAnyWayPrivate(PropagatingAnyWay *me, const std::string &name)
|
||||||
|
: SubTaskPrivate(me, name)
|
||||||
|
, it_beginnings_ (beginnings_.begin())
|
||||||
|
, it_endings_ (endings_.begin())
|
||||||
|
{}
|
||||||
|
|
||||||
|
protected:
|
||||||
|
InterfaceStateList::iterator it_beginnings_;
|
||||||
|
InterfaceStateList::iterator it_endings_;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
// for now, we use the same implementation for the reduced versions too
|
||||||
|
typedef PropagatingAnyWayPrivate PropagatingForwardPrivate;
|
||||||
|
typedef PropagatingAnyWayPrivate PropagatingBackwardPrivate;
|
||||||
|
|
||||||
|
|
||||||
|
class ConnectingPrivate : public SubTaskPrivate {
|
||||||
|
friend class Connecting;
|
||||||
|
|
||||||
|
public:
|
||||||
|
inline ConnectingPrivate(Connecting *me, const std::string &name)
|
||||||
|
: SubTaskPrivate(me, name)
|
||||||
|
, it_pairs_(beginnings_.begin(), endings_.begin())
|
||||||
|
{}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::pair<InterfaceStateList::iterator, InterfaceStateList::iterator> it_pairs_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} }
|
||||||
@ -15,7 +15,7 @@
|
|||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
CartesianPositionMotion::CartesianPositionMotion(std::string name)
|
CartesianPositionMotion::CartesianPositionMotion(std::string name)
|
||||||
: SubTask(name),
|
: PropagatingAnyWay(name),
|
||||||
step_size_(0.005)
|
step_size_(0.005)
|
||||||
{
|
{
|
||||||
ros::NodeHandle nh;
|
ros::NodeHandle nh;
|
||||||
@ -163,12 +163,7 @@ bool CartesianPositionMotion::_computeFromBeginning(){
|
|||||||
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
|
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
|
||||||
robot_state= *result_state;
|
robot_state= *result_state;
|
||||||
|
|
||||||
moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj);
|
sendForward(traj, beginning, result_scene);
|
||||||
|
|
||||||
trajectory.hasBeginning(beginning);
|
|
||||||
|
|
||||||
sendForward(trajectory, result_scene);
|
|
||||||
|
|
||||||
_publishTrajectory(*traj, *result_state);
|
_publishTrajectory(*traj, *result_state);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -238,12 +233,7 @@ bool CartesianPositionMotion::_computeFromEnding(){
|
|||||||
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
|
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
|
||||||
robot_state= *result_state;
|
robot_state= *result_state;
|
||||||
|
|
||||||
moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj);
|
sendBackward(traj, result_scene, ending);
|
||||||
|
|
||||||
trajectory.hasEnding(ending);
|
|
||||||
|
|
||||||
sendBackward(trajectory, result_scene);
|
|
||||||
|
|
||||||
_publishTrajectory(*traj, *result_state);
|
_publishTrajectory(*traj, *result_state);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
|
|||||||
@ -3,7 +3,7 @@
|
|||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
CurrentState::CurrentState(std::string name)
|
CurrentState::CurrentState(std::string name)
|
||||||
: SubTask(name)
|
: Generator(name)
|
||||||
{
|
{
|
||||||
ran_= false;
|
ran_= false;
|
||||||
}
|
}
|
||||||
@ -14,14 +14,8 @@ bool CurrentState::canCompute(){
|
|||||||
|
|
||||||
bool CurrentState::compute(){
|
bool CurrentState::compute(){
|
||||||
ran_= true;
|
ran_= true;
|
||||||
|
|
||||||
assert( scene_ );
|
assert( scene_ );
|
||||||
|
spawn(scene_);
|
||||||
// empty trajectory ref -> this node only produces states
|
|
||||||
robot_trajectory::RobotTrajectoryPtr traj;
|
|
||||||
moveit::task_constructor::SubTrajectory& trajectory= addTrajectory(traj);
|
|
||||||
|
|
||||||
sendBothWays(trajectory, scene_);
|
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -18,7 +18,7 @@
|
|||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
GenerateGraspPose::GenerateGraspPose(std::string name)
|
GenerateGraspPose::GenerateGraspPose(std::string name)
|
||||||
: SubTask(name),
|
: Generator(name),
|
||||||
timeout_(0.1),
|
timeout_(0.1),
|
||||||
angle_delta_(0.1),
|
angle_delta_(0.1),
|
||||||
max_ik_solutions_(0),
|
max_ik_solutions_(0),
|
||||||
@ -115,9 +115,6 @@ bool GenerateGraspPose::compute(){
|
|||||||
|
|
||||||
const std::string ik_link= ik_link_.empty() ? jmg_eef->getEndEffectorParentGroup().second : ik_link_;
|
const std::string ik_link= ik_link_.empty() ? jmg_eef->getEndEffectorParentGroup().second : ik_link_;
|
||||||
|
|
||||||
// empty trajectory -> this subtask only produces states
|
|
||||||
const robot_trajectory::RobotTrajectoryPtr traj;
|
|
||||||
|
|
||||||
if(!gripper_grasp_pose_.empty()){
|
if(!gripper_grasp_pose_.empty()){
|
||||||
grasp_state.setToDefaultValues(jmg_eef, gripper_grasp_pose_);
|
grasp_state.setToDefaultValues(jmg_eef, gripper_grasp_pose_);
|
||||||
}
|
}
|
||||||
@ -173,8 +170,7 @@ bool GenerateGraspPose::compute(){
|
|||||||
|
|
||||||
previous_solutions_.emplace_back();
|
previous_solutions_.emplace_back();
|
||||||
grasp_state.copyJointGroupPositions(jmg_active, previous_solutions_.back());
|
grasp_state.copyJointGroupPositions(jmg_active, previous_solutions_.back());
|
||||||
moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj);
|
spawn(grasp_scene);
|
||||||
sendBothWays(trajectory, grasp_scene);
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -12,7 +12,7 @@
|
|||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
Gripper::Gripper(std::string name)
|
Gripper::Gripper(std::string name)
|
||||||
: SubTask(name)
|
: PropagatingForward(name)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
void Gripper::setEndEffector(std::string eef){
|
void Gripper::setEndEffector(std::string eef){
|
||||||
@ -24,6 +24,7 @@ void Gripper::setAttachLink(std::string link){
|
|||||||
}
|
}
|
||||||
|
|
||||||
void Gripper::setFrom(std::string named_target){
|
void Gripper::setFrom(std::string named_target){
|
||||||
|
// TODO: this direction isn't handled yet
|
||||||
from_named_target_= named_target;
|
from_named_target_= named_target;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -53,7 +54,7 @@ bool Gripper::compute(){
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
InterfaceState& start= fetchStateBeginning();
|
const InterfaceState& start= fetchStateBeginning();
|
||||||
|
|
||||||
planning_scene::PlanningScenePtr scene(start.state->diff());
|
planning_scene::PlanningScenePtr scene(start.state->diff());
|
||||||
|
|
||||||
@ -83,9 +84,7 @@ bool Gripper::compute(){
|
|||||||
}
|
}
|
||||||
|
|
||||||
// finish subtask
|
// finish subtask
|
||||||
SubTrajectory& trajectory= addTrajectory(res.trajectory_);
|
sendForward(res.trajectory_, start, scene);
|
||||||
trajectory.hasBeginning(start);
|
|
||||||
sendForward(trajectory, scene);
|
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -12,7 +12,7 @@
|
|||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
Move::Move(std::string name)
|
Move::Move(std::string name)
|
||||||
: SubTask(name),
|
: Connecting(name),
|
||||||
timeout_(5.0)
|
timeout_(5.0)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
@ -66,9 +66,7 @@ bool Move::compute(){
|
|||||||
return false;
|
return false;
|
||||||
|
|
||||||
// finish subtask
|
// finish subtask
|
||||||
moveit::task_constructor::SubTrajectory& trajectory= addTrajectory(res.trajectory_);
|
connect(res.trajectory_, state_pair);
|
||||||
trajectory.hasBeginning(state_pair.first);
|
|
||||||
trajectory.hasEnding(state_pair.second);
|
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|||||||
24
src/task.cpp
24
src/task.cpp
@ -1,5 +1,5 @@
|
|||||||
|
#include "subtask_p.h"
|
||||||
#include <moveit_task_constructor/task.h>
|
#include <moveit_task_constructor/task.h>
|
||||||
#include <moveit_task_constructor/subtask.h>
|
|
||||||
#include <moveit_task_constructor/debug.h>
|
#include <moveit_task_constructor/debug.h>
|
||||||
|
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
@ -83,8 +83,8 @@ void Task::add( SubTaskPtr subtask ){
|
|||||||
subtask->setPlanningPipeline( planner_ );
|
subtask->setPlanningPipeline( planner_ );
|
||||||
|
|
||||||
if( !subtasks_.empty() ){
|
if( !subtasks_.empty() ){
|
||||||
subtask->addPredecessor( subtasks_.back() );
|
subtask->impl_->addPredecessor( subtasks_.back() );
|
||||||
subtasks_.back()->addSuccessor( subtask );
|
subtasks_.back()->impl_->addSuccessor( subtask );
|
||||||
}
|
}
|
||||||
|
|
||||||
subtasks_.push_back( subtask );
|
subtasks_.push_back( subtask );
|
||||||
@ -97,9 +97,9 @@ const robot_state::RobotState& Task::getCurrentRobotState() const {
|
|||||||
void Task::printState(){
|
void Task::printState(){
|
||||||
for( auto& st : subtasks_ ){
|
for( auto& st : subtasks_ ){
|
||||||
std::cout
|
std::cout
|
||||||
<< st->getBeginning().size() << " -> "
|
<< st->impl_->getBeginning().size() << " -> "
|
||||||
<< st->getTrajectories().size()
|
<< st->impl_->getTrajectories().size()
|
||||||
<< " <- " << st->getEnd().size()
|
<< " <- " << st->impl_->getEnd().size()
|
||||||
<< " / " << st->getName()
|
<< " / " << st->getName()
|
||||||
<< std::endl;
|
<< std::endl;
|
||||||
}
|
}
|
||||||
@ -120,7 +120,7 @@ bool traverseFullTrajectories(
|
|||||||
ret= cb(trace);
|
ret= cb(trace);
|
||||||
}
|
}
|
||||||
else if( start.end ){
|
else if( start.end ){
|
||||||
for( SubTrajectory* successor : start.end->next_trajectory ){
|
for( SubTrajectory* successor : start.end->nextTrajectories() ){
|
||||||
if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ){
|
if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ){
|
||||||
ret= false;
|
ret= false;
|
||||||
break;
|
break;
|
||||||
@ -134,14 +134,18 @@ bool traverseFullTrajectories(
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Task::processSolutions(const Task::SolutionCallback& processor) const {
|
bool Task::processSolutions(const Task::SolutionCallback& processor) {
|
||||||
const size_t nr_of_trajectories= subtasks_.size();
|
const size_t nr_of_trajectories= subtasks_.size();
|
||||||
std::vector<SubTrajectory*> trace;
|
std::vector<SubTrajectory*> trace;
|
||||||
trace.reserve(nr_of_trajectories);
|
trace.reserve(nr_of_trajectories);
|
||||||
for(SubTrajectory& subtraj : subtasks_.front()->getTrajectories())
|
for(SubTrajectory& st : subtasks_.front()->impl_->trajectories_)
|
||||||
if( !traverseFullTrajectories(subtraj, subtasks_.size(), processor, trace) )
|
if( !traverseFullTrajectories(st, subtasks_.size(), processor, trace) )
|
||||||
return false;
|
return false;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool Task::processSolutions(const Task::SolutionCallback& processor) const {
|
||||||
|
return const_cast<Task*>(this)->processSolutions(processor);
|
||||||
|
}
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user