diff --git a/include/moveit_task_constructor/debug.h b/include/moveit_task_constructor/debug.h index 7f429de3..c99abba5 100644 --- a/include/moveit_task_constructor/debug.h +++ b/include/moveit_task_constructor/debug.h @@ -12,7 +12,7 @@ MOVEIT_CLASS_FORWARD(Task) MOVEIT_CLASS_FORWARD(SubTrajectory) bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, - std::vector& solution, bool wait); + const std::vector& solution, bool wait); void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true); diff --git a/include/moveit_task_constructor/storage.h b/include/moveit_task_constructor/storage.h index 8b7bcb36..b7e14266 100644 --- a/include/moveit_task_constructor/storage.h +++ b/include/moveit_task_constructor/storage.h @@ -8,56 +8,69 @@ #include namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene); +MOVEIT_CLASS_FORWARD(PlanningScene) } namespace robot_trajectory { -MOVEIT_CLASS_FORWARD(RobotTrajectory); +MOVEIT_CLASS_FORWARD(RobotTrajectory) } namespace moveit { namespace task_constructor { -MOVEIT_CLASS_FORWARD(SubTrajectory); -MOVEIT_CLASS_FORWARD(InterfaceState); +MOVEIT_CLASS_FORWARD(SubTrajectory) +MOVEIT_CLASS_FORWARD(InterfaceState) + +class InterfaceState { + friend class SubTrajectory; + +public: + typedef std::vector SubTrajectoryList; -struct InterfaceState { InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next) : state(ps) { if( previous ) - previous_trajectory.push_back(previous); + previous_trajectories_.push_back(previous); if( next ) - next_trajectory.push_back(next); + next_trajectories_.push_back(next); } - std::vector previous_trajectory; - std::vector next_trajectory; + inline const SubTrajectoryList& previousTrajectories() const { return previous_trajectories_; } + inline const SubTrajectoryList& nextTrajectories() const { return next_trajectories_; } +public: planning_scene::PlanningSceneConstPtr state; + double cost; // minimal costs + +private: + mutable SubTrajectoryList previous_trajectories_; + mutable SubTrajectoryList next_trajectories_; }; struct SubTrajectory { SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj) : trajectory(traj), begin(NULL), - end(NULL) + end(NULL), + cost(0) {} - void hasBeginning(InterfaceState& state){ + void setBeginning(const InterfaceState& state){ assert(begin == NULL); 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); end= &state; - state.previous_trajectory.push_back(this); + state.previous_trajectories_.push_back(this); } robot_trajectory::RobotTrajectoryPtr trajectory; - InterfaceState* begin; - InterfaceState* end; + const InterfaceState* begin; + const InterfaceState* end; + double cost; }; } } diff --git a/include/moveit_task_constructor/subtask.h b/include/moveit_task_constructor/subtask.h index 0067470c..8e9aaa8f 100644 --- a/include/moveit_task_constructor/subtask.h +++ b/include/moveit_task_constructor/subtask.h @@ -12,72 +12,137 @@ #include namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene); +MOVEIT_CLASS_FORWARD(PlanningScene) } namespace planning_pipeline { -MOVEIT_CLASS_FORWARD(PlanningPipeline); +MOVEIT_CLASS_FORWARD(PlanningPipeline) } namespace moveit { namespace task_constructor { -MOVEIT_CLASS_FORWARD(SubTask); +typedef std::list InterfaceStateList; +typedef std::pair InterfaceStatePair; + +MOVEIT_CLASS_FORWARD(SubTask) typedef std::weak_ptr SubTaskWeakPtr; +class Task; +class SubTaskPrivate; class SubTask { + friend class SubTaskPrivate; // allow access to impl_ + friend class Task; // TODO: remove when we have SubTaskContainers + +protected: + SubTaskPrivate* const impl_; + public: - SubTask(std::string name); + enum PropagationType { FORWARD, BACKWARD, ANYWAY, GENERATOR, CONNECTING }; + + ~SubTask(); + const std::string& getName() const; virtual bool canCompute() = 0; virtual bool compute() = 0; - const std::string& getName(); - const std::list& getBeginning(); - const std::list& getEnd(); - std::list& getTrajectories(); - void setPlanningScene(planning_scene::PlanningSceneConstPtr); void setPlanningPipeline(planning_pipeline::PlanningPipelinePtr); - void addPredecessor(SubTaskPtr); - void addSuccessor(SubTaskPtr); - protected: - InterfaceState& fetchStateBeginning(); - InterfaceState& fetchStateEnding(); - std::pair fetchStatePair(); + /// can only instantiated by derived classes + SubTask(SubTaskPrivate *impl); - bool hasBeginning(); - bool hasEnding(); - bool hasStatePair(); - - 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 trajectories_; - - std::list beginnings_; - std::list endings_; + /// methods called when a new InterfaceState was spawned + virtual inline void newBeginning(const InterfaceStateList::iterator& it) {} + virtual inline void newEnd(const InterfaceStateList::iterator& it) {} planning_scene::PlanningSceneConstPtr scene_; planning_pipeline::PlanningPipelinePtr planner_; - - std::list::iterator it_beginnings_; - std::list::iterator it_endings_; - - std::pair< std::list::iterator, std::list::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); +}; + + } } diff --git a/include/moveit_task_constructor/subtasks/cartesian_position_motion.h b/include/moveit_task_constructor/subtasks/cartesian_position_motion.h index 9e435d6a..f2bc0b8d 100644 --- a/include/moveit_task_constructor/subtasks/cartesian_position_motion.h +++ b/include/moveit_task_constructor/subtasks/cartesian_position_motion.h @@ -15,7 +15,7 @@ MOVEIT_CLASS_FORWARD(MoveGroupInterface); namespace moveit { namespace task_constructor { namespace subtasks { -class CartesianPositionMotion : public SubTask { +class CartesianPositionMotion : public PropagatingAnyWay { public: CartesianPositionMotion(std::string name); diff --git a/include/moveit_task_constructor/subtasks/current_state.h b/include/moveit_task_constructor/subtasks/current_state.h index 587fa066..3680c865 100644 --- a/include/moveit_task_constructor/subtasks/current_state.h +++ b/include/moveit_task_constructor/subtasks/current_state.h @@ -6,7 +6,7 @@ namespace moveit { namespace task_constructor { namespace subtasks { -class CurrentState : public SubTask { +class CurrentState : public Generator { public: CurrentState(std::string name); diff --git a/include/moveit_task_constructor/subtasks/generate_grasp_pose.h b/include/moveit_task_constructor/subtasks/generate_grasp_pose.h index e0b6c60a..8fcce629 100644 --- a/include/moveit_task_constructor/subtasks/generate_grasp_pose.h +++ b/include/moveit_task_constructor/subtasks/generate_grasp_pose.h @@ -14,7 +14,7 @@ MOVEIT_CLASS_FORWARD(MoveGroupInterface); namespace moveit { namespace task_constructor { namespace subtasks { -class GenerateGraspPose : public SubTask { +class GenerateGraspPose : public Generator { public: GenerateGraspPose(std::string name); diff --git a/include/moveit_task_constructor/subtasks/gripper.h b/include/moveit_task_constructor/subtasks/gripper.h index c09d35a5..d04bdc00 100644 --- a/include/moveit_task_constructor/subtasks/gripper.h +++ b/include/moveit_task_constructor/subtasks/gripper.h @@ -5,12 +5,12 @@ #include namespace moveit { namespace planning_interface { -MOVEIT_CLASS_FORWARD(MoveGroupInterface); +MOVEIT_CLASS_FORWARD(MoveGroupInterface) } } namespace moveit { namespace task_constructor { namespace subtasks { -class Gripper : public SubTask { +class Gripper : public PropagatingForward { public: Gripper(std::string name); diff --git a/include/moveit_task_constructor/subtasks/move.h b/include/moveit_task_constructor/subtasks/move.h index 58efa932..ce3120bc 100644 --- a/include/moveit_task_constructor/subtasks/move.h +++ b/include/moveit_task_constructor/subtasks/move.h @@ -10,7 +10,7 @@ MOVEIT_CLASS_FORWARD(MoveGroupInterface) namespace moveit { namespace task_constructor { namespace subtasks { -class Move : public SubTask { +class Move : public Connecting { public: Move(std::string name); diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 8c0a14e8..9f9cdb11 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -33,7 +33,7 @@ MOVEIT_CLASS_FORWARD(Task) class Task { public: - typedef std::function&)> SolutionCallback; + typedef std::function&)> SolutionCallback; Task(); ~Task(); @@ -42,6 +42,7 @@ public: bool plan(); + bool processSolutions(const SolutionCallback &processor); bool processSolutions(const SolutionCallback &processor) const; const moveit::core::RobotState &getCurrentRobotState() const; @@ -50,7 +51,7 @@ public: void clear(); -protected: + protected: std::vector subtasks_; planning_scene::PlanningScenePtr scene_; diff --git a/src/debug.cpp b/src/debug.cpp index 8335a457..648df3b1 100644 --- a/src/debug.cpp +++ b/src/debug.cpp @@ -8,7 +8,7 @@ namespace moveit { namespace task_constructor { bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, - std::vector& solution, bool wait){ + const std::vector& solution, bool wait){ dt.trajectory.clear(); for(const SubTrajectory* t : solution){ if(t->trajectory){ @@ -26,14 +26,14 @@ bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, return true; } -void publishAllPlans(const Task &task, const std::string &topic, bool wait){ +void publishAllPlans(const Task &task, const std::string &topic, bool wait) { ros::NodeHandle n; ros::Publisher pub = n.advertise(topic, 1, true); moveit_msgs::DisplayTrajectory dt; robot_state::robotStateToRobotStateMsg(task.getCurrentRobotState(), dt.trajectory_start); dt.model_id= task.getCurrentRobotState().getRobotModel()->getName(); - Task::SolutionCallback processor= std::bind( + Task::SolutionCallback processor = std::bind( &publishSolution, pub, dt, std::placeholders::_1, wait); task.processSolutions(processor); @@ -53,7 +53,7 @@ void NewSolutionPublisher::publish() robot_state::robotStateToRobotStateMsg(task_.getCurrentRobotState(), dt.trajectory_start); dt.model_id = task_.getCurrentRobotState().getRobotModel()->getName(); - Task::SolutionCallback processor = [this,&dt](std::vector& solution) { + Task::SolutionCallback processor = [this,&dt](const std::vector& solution) { bool all_published = true; for(const SubTrajectory* t : solution){ auto result = published_.insert(t); diff --git a/src/subtask.cpp b/src/subtask.cpp index aae04cbc..188d6372 100644 --- a/src/subtask.cpp +++ b/src/subtask.cpp @@ -1,162 +1,219 @@ -#include +#include "subtask_p.h" + +// get correctly casted private impl pointer +#define I(Class) Class##Private * const impl = reinterpret_cast(impl_) namespace moveit { namespace task_constructor { -SubTask::SubTask(std::string name) - : name_(name), - it_beginnings_(beginnings_.begin()), - it_endings_(endings_.begin()), - it_pairs_(beginnings_.begin(), endings_.begin()) -{} - -void SubTask::addPredecessor(SubTaskPtr prev_task){ - predecessor_= SubTaskWeakPtr(prev_task); +SubTask::SubTask(SubTaskPrivate *impl) + : impl_(impl) +{ } -void SubTask::addSuccessor(SubTaskPtr next_task){ - successor_= next_task; +SubTask::~SubTask() +{ + delete impl_; } -const std::string& -SubTask::getName(){ - return name_; +const std::string& SubTask::getName() const { + return impl_->name_; } -const std::list& -SubTask::getBeginning(){ - return beginnings_; -} - -const std::list& -SubTask::getEnd(){ - return endings_; -} - -std::list& -SubTask::getTrajectories(){ - return trajectories_; -} - -void -SubTask::setPlanningScene(planning_scene::PlanningSceneConstPtr scene){ +void SubTask::setPlanningScene(planning_scene::PlanningSceneConstPtr scene){ scene_= scene; } -void -SubTask::setPlanningPipeline(planning_pipeline::PlanningPipelinePtr planner){ +void SubTask::setPlanningPipeline(planning_pipeline::PlanningPipelinePtr 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_; - ++it_beginnings_; - - return state; +void SubTaskPrivate::addPredecessor(SubTaskPtr prev_task){ + predecessor_= SubTaskWeakPtr(prev_task); } -InterfaceState& -SubTask::fetchStateEnding(){ - if(it_endings_ == endings_.end()) - throw std::runtime_error("no new state for ending available"); - - InterfaceState& state= *it_endings_; - ++it_endings_; - - return state; +void SubTaskPrivate::addSuccessor(SubTaskPtr next_task){ + successor_= next_task; } -std::pair -SubTask::fetchStatePair(){ - // TODO: implement this properly - return std::pair( - *it_pairs_.first, - *(it_pairs_.second++)); -} - -SubTrajectory& -SubTask::addTrajectory(robot_trajectory::RobotTrajectoryPtr trajectory){ +SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){ trajectories_.emplace_back(trajectory); return trajectories_.back(); } -void -SubTask::sendForward(SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){ +void SubTaskPrivate::sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps){ if( successor_ ){ std::cout << "sending state forward to successor" << std::endl; - traj.end= successor_->newBeginning(ps, &traj); + traj.end= successor_->impl_->newBeginning(ps, &traj); } } -void -SubTask::sendBackward(SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){ +void SubTaskPrivate::sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps){ if( !predecessor_.expired() ){ 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 -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){ +InterfaceState* SubTaskPrivate::newBeginning(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; - + me_->newBeginning(--beginnings_.end()); return &beginnings_.back(); } -InterfaceState* -SubTask::newEnd(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_beginning){ +InterfaceState* SubTaskPrivate::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; - + me_->newEnd(--endings_.end()); return &endings_.back(); } -bool -SubTask::hasBeginning(){ - return it_beginnings_ != beginnings_.end(); + +PropagatingAnyWay::PropagatingAnyWay(const std::string &name) + : 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 -SubTask::hasEnding(){ - return it_endings_ != endings_.end(); +InterfaceState& PropagatingAnyWay::fetchStateBeginning(){ + I(PropagatingAnyWay); + 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 -SubTask::hasStatePair(){ +void PropagatingAnyWay::sendForward(const robot_trajectory::RobotTrajectoryPtr& t, + 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 - 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 Connecting::fetchStatePair(){ + I(Connecting); + // TODO: implement this properly + return std::pair + (*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); +} + + } } diff --git a/src/subtask_p.h b/src/subtask_p.h new file mode 100644 index 00000000..9782a204 --- /dev/null +++ b/src/subtask_p.h @@ -0,0 +1,75 @@ +#pragma once + +#include + +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& 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 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 it_pairs_; +}; + +} } diff --git a/src/subtasks/cartesian_position_motion.cpp b/src/subtasks/cartesian_position_motion.cpp index 7aebb679..ba55195d 100644 --- a/src/subtasks/cartesian_position_motion.cpp +++ b/src/subtasks/cartesian_position_motion.cpp @@ -15,7 +15,7 @@ namespace moveit { namespace task_constructor { namespace subtasks { CartesianPositionMotion::CartesianPositionMotion(std::string name) -: SubTask(name), +: PropagatingAnyWay(name), step_size_(0.005) { ros::NodeHandle nh; @@ -163,12 +163,7 @@ bool CartesianPositionMotion::_computeFromBeginning(){ moveit::core::RobotStatePtr result_state= trajectory_steps.back(); robot_state= *result_state; - moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj); - - trajectory.hasBeginning(beginning); - - sendForward(trajectory, result_scene); - + sendForward(traj, beginning, result_scene); _publishTrajectory(*traj, *result_state); } @@ -238,12 +233,7 @@ bool CartesianPositionMotion::_computeFromEnding(){ moveit::core::RobotStatePtr result_state= trajectory_steps.back(); robot_state= *result_state; - moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj); - - trajectory.hasEnding(ending); - - sendBackward(trajectory, result_scene); - + sendBackward(traj, result_scene, ending); _publishTrajectory(*traj, *result_state); return true; diff --git a/src/subtasks/current_state.cpp b/src/subtasks/current_state.cpp index d8e8d649..174ff0f7 100644 --- a/src/subtasks/current_state.cpp +++ b/src/subtasks/current_state.cpp @@ -3,7 +3,7 @@ namespace moveit { namespace task_constructor { namespace subtasks { CurrentState::CurrentState(std::string name) -: SubTask(name) +: Generator(name) { ran_= false; } @@ -14,14 +14,8 @@ bool CurrentState::canCompute(){ bool CurrentState::compute(){ ran_= true; - assert( scene_ ); - - // empty trajectory ref -> this node only produces states - robot_trajectory::RobotTrajectoryPtr traj; - moveit::task_constructor::SubTrajectory& trajectory= addTrajectory(traj); - - sendBothWays(trajectory, scene_); + spawn(scene_); return true; } diff --git a/src/subtasks/generate_grasp_pose.cpp b/src/subtasks/generate_grasp_pose.cpp index 2b609d2b..85003af7 100644 --- a/src/subtasks/generate_grasp_pose.cpp +++ b/src/subtasks/generate_grasp_pose.cpp @@ -18,7 +18,7 @@ namespace moveit { namespace task_constructor { namespace subtasks { GenerateGraspPose::GenerateGraspPose(std::string name) -: SubTask(name), +: Generator(name), timeout_(0.1), angle_delta_(0.1), 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_; - // empty trajectory -> this subtask only produces states - const robot_trajectory::RobotTrajectoryPtr traj; - if(!gripper_grasp_pose_.empty()){ grasp_state.setToDefaultValues(jmg_eef, gripper_grasp_pose_); } @@ -173,8 +170,7 @@ bool GenerateGraspPose::compute(){ previous_solutions_.emplace_back(); grasp_state.copyJointGroupPositions(jmg_active, previous_solutions_.back()); - moveit::task_constructor::SubTrajectory &trajectory = addTrajectory(traj); - sendBothWays(trajectory, grasp_scene); + spawn(grasp_scene); return true; } } diff --git a/src/subtasks/gripper.cpp b/src/subtasks/gripper.cpp index 90df0842..5c272d50 100644 --- a/src/subtasks/gripper.cpp +++ b/src/subtasks/gripper.cpp @@ -12,7 +12,7 @@ namespace moveit { namespace task_constructor { namespace subtasks { Gripper::Gripper(std::string name) - : SubTask(name) + : PropagatingForward(name) {} void Gripper::setEndEffector(std::string eef){ @@ -24,6 +24,7 @@ void Gripper::setAttachLink(std::string link){ } void Gripper::setFrom(std::string named_target){ + // TODO: this direction isn't handled yet 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()); @@ -83,9 +84,7 @@ bool Gripper::compute(){ } // finish subtask - SubTrajectory& trajectory= addTrajectory(res.trajectory_); - trajectory.hasBeginning(start); - sendForward(trajectory, scene); + sendForward(res.trajectory_, start, scene); return true; } diff --git a/src/subtasks/move.cpp b/src/subtasks/move.cpp index cb5110ea..f5f2c60e 100644 --- a/src/subtasks/move.cpp +++ b/src/subtasks/move.cpp @@ -12,7 +12,7 @@ namespace moveit { namespace task_constructor { namespace subtasks { Move::Move(std::string name) - : SubTask(name), + : Connecting(name), timeout_(5.0) {} @@ -66,9 +66,7 @@ bool Move::compute(){ return false; // finish subtask - moveit::task_constructor::SubTrajectory& trajectory= addTrajectory(res.trajectory_); - trajectory.hasBeginning(state_pair.first); - trajectory.hasEnding(state_pair.second); + connect(res.trajectory_, state_pair); return true; } diff --git a/src/task.cpp b/src/task.cpp index b9437f6c..75d2051e 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -1,5 +1,5 @@ +#include "subtask_p.h" #include -#include #include #include @@ -83,8 +83,8 @@ void Task::add( SubTaskPtr subtask ){ subtask->setPlanningPipeline( planner_ ); if( !subtasks_.empty() ){ - subtask->addPredecessor( subtasks_.back() ); - subtasks_.back()->addSuccessor( subtask ); + subtask->impl_->addPredecessor( subtasks_.back() ); + subtasks_.back()->impl_->addSuccessor( subtask ); } subtasks_.push_back( subtask ); @@ -97,9 +97,9 @@ const robot_state::RobotState& Task::getCurrentRobotState() const { void Task::printState(){ for( auto& st : subtasks_ ){ std::cout - << st->getBeginning().size() << " -> " - << st->getTrajectories().size() - << " <- " << st->getEnd().size() + << st->impl_->getBeginning().size() << " -> " + << st->impl_->getTrajectories().size() + << " <- " << st->impl_->getEnd().size() << " / " << st->getName() << std::endl; } @@ -120,7 +120,7 @@ bool traverseFullTrajectories( ret= cb(trace); } 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) ){ ret= false; 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(); std::vector trace; trace.reserve(nr_of_trajectories); - for(SubTrajectory& subtraj : subtasks_.front()->getTrajectories()) - if( !traverseFullTrajectories(subtraj, subtasks_.size(), processor, trace) ) + for(SubTrajectory& st : subtasks_.front()->impl_->trajectories_) + if( !traverseFullTrajectories(st, subtasks_.size(), processor, trace) ) return false; return true; } +bool Task::processSolutions(const Task::SolutionCallback& processor) const { + return const_cast(this)->processSolutions(processor); +} + } }