From bb06eda33ca99aef187cabc532ce9bc31c651687 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 2 Oct 2017 20:05:25 +0200 Subject: [PATCH] containers - allow hierarchical organization of stages (serially for now) - validate correctness of tree (at composition time, i.e. runtime) - derive Task from SerialContainer - fix pimpl_func(), PRIVATE_CLASS declaration in "public" section to allow access in tests --- CMakeLists.txt | 23 +- include/moveit_task_constructor/container.h | 44 ++++ include/moveit_task_constructor/storage.h | 86 ++++++-- include/moveit_task_constructor/subtask.h | 101 +++++---- .../subtasks/cartesian_position_motion.h | 12 +- .../subtasks/current_state.h | 5 +- .../subtasks/generate_grasp_pose.h | 5 +- .../subtasks/gripper.h | 5 +- .../moveit_task_constructor/subtasks/move.h | 11 +- include/moveit_task_constructor/task.h | 43 +--- include/moveit_task_constructor/utils.h | 58 +++++ package.xml | 2 +- src/container.cpp | 157 +++++++++++++ src/container_p.h | 57 +++++ src/debug.cpp | 1 + src/demo/CMakeLists.txt | 3 + src/demo/plan_pick_trixi.cpp | 26 +-- src/demo/plan_pick_ur5.cpp | 26 +-- src/demo/test.cpp | 33 +++ src/storage.cpp | 22 ++ src/subtask.cpp | 208 ++++++++++-------- src/subtask_p.h | 128 ++++++++--- src/subtasks/CMakeLists.txt | 14 ++ src/subtasks/cartesian_position_motion.cpp | 21 +- src/subtasks/current_state.cpp | 5 +- src/subtasks/generate_grasp_pose.cpp | 10 +- src/subtasks/gripper.cpp | 11 +- src/subtasks/move.cpp | 9 +- src/task.cpp | 153 ++++++------- src/test/CMakeLists.txt | 13 ++ src/test/container.cpp | 135 ++++++++++++ src/test/test_plan_cartesian_forward.cpp | 10 +- src/test/test_plan_current_state.cpp | 2 +- src/test/test_plan_generate_grasp_pose.cpp | 4 +- src/test/test_plan_gripper.cpp | 12 +- 35 files changed, 1066 insertions(+), 389 deletions(-) create mode 100644 include/moveit_task_constructor/container.h create mode 100644 include/moveit_task_constructor/utils.h create mode 100644 src/container.cpp create mode 100644 src/container_p.h create mode 100644 src/demo/test.cpp create mode 100644 src/storage.cpp create mode 100644 src/subtasks/CMakeLists.txt create mode 100644 src/test/container.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6426cd77..9b86d014 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,35 +20,24 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -add_compile_options(-std=c++14) - -add_library(${PROJECT_NAME}_subtasks - src/subtasks/move.cpp - src/subtasks/current_state.cpp - src/subtasks/gripper.cpp - src/subtasks/generate_grasp_pose.cpp - src/subtasks/cartesian_position_motion.cpp - - include/${PROJECT_NAME}/subtasks/move.h - include/${PROJECT_NAME}/subtasks/current_state.h - include/${PROJECT_NAME}/subtasks/gripper.h - include/${PROJECT_NAME}/subtasks/generate_grasp_pose.h - include/${PROJECT_NAME}/subtasks/cartesian_position_motion.h -) -target_link_libraries(${PROJECT_NAME}_subtasks ${catkin_LIBRARIES}) +set(CMAKE_CXX_STANDARD 14) add_library(${PROJECT_NAME} + src/storage.cpp src/subtask.cpp + src/container.cpp src/task.cpp src/debug.cpp src/subtask_p.h + include/${PROJECT_NAME}/utils.h include/${PROJECT_NAME}/storage.h include/${PROJECT_NAME}/subtask.h + include/${PROJECT_NAME}/container.h include/${PROJECT_NAME}/task.h include/${PROJECT_NAME}/debug.h ) +add_subdirectory(src/subtasks) add_subdirectory(src/demo) - add_subdirectory(src/test) diff --git a/include/moveit_task_constructor/container.h b/include/moveit_task_constructor/container.h new file mode 100644 index 00000000..ffdea3fd --- /dev/null +++ b/include/moveit_task_constructor/container.h @@ -0,0 +1,44 @@ +#pragma once + +#include "subtask.h" + +namespace moveit { namespace task_constructor { + +class ContainerBasePrivate; +class ContainerBase : public SubTask +{ +public: + PRIVATE_CLASS(ContainerBase) + typedef std::unique_ptr value_type; + + typedef std::function StageCallback; + typedef std::function&)> SolutionCallback; + + virtual bool canInsert(const value_type& stage, int before = -1) const = 0; + virtual bool insert(value_type&& stage, int before = -1) = 0; + virtual void clear(); + + bool traverseStages(const StageCallback &processor) const; + +protected: + ContainerBase(ContainerBasePrivate* impl); +}; + +class SerialContainerPrivate; +class SerialContainer : public ContainerBase +{ +public: + PRIVATE_CLASS(SerialContainer) + SerialContainer(const std::string& name); + + bool canInsert(const value_type& stage, int before = -1) const override; + bool insert(value_type&& stage, int before = -1) override; + + bool canCompute() const override; + bool compute() override; + +protected: + SerialContainer(SerialContainerPrivate* impl); +}; + +} } diff --git a/include/moveit_task_constructor/storage.h b/include/moveit_task_constructor/storage.h index b7e14266..256b252f 100644 --- a/include/moveit_task_constructor/storage.h +++ b/include/moveit_task_constructor/storage.h @@ -4,8 +4,10 @@ #include -#include +#include +#include #include +#include namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene) @@ -19,34 +21,79 @@ namespace moveit { namespace task_constructor { MOVEIT_CLASS_FORWARD(SubTrajectory) MOVEIT_CLASS_FORWARD(InterfaceState) +MOVEIT_CLASS_FORWARD(Interface) +MOVEIT_CLASS_FORWARD(SubTask) + +/** InterfaceState describes a potential start or goal state for a planning stage. + * + * A start or goal state for planning is essentially defined by the state of a planning scene. + */ class InterfaceState { friend class SubTrajectory; public: - typedef std::vector SubTrajectoryList; + typedef std::deque SubTrajectoryList; - InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next) + InterfaceState(const planning_scene::PlanningSceneConstPtr& ps) : state(ps) - { - if( previous ) - previous_trajectories_.push_back(previous); - if( next ) - next_trajectories_.push_back(next); - } + {} - inline const SubTrajectoryList& previousTrajectories() const { return previous_trajectories_; } - inline const SubTrajectoryList& nextTrajectories() const { return next_trajectories_; } + inline const SubTrajectoryList& incomingTrajectories() const { return incoming_trajectories_; } + inline const SubTrajectoryList& outgoingTrajectories() const { return outgoing_trajectories_; } public: planning_scene::PlanningSceneConstPtr state; - double cost; // minimal costs private: - mutable SubTrajectoryList previous_trajectories_; - mutable SubTrajectoryList next_trajectories_; + mutable SubTrajectoryList incoming_trajectories_; + mutable SubTrajectoryList outgoing_trajectories_; }; + +/** Interface provides a list of InterfaceStates available as input for a stage. + * + * This is essentially an adaptor to a container class, to allow for notification + * of the interface's owner when new states become available + */ +class Interface : protected std::list { +public: + typedef std::list container_type; + typedef std::function NotifyFunction; + Interface(const NotifyFunction ¬ify); + // add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state + // and finally run the notify callback + container_type::iterator add(const planning_scene::PlanningSceneConstPtr& ps, SubTrajectory* incoming, SubTrajectory* outgoing); + + using container_type::value_type; + using container_type::reference; + using container_type::const_reference; + + using container_type::iterator; + using container_type::const_iterator; + using container_type::reverse_iterator; + using container_type::const_reverse_iterator; + + using container_type::empty; + using container_type::size; + using container_type::clear; + using container_type::front; + using container_type::back; + + using container_type::begin; + using container_type::cbegin; + using container_type::end; + using container_type::cend; + using container_type::rbegin; + using container_type::crbegin; + using container_type::rend; + using container_type::crend; + +private: + const NotifyFunction notify_; +}; + + struct SubTrajectory { SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj) : trajectory(traj), @@ -55,19 +102,20 @@ struct SubTrajectory { cost(0) {} - void setBeginning(const InterfaceState& state){ + inline void setStartState(const InterfaceState& state){ assert(begin == NULL); begin= &state; - state.next_trajectories_.push_back(this); + state.outgoing_trajectories_.push_back(this); } - void setEnding(const InterfaceState& state){ + inline void setEndState(const InterfaceState& state){ assert(end == NULL); end= &state; - state.previous_trajectories_.push_back(this); + state.incoming_trajectories_.push_back(this); } - robot_trajectory::RobotTrajectoryPtr trajectory; + // TODO: trajectories could have a name, e.g. a generator could name its solutions + const robot_trajectory::RobotTrajectoryPtr trajectory; 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 8e9aaa8f..bd6d018a 100644 --- a/include/moveit_task_constructor/subtask.h +++ b/include/moveit_task_constructor/subtask.h @@ -1,15 +1,17 @@ // copyright Michael 'v4hn' Goerner @ 2017 +// copyright Robert Haschke @ 2017 #pragma once -#include - +#include "utils.h" #include -#include - #include #include -#include + +#define PRIVATE_CLASS(Class) \ + friend class Class##Private; \ + Class##Private* pimpl_func(); \ + const Class##Private* pimpl_func() const; namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene) @@ -19,63 +21,84 @@ namespace planning_pipeline { MOVEIT_CLASS_FORWARD(PlanningPipeline) } +namespace robot_trajectory { +MOVEIT_CLASS_FORWARD(RobotTrajectory) +} + namespace moveit { namespace task_constructor { -typedef std::list InterfaceStateList; -typedef std::pair InterfaceStatePair; - +MOVEIT_CLASS_FORWARD(Interface) MOVEIT_CLASS_FORWARD(SubTask) -typedef std::weak_ptr SubTaskWeakPtr; +MOVEIT_CLASS_FORWARD(SubTrajectory) +class InterfaceState; +typedef std::pair InterfaceStatePair; -class Task; + +class ContainerBasePrivate; class SubTaskPrivate; class SubTask { - friend class SubTaskPrivate; // allow access to impl_ - friend class Task; // TODO: remove when we have SubTaskContainers - -protected: - SubTaskPrivate* const impl_; + friend std::ostream& operator<<(std::ostream &os, const SubTask& stage); + friend class SubTaskPrivate; + friend class ContainerBasePrivate; public: - enum PropagationType { FORWARD, BACKWARD, ANYWAY, GENERATOR, CONNECTING }; + inline const SubTaskPrivate* pimpl_func() const { return pimpl_; } + inline SubTaskPrivate* pimpl_func() { return pimpl_; } + + enum InterfaceFlag { + READS_INPUT = 0x01, + READS_OUTPUT = 0x02, + READS_MASK = READS_INPUT | READS_OUTPUT, + WRITES_NEXT_INPUT = 0x04, + WRITES_PREV_OUTPUT = 0x08, + WRITES_UNKNOWN = 0x10, + }; + typedef Flags InterfaceFlags; + InterfaceFlags interfaceFlags() const; ~SubTask(); const std::string& getName() const; - virtual bool canCompute() = 0; + + // TODO: results from { TIMEOUT, EXHAUSTED, FINISHED, WAITING } + virtual bool canCompute() const = 0; virtual bool compute() = 0; - void setPlanningScene(planning_scene::PlanningSceneConstPtr); - void setPlanningPipeline(planning_pipeline::PlanningPipelinePtr); + planning_scene::PlanningSceneConstPtr scene() const; + planning_pipeline::PlanningPipelinePtr planner() const; + void setPlanningScene(const planning_scene::PlanningSceneConstPtr&); + void setPlanningPipeline(const planning_pipeline::PlanningPipelinePtr&); protected: /// can only instantiated by derived classes SubTask(SubTaskPrivate *impl); /// methods called when a new InterfaceState was spawned - virtual inline void newBeginning(const InterfaceStateList::iterator& it) {} - virtual inline void newEnd(const InterfaceStateList::iterator& it) {} + virtual void newInputState(const std::list::iterator&) {} + virtual void newOutputState(const std::list::iterator&) {} - planning_scene::PlanningSceneConstPtr scene_; - planning_pipeline::PlanningPipelinePtr planner_; +protected: + // TODO: use unique_ptr and get rid of destructor + SubTaskPrivate* const pimpl_; // constness guarantees one initial write }; +std::ostream& operator<<(std::ostream &os, const SubTask& stage); class PropagatingAnyWayPrivate; class PropagatingAnyWay : public SubTask { public: + PRIVATE_CLASS(PropagatingAnyWay) PropagatingAnyWay(const std::string& name); - static inline constexpr PropagationType type() { return ANYWAY; } bool hasBeginning() const; - InterfaceState& fetchStateBeginning(); + 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(); + const InterfaceState &fetchStateEnding(); void sendBackward(const robot_trajectory::RobotTrajectoryPtr& trajectory, const planning_scene::PlanningSceneConstPtr& from, const InterfaceState& to, @@ -83,65 +106,65 @@ public: protected: // constructor for use in derived classes - inline PropagatingAnyWay(PropagatingAnyWayPrivate* impl); + PropagatingAnyWay(PropagatingAnyWayPrivate* impl); // get informed when new beginnings and endings become available - void newBeginning(const InterfaceStateList::iterator& it); - void newEnd(const InterfaceStateList::iterator& it); + void newInputState(const std::list::iterator& it); + void newOutputState(const std::list::iterator& it); }; +class PropagatingForwardPrivate; class PropagatingForward : public PropagatingAnyWay { public: + PRIVATE_CLASS(PropagatingForward) 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 PropagatingBackwardPrivate; class PropagatingBackward : public PropagatingAnyWay { public: + PRIVATE_CLASS(PropagatingBackward) 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 GeneratorPrivate; class Generator : public SubTask { public: + PRIVATE_CLASS(Generator) Generator(const std::string& name); - static inline constexpr PropagationType type() { return GENERATOR; } void spawn(const planning_scene::PlanningSceneConstPtr &ps, double cost = 0); }; +class ConnectingPrivate; class Connecting : public SubTask { public: + PRIVATE_CLASS(Connecting) 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); + virtual void newInputState(const std::list::iterator& it); + virtual void newOutputState(const std::list::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 f2bc0b8d..2fa978b5 100644 --- a/include/moveit_task_constructor/subtasks/cartesian_position_motion.h +++ b/include/moveit_task_constructor/subtasks/cartesian_position_motion.h @@ -9,9 +9,10 @@ #include #include -namespace moveit { namespace planning_interface { -MOVEIT_CLASS_FORWARD(MoveGroupInterface); -} } +namespace moveit { +namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface) } +namespace core { MOVEIT_CLASS_FORWARD(RobotState) } +} namespace moveit { namespace task_constructor { namespace subtasks { @@ -19,9 +20,8 @@ class CartesianPositionMotion : public PropagatingAnyWay { public: CartesianPositionMotion(std::string name); - virtual bool canCompute(); - - virtual bool compute(); + virtual bool canCompute() const override; + virtual bool compute() override; void setGroup(std::string group); void setLink(std::string link); diff --git a/include/moveit_task_constructor/subtasks/current_state.h b/include/moveit_task_constructor/subtasks/current_state.h index 3680c865..a055154e 100644 --- a/include/moveit_task_constructor/subtasks/current_state.h +++ b/include/moveit_task_constructor/subtasks/current_state.h @@ -10,9 +10,8 @@ class CurrentState : public Generator { public: CurrentState(std::string name); - virtual bool canCompute(); - - virtual bool compute(); + virtual bool canCompute() const override; + virtual bool compute() override; protected: bool ran_; diff --git a/include/moveit_task_constructor/subtasks/generate_grasp_pose.h b/include/moveit_task_constructor/subtasks/generate_grasp_pose.h index 8fcce629..c1ac7301 100644 --- a/include/moveit_task_constructor/subtasks/generate_grasp_pose.h +++ b/include/moveit_task_constructor/subtasks/generate_grasp_pose.h @@ -18,9 +18,8 @@ class GenerateGraspPose : public Generator { public: GenerateGraspPose(std::string name); - virtual bool canCompute(); - - virtual bool compute(); + virtual bool canCompute() const override; + virtual bool compute() override; void setEndEffector(std::string eef); diff --git a/include/moveit_task_constructor/subtasks/gripper.h b/include/moveit_task_constructor/subtasks/gripper.h index d04bdc00..0bb93bb4 100644 --- a/include/moveit_task_constructor/subtasks/gripper.h +++ b/include/moveit_task_constructor/subtasks/gripper.h @@ -14,9 +14,8 @@ class Gripper : public PropagatingForward { public: Gripper(std::string name); - virtual bool canCompute(); - - virtual bool compute(); + virtual bool canCompute() const override; + virtual bool compute() override; void setEndEffector(std::string eef); diff --git a/include/moveit_task_constructor/subtasks/move.h b/include/moveit_task_constructor/subtasks/move.h index ce3120bc..0b0678cf 100644 --- a/include/moveit_task_constructor/subtasks/move.h +++ b/include/moveit_task_constructor/subtasks/move.h @@ -4,9 +4,9 @@ #include -namespace moveit { namespace planning_interface { -MOVEIT_CLASS_FORWARD(MoveGroupInterface) -} } +namespace moveit { +namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface)} +} namespace moveit { namespace task_constructor { namespace subtasks { @@ -14,9 +14,8 @@ class Move : public Connecting { public: Move(std::string name); - virtual bool canCompute(); - - virtual bool compute(); + virtual bool canCompute() const override; + virtual bool compute() override; void setGroup(std::string group); void setLink(std::string link); diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 9f9cdb11..e39be6a6 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -2,7 +2,7 @@ #pragma once -#include +#include "container.h" #include @@ -10,18 +10,6 @@ #include -namespace planning_scene { - MOVEIT_CLASS_FORWARD(PlanningScene) -} - -namespace robot_model_loader { - MOVEIT_CLASS_FORWARD(RobotModelLoader) -} - -namespace planning_pipeline { - MOVEIT_CLASS_FORWARD(PlanningPipeline) -} - namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotState) }} @@ -29,35 +17,26 @@ namespace moveit { namespace core { namespace moveit { namespace task_constructor { MOVEIT_CLASS_FORWARD(SubTask) +MOVEIT_CLASS_FORWARD(ContainerBase) MOVEIT_CLASS_FORWARD(Task) -class Task { +class TaskPrivate; +class Task : protected SerialContainer { + PRIVATE_CLASS(Task) public: typedef std::function&)> SolutionCallback; - Task(); - ~Task(); + Task(const std::string &name = std::string()); - void add( SubTaskPtr ); + void add(std::unique_ptr &&stage); + using SerialContainer::clear; bool plan(); - - bool processSolutions(const SolutionCallback &processor); - bool processSolutions(const SolutionCallback &processor) const; - - const moveit::core::RobotState &getCurrentRobotState() const; - void printState(); - void clear(); - - protected: - std::vector subtasks_; - - planning_scene::PlanningScenePtr scene_; - robot_model_loader::RobotModelLoaderPtr rml_; - - planning_pipeline::PlanningPipelinePtr planner_; + const moveit::core::RobotState &getCurrentRobotState() const; + bool processSolutions(const SolutionCallback &processor); + bool processSolutions(const SolutionCallback &processor) const; }; } } diff --git a/include/moveit_task_constructor/utils.h b/include/moveit_task_constructor/utils.h new file mode 100644 index 00000000..d59dc96d --- /dev/null +++ b/include/moveit_task_constructor/utils.h @@ -0,0 +1,58 @@ +#pragma once + +#include +#include + +/** template class to compose flags from enums in a type-safe fashion */ +template +class Flags +{ + static_assert((sizeof(Enum) <= sizeof(int)), + "Flags uses an int as storage, this enum will overflow!"); +public: + typedef typename std::conditional::value, unsigned int, signed int>::type Int; + typedef Enum enum_type; + // compiler-generated copy/move ctor/assignment operators are fine! + + // zero flags + constexpr inline Flags() noexcept : i(Int(0)) {} + // initialization from single enum + constexpr inline Flags(Enum f) noexcept : i(Int(f)) {} + // initialization from initializer_list + constexpr inline Flags(std::initializer_list flags) noexcept + : i(initializer_list_helper(flags.begin(), flags.end())) {} + + const inline Flags &operator&=(int mask) noexcept { i &= mask; return *this; } + const inline Flags &operator&=(unsigned int mask) noexcept { i &= mask; return *this; } + const inline Flags &operator&=(Enum mask) noexcept { i &= Int(mask); return *this; } + const inline Flags &operator|=(Flags f) noexcept { i |= f.i; return *this; } + const inline Flags &operator|=(Enum f) noexcept { i |= Int(f); return *this; } + const inline Flags &operator^=(Flags f) noexcept { i ^= f.i; return *this; } + const inline Flags &operator^=(Enum f) noexcept { i ^= Int(f); return *this; } + + constexpr inline operator Int() const noexcept { return i; } + + constexpr inline Flags operator|(Flags f) const noexcept { return Flags(i | f.i); } + constexpr inline Flags operator|(Enum f) const noexcept { return Flags(i | Int(f)); } + constexpr inline Flags operator^(Flags f) const noexcept { return Flags(i ^ f.i); } + constexpr inline Flags operator^(Enum f) const noexcept { return Flags(i ^ Int(f)); } + constexpr inline Flags operator&(int mask) const noexcept { return Flags(i & mask); } + constexpr inline Flags operator&(unsigned int mask) const noexcept { return Flags(i & mask); } + constexpr inline Flags operator&(Enum f) const noexcept { return Flags(i & Int(f)); } + constexpr inline Flags operator~() const noexcept { return Flags(~i); } + + constexpr inline bool operator!() const noexcept { return !i; } + + constexpr inline bool testFlag(Enum f) const noexcept { return (i & Int(f)) == Int(f) && (Int(f) != 0 || i == Int(f) ); } + +private: + constexpr inline Flags(Int i) : i(i) {} + constexpr static inline Int initializer_list_helper(typename std::initializer_list::const_iterator it, + typename std::initializer_list::const_iterator end) noexcept { + return (it == end ? Int(0) : (Int(*it) | initializer_list_helper(it + 1, end))); + } + + Int i; +}; + +#define DECLARE_FLAGS(Flags, Enum) typedef QFlags Flags; diff --git a/package.xml b/package.xml index b295d1ad..7cde1b48 100644 --- a/package.xml +++ b/package.xml @@ -9,5 +9,5 @@ moveit_core moveit_core - + rosunit diff --git a/src/container.cpp b/src/container.cpp new file mode 100644 index 00000000..50be978a --- /dev/null +++ b/src/container.cpp @@ -0,0 +1,157 @@ +#include "container_p.h" + +#include +#include + +namespace moveit { namespace task_constructor { + +ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int before) const { + const_iterator position = children_.begin(); + if (before > 0) { + for (auto end = children_.end(); before > 0 && position != end; --before) + ++position; + } else if (++before <= 0) { + array_type::const_reverse_iterator from_end = children_.rbegin(); + for (auto end = children_.rend(); before < 0 && from_end != end; ++before) + ++from_end; + position = from_end.base(); + } + return position; +} + +bool ContainerBasePrivate::canInsert(const SubTask &stage) const { + const SubTaskPrivate* impl = stage.pimpl_func(); + return impl->parent_ == nullptr // re-parenting is not supported + && impl->predeccessor_ == nullptr + && impl->successor_ == nullptr + && impl->trajectories_.empty(); // existing trajectories would become invalid +} + +// insert child into chain, before where +void ContainerBasePrivate::insertSerial(ContainerBasePrivate::value_type &&child, ContainerBasePrivate::const_iterator before) { + SubTaskPrivate* child_impl = child->pimpl_func(); + bool at_end = before == children_.cend(); + bool at_begin = before == children_.cbegin(); + + // child should not be connected yet + assert(child_impl->parent_ == nullptr); + assert(child_impl->predeccessor_ == nullptr); + assert(child_impl->successor_ == nullptr); + + child_impl->parent_ = this; + if (children_.empty()) { + child_impl->successor_ = this; + child_impl->predeccessor_ = this; + } else if (at_end) { + const_iterator prev = before; --prev; + SubTaskPrivate* prev_impl = prev->get()->pimpl_func(); + child_impl->successor_ = this; + child_impl->predeccessor_ = prev_impl; + prev_impl->successor_ = child_impl; + } else if (at_begin) { + SubTaskPrivate* next_impl = before->get()->pimpl_func(); + child_impl->predeccessor_ = this; + child_impl->successor_ = next_impl; + next_impl->predeccessor_ = child_impl; + } else { + const_iterator prev = before; --prev; + SubTaskPrivate* prev_impl = prev->get()->pimpl_func(); + SubTaskPrivate* next_impl = before->get()->pimpl_func(); + child_impl->successor_ = next_impl; + child_impl->predeccessor_ = prev_impl; + next_impl->predeccessor_ = child_impl; + prev_impl->successor_ = child_impl; + } + ContainerBasePrivate::insert(std::move(child), before); +} + +bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &processor, int depth) const { + for (auto &stage : children_) { + if (!processor(*stage, depth)) + continue; + ContainerBase *container = dynamic_cast(stage.get()); + if (container) + container->pimpl_func()->traverseStages(processor, depth+1); + } + return true; +} + +ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate::value_type &&subtask, ContainerBasePrivate::const_iterator pos) { + subtask->setPlanningScene(scene_); + subtask->setPlanningPipeline(planner_); + return children_.insert(pos, std::move(subtask)); +} + + +PRIVATE_CLASS_IMPL(ContainerBase) +ContainerBase::ContainerBase(ContainerBasePrivate *impl) + : SubTask(impl) +{ +} + +void ContainerBase::clear() +{ + IMPL(ContainerBase); + impl->clear(); +} + +bool ContainerBase::traverseStages(const ContainerBase::StageCallback &processor) const +{ + IMPL(const ContainerBase); + return impl->traverseStages(processor, 0); +} + + +bool SerialContainerPrivate::canInsert(const ContainerBasePrivate::value_type &subtask, ContainerBasePrivate::const_iterator before) const { + return ContainerBasePrivate::canInsert(*subtask); +} + + +PRIVATE_CLASS_IMPL(SerialContainer) +SerialContainer::SerialContainer(SerialContainerPrivate *impl) + : ContainerBase(impl) +{} +SerialContainer::SerialContainer(const std::string &name) + : SerialContainer(new SerialContainerPrivate(this, name)) +{} + +bool SerialContainer::canInsert(const value_type& subtask, int before) const +{ + IMPL(const SerialContainer); + return impl->canInsert(subtask, impl->position(before)); +} + +bool SerialContainer::insert(value_type&& subtask, int before) +{ + IMPL(SerialContainer); + + ContainerBasePrivate::const_iterator where = impl->position(before); + if (!impl->canInsert(subtask, where)) + return false; + + impl->insertSerial(std::move(subtask), where); + return true; +} + +bool SerialContainer::canCompute() const +{ + IMPL(const SerialContainer); + return impl->children_.size() > 0; +} + +bool SerialContainer::compute() +{ + IMPL(SerialContainer); + bool computed = false; + for(const auto& stage : impl->children_){ + if(!stage->canCompute()) + continue; + std::cout << "Computing subtask '" << stage->getName() << "':" << std::endl; + bool success = stage->compute(); + computed = true; + std::cout << (success ? "succeeded" : "failed") << std::endl; + } + return computed; +} + +} } diff --git a/src/container_p.h b/src/container_p.h new file mode 100644 index 00000000..3a8345c9 --- /dev/null +++ b/src/container_p.h @@ -0,0 +1,57 @@ +#pragma once + +#include +#include "subtask_p.h" + +namespace moveit { namespace task_constructor { + +class ContainerBasePrivate : public SubTaskPrivate +{ +public: + typedef ContainerBase::value_type value_type; + typedef std::vector array_type; + typedef array_type::iterator iterator; + typedef array_type::const_iterator const_iterator; + + array_type children_; + + const_iterator position(int before = -1) const; + + bool canInsert(const SubTask& stage) const; + + /* SerialContainer doesn't have own input_, output_ interfaces, + * but share the interface pointer with their first resp. last child stage. + * In this fashion, spawned states directly get propagated to the actual stage. + * Consequently, when the container is empty, both interface pointers are invalid. */ + void insertSerial(value_type&& child, const_iterator before); + + void clear() { + children_.clear(); + } + + bool traverseStages(const ContainerBase::StageCallback &processor, int depth) const; + +protected: + ContainerBasePrivate(ContainerBase *me, const std::string &name) + : SubTaskPrivate(me, name) + {} + +private: + iterator insert(value_type &&subtask, const_iterator pos); +}; + + +class SerialContainerPrivate : public ContainerBasePrivate { +public: + SerialContainerPrivate(SerialContainer* me, const std::string &name) + : ContainerBasePrivate(me, name) + { + input_.reset(new Interface(Interface::NotifyFunction())); + output_.reset(new Interface(Interface::NotifyFunction())); + } + + bool canInsert(const value_type& subtask, const_iterator before) const; +}; + + +} } diff --git a/src/debug.cpp b/src/debug.cpp index 648df3b1..7c40706b 100644 --- a/src/debug.cpp +++ b/src/debug.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include diff --git a/src/demo/CMakeLists.txt b/src/demo/CMakeLists.txt index 56e5edfd..142c0dc4 100644 --- a/src/demo/CMakeLists.txt +++ b/src/demo/CMakeLists.txt @@ -1,3 +1,6 @@ +add_executable(foo test.cpp) +target_link_libraries(foo) + add_executable(plan_pick_ur5 plan_pick_ur5.cpp) target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_subtasks ${PROJECT_NAME}) diff --git a/src/demo/plan_pick_trixi.cpp b/src/demo/plan_pick_trixi.cpp index 4100153d..911a9985 100644 --- a/src/demo/plan_pick_trixi.cpp +++ b/src/demo/plan_pick_trixi.cpp @@ -43,26 +43,26 @@ int main(int argc, char** argv){ Task t; - t.add( std::make_shared("current state") ); + t.add( std::make_unique("current state") ); { - auto move= std::make_shared("open gripper"); + auto move= std::make_unique("open gripper"); move->setEndEffector("left_gripper"); move->setTo("open"); - t.add(move); + t.add(std::move(move)); } { - auto move= std::make_shared("move to pre-grasp"); + auto move= std::make_unique("move to pre-grasp"); move->setGroup("left_arm"); move->setLink("l_gripper_tool_frame"); move->setPlannerId("RRTConnectkConfigDefault"); move->setTimeout(8.0); - t.add(move); + t.add(std::move(move)); } { - auto move= std::make_shared("proceed to grasp pose"); + auto move= std::make_unique("proceed to grasp pose"); move->setGroup("left_arm"); move->setLink("l_gripper_tool_frame"); move->setMinMaxDistance(.03, 0.1); @@ -71,11 +71,11 @@ int main(int argc, char** argv){ geometry_msgs::PointStamped target; target.header.frame_id= "object"; move->towards(target); - t.add(move); + t.add(std::move(move)); } { - auto gengrasp= std::make_shared("generate grasp pose"); + auto gengrasp= std::make_unique("generate grasp pose"); gengrasp->setEndEffector("left_gripper"); //gengrasp->setGroup("arm"); gengrasp->setLink("l_gripper_tool_frame"); @@ -83,19 +83,19 @@ int main(int argc, char** argv){ gengrasp->setObject("object"); gengrasp->setGraspOffset(.00); gengrasp->setAngleDelta(.2); - t.add(gengrasp); + t.add(std::move(gengrasp)); } { - auto move= std::make_shared("grasp"); + auto move= std::make_unique("grasp"); move->setEndEffector("left_gripper"); move->setTo("closed"); move->graspObject("object"); - t.add(move); + t.add(std::move(move)); } { - auto move= std::make_shared("lift object"); + auto move= std::make_unique("lift object"); move->setGroup("left_arm"); move->setLink("l_gripper_tool_frame"); move->setMinMaxDistance(0.03, 0.05); @@ -105,7 +105,7 @@ int main(int argc, char** argv){ direction.header.frame_id= "base_link"; direction.vector.z= 1.0; move->along(direction); - t.add(move); + t.add(std::move(move)); } t.plan(); diff --git a/src/demo/plan_pick_ur5.cpp b/src/demo/plan_pick_ur5.cpp index e663e612..064a8b85 100644 --- a/src/demo/plan_pick_ur5.cpp +++ b/src/demo/plan_pick_ur5.cpp @@ -41,25 +41,25 @@ int main(int argc, char** argv){ Task t; - t.add( std::make_shared("current state") ); + t.add(std::make_unique("current state")); { - auto move= std::make_shared("open gripper"); + auto move = std::make_unique("open gripper"); move->setEndEffector("gripper"); move->setTo("open"); - t.add(move); + t.add(std::move(std::move(move))); } { - auto move= std::make_shared("move to pre-grasp"); + auto move = std::make_unique("move to pre-grasp"); move->setGroup("arm"); move->setPlannerId("RRTConnectkConfigDefault"); move->setTimeout(8.0); - t.add(move); + t.add(std::move(std::move(move))); } { - auto move= std::make_shared("proceed to grasp pose"); + auto move = std::make_unique("proceed to grasp pose"); move->setGroup("arm"); move->setLink("s_model_tool0"); move->setMinMaxDistance(.03, 0.1); @@ -68,11 +68,11 @@ int main(int argc, char** argv){ geometry_msgs::PointStamped target; target.header.frame_id= "object"; move->towards(target); - t.add(move); + t.add(std::move(std::move(move))); } { - auto gengrasp= std::make_shared("generate grasp pose"); + auto gengrasp = std::make_unique("generate grasp pose"); gengrasp->setEndEffector("gripper"); //gengrasp->setGroup("arm"); gengrasp->setGripperGraspPose("open"); @@ -80,19 +80,19 @@ int main(int argc, char** argv){ gengrasp->setGraspOffset(.03); gengrasp->setAngleDelta(-.2); gengrasp->setMaxIKSolutions(8); - t.add(gengrasp); + t.add(std::move(std::move(gengrasp))); } { - auto move= std::make_shared("grasp"); + auto move = std::make_unique("grasp"); move->setEndEffector("gripper"); move->setTo("closed"); move->graspObject("object"); - t.add(move); + t.add(std::move(std::move(move))); } { - auto move= std::make_shared("lift object"); + auto move = std::make_unique("lift object"); move->setGroup("arm"); move->setLink("s_model_tool0"); move->setMinMaxDistance(0.03, 0.05); @@ -102,7 +102,7 @@ int main(int argc, char** argv){ direction.header.frame_id= "world"; direction.vector.z= 1.0; move->along(direction); - t.add(move); + t.add(std::move(std::move(move))); } t.plan(); diff --git a/src/demo/test.cpp b/src/demo/test.cpp new file mode 100644 index 00000000..d53a14b7 --- /dev/null +++ b/src/demo/test.cpp @@ -0,0 +1,33 @@ +#include +#include + +void overloaded( std::unique_ptr const &arg ) { + std::cout << " by lvalue " << arg.get() << std::endl; +} +void overloaded( std::unique_ptr && arg ) { + std::unique_ptr x = std::move(arg); + std::cout << " by rvalue, x: " << x.get() << " arg: " << arg.get() << std::endl; +} + +/* "t &&" with "t" being template param is special, and adjusts "t" to be + (for example) "int &" or non-ref "int" so std::forward knows what to do. */ +void forwarding(std::unique_ptr &&arg ) { + std::cout << "- via std::forward: "; + overloaded( std::forward&>( arg ) ); + std::cout << "- via std::move: "; + overloaded( std::move( arg ) ); // conceptually this would invalidate arg + std::cout << "- by simple passing: "; + overloaded( arg ); +} +void forwarding(std::unique_ptr &arg) { + std::cout << "* via extra std::move" << std::endl; + forwarding(std::move(arg)); +} + +int main() { + std::cout << "initial caller passes rvalue:\n"; + forwarding(std::make_unique(5)); + std::cout << "initial caller passes lvalue:\n"; + auto x = std::make_unique(5); + forwarding( x ); +} diff --git a/src/storage.cpp b/src/storage.cpp new file mode 100644 index 00000000..7f873cbc --- /dev/null +++ b/src/storage.cpp @@ -0,0 +1,22 @@ +#include + +namespace moveit { namespace task_constructor { + +Interface::Interface(const Interface::NotifyFunction ¬ify) + : notify_(notify) +{} + +Interface::iterator Interface::add(const planning_scene::PlanningSceneConstPtr& ps, SubTrajectory* incoming, SubTrajectory* outgoing) { + assert(bool(ps)); + assert(bool(incoming) ^ bool(outgoing)); // either incoming or outgoing is set + emplace_back(InterfaceState(ps)); + iterator back = --end(); + // adjust subtrajectories ... + if (incoming) incoming->setEndState(*back); + if (outgoing) outgoing->setStartState(*back); + // ... before calling notify callback + if (notify_) notify_(back); + return back; +} + +} } diff --git a/src/subtask.cpp b/src/subtask.cpp index 188d6372..076ae9a5 100644 --- a/src/subtask.cpp +++ b/src/subtask.cpp @@ -1,39 +1,71 @@ #include "subtask_p.h" - -// get correctly casted private impl pointer -#define I(Class) Class##Private * const impl = reinterpret_cast(impl_) +#include +#include namespace moveit { namespace task_constructor { SubTask::SubTask(SubTaskPrivate *impl) - : impl_(impl) + : pimpl_(impl) { } +SubTask::InterfaceFlags SubTask::interfaceFlags() const +{ + SubTask::InterfaceFlags result = pimpl_->interfaceFlags(); + result &= ~SubTask::InterfaceFlags(SubTask::READS_MASK); + result |= pimpl_->SubTaskPrivate::interfaceFlags(); + return result; +} + SubTask::~SubTask() { - delete impl_; + delete pimpl_; } const std::string& SubTask::getName() const { - return impl_->name_; + return pimpl_->name_; } -void SubTask::setPlanningScene(planning_scene::PlanningSceneConstPtr scene){ - scene_= scene; +std::ostream& operator<<(std::ostream &os, const SubTask& stage) { + os << *stage.pimpl_func(); + return os; +} +std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) { + // inputs + for (const InterfacePtr &i : {stage.prevOutput(), stage.input_}) { + os << std::setw(3); + if (i) os << i->size(); + else os << "-"; + } + // trajectories + os << " -> " << std::setw(3) << stage.trajectories_.size() << " <- "; + // outputs + for (const InterfacePtr &i : {stage.output_, stage.nextInput()}) { + os << std::setw(3); + if (i) os << i->size(); + else os << "-"; + } + // name + os << " / " << stage.name_; + return os; } -void SubTask::setPlanningPipeline(planning_pipeline::PlanningPipelinePtr planner){ - planner_= planner; +planning_scene::PlanningSceneConstPtr SubTask::scene() const +{ + return pimpl_->scene_; } - -void SubTaskPrivate::addPredecessor(SubTaskPtr prev_task){ - predecessor_= SubTaskWeakPtr(prev_task); +planning_pipeline::PlanningPipelinePtr SubTask::planner() const +{ + return pimpl_->planner_; } -void SubTaskPrivate::addSuccessor(SubTaskPtr next_task){ - successor_= next_task; +void SubTask::setPlanningScene(const planning_scene::PlanningSceneConstPtr &scene){ + pimpl_->scene_= scene; +} + +void SubTask::setPlanningPipeline(const planning_pipeline::PlanningPipelinePtr &planner){ + pimpl_->planner_= planner; } SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){ @@ -41,37 +73,27 @@ SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajec return trajectories_.back(); } -void SubTaskPrivate::sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps){ - if( successor_ ){ - std::cout << "sending state forward to successor" << std::endl; - traj.end= successor_->impl_->newBeginning(ps, &traj); - } +SubTask::InterfaceFlags SubTaskPrivate::interfaceFlags() const +{ + // the base class provides the read flags + SubTask::InterfaceFlags f; + if (input_) f |= SubTask::READS_INPUT; + if (output_) f |= SubTask::READS_OUTPUT; + return f; } -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()->impl_->newEnd(ps, &traj); - } +void SubTaskPrivate::sendForward(SubTrajectory& trajectory, const planning_scene::PlanningSceneConstPtr& ps){ + std::cout << "sending state to start" << std::endl; + nextInput()->add(ps, &trajectory, NULL); } -InterfaceState* SubTaskPrivate::newBeginning(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_end){ - assert( bool(ps) ); - beginnings_.push_back( InterfaceState(ps, old_end, NULL) ); - - me_->newBeginning(--beginnings_.end()); - return &beginnings_.back(); -} - -InterfaceState* SubTaskPrivate::newEnd(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_beginning){ - assert( bool(ps) ); - endings_.push_back( InterfaceState(ps, NULL, old_beginning) ); - - me_->newEnd(--endings_.end()); - return &endings_.back(); +void SubTaskPrivate::sendBackward(SubTrajectory& trajectory, const planning_scene::PlanningSceneConstPtr& ps){ + std::cout << "sending state to end" << std::endl; + prevOutput()->add(ps, NULL, &trajectory); } +PRIVATE_CLASS_IMPL(PropagatingAnyWay) PropagatingAnyWay::PropagatingAnyWay(const std::string &name) : SubTask(new PropagatingAnyWayPrivate(this, name)) {} @@ -79,17 +101,17 @@ PropagatingAnyWay::PropagatingAnyWay(const std::string &name) PropagatingAnyWay::PropagatingAnyWay(PropagatingAnyWayPrivate *impl) : SubTask(impl) {} bool PropagatingAnyWay::hasBeginning() const{ - I(PropagatingAnyWay); - return impl->it_beginnings_ != impl->beginnings_.end(); + IMPL(const PropagatingAnyWay); + return impl->next_input_ != impl->input_->end(); } -InterfaceState& PropagatingAnyWay::fetchStateBeginning(){ - I(PropagatingAnyWay); - if(impl->it_beginnings_ == impl->beginnings_.end()) +const InterfaceState& PropagatingAnyWay::fetchStateBeginning(){ + IMPL(PropagatingAnyWay); + if (!hasBeginning()) throw std::runtime_error("no new state for beginning available"); - InterfaceState& state= *impl->it_beginnings_; - ++impl->it_beginnings_; + const InterfaceState& state= *impl->next_input_; + ++impl->next_input_; return state; } @@ -98,121 +120,129 @@ void PropagatingAnyWay::sendForward(const robot_trajectory::RobotTrajectoryPtr& const InterfaceState& from, const planning_scene::PlanningSceneConstPtr& to, double cost){ - SubTrajectory &trajectory = impl_->addTrajectory(t, cost); - trajectory.setBeginning(from); - impl_->sendForward(trajectory, to); + IMPL(PropagatingAnyWay); + SubTrajectory &trajectory = impl->addTrajectory(t, cost); + trajectory.setStartState(from); + impl->sendForward(trajectory, to); } -void PropagatingAnyWay::newBeginning(const InterfaceStateList::iterator &it) +void PropagatingAnyWay::newInputState(const Interface::iterator &it) { - I(PropagatingAnyWay); + IMPL(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_; + if( impl->next_input_ == impl->input_->end() ) + --impl->next_input_; } bool PropagatingAnyWay::hasEnding() const{ - I(PropagatingAnyWay); - return impl->it_endings_ != impl->endings_.end(); + IMPL(const PropagatingAnyWay); + return impl->next_output_ != impl->output_->end(); } -InterfaceState& PropagatingAnyWay::fetchStateEnding(){ - I(PropagatingAnyWay); - if(impl->it_endings_ == impl->endings_.end()) +const InterfaceState& PropagatingAnyWay::fetchStateEnding(){ + IMPL(PropagatingAnyWay); + if(!hasEnding()) throw std::runtime_error("no new state for ending available"); - InterfaceState& state= *impl->it_endings_; - ++impl->it_endings_; + const InterfaceState& state= *impl->next_output_; + ++impl->next_output_; 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); + const planning_scene::PlanningSceneConstPtr& from, + const InterfaceState& to, + double cost){ + IMPL(PropagatingAnyWay); + SubTrajectory& trajectory = impl->addTrajectory(t, cost); + trajectory.setEndState(to); + impl->sendBackward(trajectory, from); } -void PropagatingAnyWay::newEnd(const InterfaceStateList::iterator &it) +void PropagatingAnyWay::newOutputState(const Interface::iterator &it) { - I(PropagatingAnyWay); + IMPL(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_; + if( impl->next_output_ == impl->output_->end() ) + --impl->next_output_; } +PRIVATE_CLASS_IMPL(PropagatingForward) PropagatingForward::PropagatingForward(const std::string& name) : PropagatingAnyWay(new PropagatingForwardPrivate(this, name)) {} +PRIVATE_CLASS_IMPL(PropagatingBackward) PropagatingBackward::PropagatingBackward(const std::string &name) : PropagatingAnyWay(new PropagatingBackwardPrivate(this, name)) {} +PRIVATE_CLASS_IMPL(Generator) Generator::Generator(const std::string &name) - : SubTask(new SubTaskPrivate(this, name)) + : SubTask(new GeneratorPrivate(this, name)) {} void Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost) { + IMPL(Generator); // empty trajectory ref -> this node only produces states robot_trajectory::RobotTrajectoryPtr dummy; - moveit::task_constructor::SubTrajectory& trajectory = impl_->addTrajectory(dummy, cost); + SubTrajectory& trajectory = impl->addTrajectory(dummy, cost); std::cout << "spawning state" << std::endl; - impl_->sendBackward(trajectory, ps); - impl_->sendForward(trajectory, ps); + impl->sendBackward(trajectory, ps); + impl->sendForward(trajectory, ps); } +PRIVATE_CLASS_IMPL(Connecting) Connecting::Connecting(const std::string &name) : SubTask(new ConnectingPrivate(this, name)) { } -void Connecting::newBeginning(const InterfaceStateList::iterator& it) +void Connecting::newInputState(const Interface::iterator& it) { - I(Connecting); + IMPL(Connecting); // TODO: need to handle the pairs iterator - if( impl->it_pairs_.first == impl->beginnings_.end() ) + if( impl->it_pairs_.first == impl->input_->end() ) --impl->it_pairs_.first; } -void Connecting::newEnd(const InterfaceStateList::iterator& it) +void Connecting::newOutputState(const Interface::iterator& it) { - I(Connecting); + IMPL(Connecting); // TODO: need to handle the pairs iterator properly - if( impl->it_pairs_.second == impl->endings_.end() ) + if( impl->it_pairs_.second == impl->output_->end() ) --impl->it_pairs_.second; } bool Connecting::hasStatePair() const{ - I(Connecting); + IMPL(const Connecting); // TODO: implement this properly - return impl->it_pairs_.first != impl->beginnings_.end() && - impl->it_pairs_.second != impl->endings_.end(); + return impl->it_pairs_.first != impl->input_->end() && + impl->it_pairs_.second != impl->output_->end(); } -std::pair Connecting::fetchStatePair(){ - I(Connecting); +InterfaceStatePair Connecting::fetchStatePair(){ + IMPL(Connecting); // TODO: implement this properly - return std::pair + 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); + IMPL(Connecting); + SubTrajectory& trajectory = impl->addTrajectory(t, cost); + trajectory.setStartState(state_pair.first); + trajectory.setEndState(state_pair.second); } diff --git a/src/subtask_p.h b/src/subtask_p.h index 9782a204..0b9614bb 100644 --- a/src/subtask_p.h +++ b/src/subtask_p.h @@ -1,41 +1,51 @@ +// copyright Robert Haschke @ 2017 + #pragma once #include +#include namespace moveit { namespace task_constructor { class SubTaskPrivate { + friend class SubTask; + friend class SubTaskTest; // allow unit tests + // ContainerBase will maintain the double-linked list of interfaces + friend class ContainerBasePrivate; + friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage); + public: inline SubTaskPrivate(SubTask* me, const std::string& name) - : me_(me), name_(name) + : me_(me), name_(name), parent_(nullptr), predeccessor_(nullptr), successor_(nullptr) {} - 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); + virtual SubTask::InterfaceFlags interfaceFlags() const; 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 + SubTask* const me_; // associated/owning SubTask instance const std::string name_; - // avoid shared pointers back: why? to allow proper deallocation? - SubTaskWeakPtr predecessor_; - SubTaskPtr successor_; + planning_scene::PlanningSceneConstPtr scene_; + planning_pipeline::PlanningPipelinePtr planner_; - InterfaceStateList beginnings_; - InterfaceStateList endings_; + InterfacePtr input_; + InterfacePtr output_; std::list trajectories_; + + const InterfacePtr prevOutput() const { return predeccessor_ ? predeccessor_->output_ : InterfacePtr(); } + const InterfacePtr nextInput() const { return successor_ ? successor_->input_ : InterfacePtr(); } + +private: + // items accessed by ContainerBasePrivate only to maintain hierarchy + SubTaskPrivate* parent_; + SubTaskPrivate* predeccessor_; + SubTaskPrivate* successor_; }; +std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage); class PropagatingAnyWayPrivate : public SubTaskPrivate { @@ -44,19 +54,67 @@ class PropagatingAnyWayPrivate : public SubTaskPrivate { public: inline PropagatingAnyWayPrivate(PropagatingAnyWay *me, const std::string &name) : SubTaskPrivate(me, name) - , it_beginnings_ (beginnings_.begin()) - , it_endings_ (endings_.begin()) - {} + { + input_.reset(new Interface([me](const Interface::iterator& it) { me->newInputState(it); })); + output_.reset(new Interface(std::bind(&PropagatingAnyWay::newOutputState, me, std::placeholders::_1))); + next_input_ = input_->begin(); + next_output_ = output_->end(); + } + + SubTask::InterfaceFlags interfaceFlags() const override { + return SubTask::InterfaceFlags({SubTask::READS_INPUT, SubTask::WRITES_NEXT_INPUT, + SubTask::READS_OUTPUT, SubTask::WRITES_PREV_OUTPUT, + SubTask::WRITES_UNKNOWN}); + } protected: - InterfaceStateList::iterator it_beginnings_; - InterfaceStateList::iterator it_endings_; + Interface::const_iterator next_input_; + Interface::const_iterator next_output_; }; -// for now, we use the same implementation for the reduced versions too -typedef PropagatingAnyWayPrivate PropagatingForwardPrivate; -typedef PropagatingAnyWayPrivate PropagatingBackwardPrivate; +class PropagatingForwardPrivate : public PropagatingAnyWayPrivate { +public: + inline PropagatingForwardPrivate(PropagatingForward *me, const std::string &name) + : PropagatingAnyWayPrivate(me, name) + { + // indicate, that we don't accept new states from output interface + output_.reset(); + next_output_ = Interface::iterator(); + } + + SubTask::InterfaceFlags interfaceFlags() const override { + return SubTask::InterfaceFlags({SubTask::READS_INPUT, SubTask::WRITES_NEXT_INPUT}); + } +}; + + +class PropagatingBackwardPrivate : public PropagatingAnyWayPrivate { +public: + inline PropagatingBackwardPrivate(PropagatingBackward *me, const std::string &name) + : PropagatingAnyWayPrivate(me, name) + { + // indicate, that we don't accept new states from input interface + input_.reset(); + next_input_ = Interface::iterator(); + } + + SubTask::InterfaceFlags interfaceFlags() const override { + return SubTask::InterfaceFlags({SubTask::READS_OUTPUT, SubTask::WRITES_PREV_OUTPUT}); + } +}; + + +class GeneratorPrivate : public SubTaskPrivate { +public: + inline GeneratorPrivate(Generator *me, const std::string &name) + : SubTaskPrivate(me, name) + {} + + SubTask::InterfaceFlags interfaceFlags() const override { + return SubTask::InterfaceFlags({SubTask::WRITES_NEXT_INPUT, SubTask::WRITES_PREV_OUTPUT}); + } +}; class ConnectingPrivate : public SubTaskPrivate { @@ -65,11 +123,27 @@ class ConnectingPrivate : public SubTaskPrivate { public: inline ConnectingPrivate(Connecting *me, const std::string &name) : SubTaskPrivate(me, name) - , it_pairs_(beginnings_.begin(), endings_.begin()) - {} + { + input_.reset(new Interface(std::bind(&Connecting::newInputState, me, std::placeholders::_1))); + output_.reset(new Interface(std::bind(&Connecting::newOutputState, me, std::placeholders::_1))); + it_pairs_ = std::make_pair(input_->begin(), output_->begin()); + } + + SubTask::InterfaceFlags interfaceFlags() const override { + return SubTask::InterfaceFlags({SubTask::READS_INPUT, SubTask::READS_OUTPUT}); + } private: - std::pair it_pairs_; + std::pair it_pairs_; }; } } + + +// implement pimpl_func() +#define PRIVATE_CLASS_IMPL(Class) \ + inline Class##Private* Class::pimpl_func() { return static_cast(pimpl_); } \ + inline const Class##Private* Class::pimpl_func() const { return static_cast(pimpl_); } \ + +// get correctly casted private impl pointer +#define IMPL(Class) Class##Private * const impl = pimpl_func() diff --git a/src/subtasks/CMakeLists.txt b/src/subtasks/CMakeLists.txt new file mode 100644 index 00000000..d32f43ac --- /dev/null +++ b/src/subtasks/CMakeLists.txt @@ -0,0 +1,14 @@ +add_library(${PROJECT_NAME}_subtasks + move.cpp + current_state.cpp + gripper.cpp + generate_grasp_pose.cpp + cartesian_position_motion.cpp + + ${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/move.h + ${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/current_state.h + ${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/gripper.h + ${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/generate_grasp_pose.h + ${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/cartesian_position_motion.h +) +target_link_libraries(${PROJECT_NAME}_subtasks ${catkin_LIBRARIES}) diff --git a/src/subtasks/cartesian_position_motion.cpp b/src/subtasks/cartesian_position_motion.cpp index ba55195d..860b102e 100644 --- a/src/subtasks/cartesian_position_motion.cpp +++ b/src/subtasks/cartesian_position_motion.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -58,7 +59,7 @@ void CartesianPositionMotion::setCartesianStepSize(double distance){ step_size_= distance; } -bool CartesianPositionMotion::canCompute(){ +bool CartesianPositionMotion::canCompute() const{ return hasBeginning() || hasEnding(); } @@ -83,7 +84,7 @@ bool CartesianPositionMotion::compute(){ bool CartesianPositionMotion::_computeFromBeginning(){ assert( hasBeginning() ); - moveit::task_constructor::InterfaceState& beginning= fetchStateBeginning(); + const InterfaceState& beginning= fetchStateBeginning(); planning_scene::PlanningScenePtr result_scene = beginning.state->diff(); robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst(); @@ -173,7 +174,7 @@ bool CartesianPositionMotion::_computeFromBeginning(){ bool CartesianPositionMotion::_computeFromEnding(){ assert( hasEnding() ); - moveit::task_constructor::InterfaceState& ending= fetchStateEnding(); + const InterfaceState& ending= fetchStateEnding(); planning_scene::PlanningScenePtr result_scene = ending.state->diff(); robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst(); @@ -244,13 +245,13 @@ bool CartesianPositionMotion::_computeFromEnding(){ void CartesianPositionMotion::_publishTrajectory(const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start){ - moveit_msgs::DisplayTrajectory dt; - robot_state::robotStateToRobotStateMsg(start, dt.trajectory_start); - dt.model_id= scene_->getRobotModel()->getName(); - dt.trajectory.resize(1); - trajectory.getRobotTrajectoryMsg(dt.trajectory[0]); - dt.model_id= start.getRobotModel()->getName(); - pub.publish(dt); + moveit_msgs::DisplayTrajectory dt; + robot_state::robotStateToRobotStateMsg(start, dt.trajectory_start); + dt.model_id= scene()->getRobotModel()->getName(); + dt.trajectory.resize(1); + trajectory.getRobotTrajectoryMsg(dt.trajectory[0]); + dt.model_id= start.getRobotModel()->getName(); + pub.publish(dt); } } } } diff --git a/src/subtasks/current_state.cpp b/src/subtasks/current_state.cpp index 174ff0f7..dc4dd423 100644 --- a/src/subtasks/current_state.cpp +++ b/src/subtasks/current_state.cpp @@ -8,14 +8,13 @@ CurrentState::CurrentState(std::string name) ran_= false; } -bool CurrentState::canCompute(){ +bool CurrentState::canCompute() const{ return !ran_; } bool CurrentState::compute(){ ran_= true; - assert( scene_ ); - spawn(scene_); + spawn(scene()); return true; } diff --git a/src/subtasks/generate_grasp_pose.cpp b/src/subtasks/generate_grasp_pose.cpp index 85003af7..c9475287 100644 --- a/src/subtasks/generate_grasp_pose.cpp +++ b/src/subtasks/generate_grasp_pose.cpp @@ -70,7 +70,7 @@ void GenerateGraspPose::setMaxIKSolutions(uint32_t n){ max_ik_solutions_= n; } -bool GenerateGraspPose::canCompute(){ +bool GenerateGraspPose::canCompute() const{ return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI; } @@ -102,9 +102,9 @@ namespace { } bool GenerateGraspPose::compute(){ - assert( scene_->getRobotModel()->hasEndEffector(eef_) && "The specified end effector is not defined in the srdf" ); + assert(scene()->getRobotModel()->hasEndEffector(eef_) && "The specified end effector is not defined in the srdf"); - planning_scene::PlanningScenePtr grasp_scene = scene_->diff(); + planning_scene::PlanningScenePtr grasp_scene = scene()->diff(); robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst(); const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef_); @@ -122,7 +122,7 @@ bool GenerateGraspPose::compute(){ const moveit::core::GroupStateValidityCallbackFn is_valid= std::bind( &isValid, - scene_, + scene(), ignore_collisions_, &previous_solutions_, std::placeholders::_1, @@ -130,7 +130,7 @@ bool GenerateGraspPose::compute(){ std::placeholders::_3); geometry_msgs::Pose object_pose, grasp_pose; - const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(object_); + const Eigen::Affine3d object_pose_eigen= scene()->getFrameTransform(object_); if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) throw std::runtime_error("requested object does not exist or could not be retrieved"); diff --git a/src/subtasks/gripper.cpp b/src/subtasks/gripper.cpp index 5c272d50..aac4498f 100644 --- a/src/subtasks/gripper.cpp +++ b/src/subtasks/gripper.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -36,16 +37,16 @@ void Gripper::graspObject(std::string grasp_object){ grasp_object_= grasp_object; } -bool Gripper::canCompute(){ +bool Gripper::canCompute() const{ return hasBeginning(); } bool Gripper::compute(){ - assert( scene_->getRobotModel() ); + assert(scene()->getRobotModel()); if(!mgi_){ - assert( scene_->getRobotModel()->hasEndEffector(eef_) && "no end effector with that name defined in srdf" ); - const moveit::core::JointModelGroup* jmg= scene_->getRobotModel()->getEndEffector(eef_); + assert(scene()->getRobotModel()->hasEndEffector(eef_) && "no end effector with that name defined in srdf"); + const moveit::core::JointModelGroup* jmg= scene()->getRobotModel()->getEndEffector(eef_); const std::string group_name= jmg->getName(); mgi_= std::make_shared(group_name); @@ -68,7 +69,7 @@ bool Gripper::compute(){ } ::planning_interface::MotionPlanResponse res; - if(!planner_->generatePlan(scene, req, res)) + if(!planner()->generatePlan(scene, req, res)) return false; // set end state diff --git a/src/subtasks/move.cpp b/src/subtasks/move.cpp index f5f2c60e..84400db1 100644 --- a/src/subtasks/move.cpp +++ b/src/subtasks/move.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -43,14 +44,12 @@ void Move::setTo(std::string named_target){ } */ -bool Move::canCompute(){ +bool Move::canCompute() const{ return hasStatePair(); } bool Move::compute(){ - assert( scene_->getRobotModel() ); - - std::pair state_pair= fetchStatePair(); + InterfaceStatePair state_pair= fetchStatePair(); mgi_->setJointValueTarget(state_pair.second.state->getCurrentState()); if( !planner_id_.empty() ) @@ -62,7 +61,7 @@ bool Move::compute(){ ros::Duration(4.0).sleep(); ::planning_interface::MotionPlanResponse res; - if(!planner_->generatePlan(state_pair.first.state, req, res)) + if(!planner()->generatePlan(state_pair.first.state, req, res)) return false; // finish subtask diff --git a/src/task.cpp b/src/task.cpp index 75d2051e..dd96bcf5 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -1,5 +1,7 @@ -#include "subtask_p.h" +#include "container_p.h" + #include +#include #include #include @@ -11,98 +13,96 @@ namespace moveit { namespace task_constructor { -Task::Task(){ - rml_.reset(new robot_model_loader::RobotModelLoader); - if( !rml_->getModel() ) - throw Exception("Task failed to construct RobotModel"); +class TaskPrivate : public SerialContainerPrivate { + friend class Task; + robot_model_loader::RobotModelLoaderPtr rml_; - ros::NodeHandle h; - - ros::ServiceClient client = h.serviceClient("get_planning_scene"); - client.waitForExistence(); - - moveit_msgs::GetPlanningScene::Request req; - moveit_msgs::GetPlanningScene::Response res; - - req.components.components = - moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS - | moveit_msgs::PlanningSceneComponents::ROBOT_STATE - | moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS - | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES - | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY - | moveit_msgs::PlanningSceneComponents::OCTOMAP - | moveit_msgs::PlanningSceneComponents::TRANSFORMS - | moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX - | moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING - | moveit_msgs::PlanningSceneComponents::OBJECT_COLORS; - - if(!client.call(req, res)){ - throw Exception("Task failed to acquire current PlanningScene"); +public: + TaskPrivate(Task* me, const std::string &name) + : SerialContainerPrivate(me, name) + { + initModel(); + initScene(); + initPlanner(); } - scene_.reset(new planning_scene::PlanningScene(rml_->getModel())); - scene_->setPlanningSceneMsg(res.scene); + void initModel () { + rml_.reset(new robot_model_loader::RobotModelLoader); + if( !rml_->getModel() ) + throw Exception("Task failed to construct RobotModel"); + } + void initScene() { + assert(rml_); - planner_.reset(new planning_pipeline::PlanningPipeline(rml_->getModel(), ros::NodeHandle("move_group"))); + ros::NodeHandle h; + ros::ServiceClient client = h.serviceClient("get_planning_scene"); + client.waitForExistence(); + + moveit_msgs::GetPlanningScene::Request req; + moveit_msgs::GetPlanningScene::Response res; + + req.components.components = + moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS + | moveit_msgs::PlanningSceneComponents::ROBOT_STATE + | moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS + | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES + | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY + | moveit_msgs::PlanningSceneComponents::OCTOMAP + | moveit_msgs::PlanningSceneComponents::TRANSFORMS + | moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX + | moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING + | moveit_msgs::PlanningSceneComponents::OBJECT_COLORS; + + if(!client.call(req, res)){ + throw Exception("Task failed to acquire current PlanningScene"); + } + + scene_ = std::make_shared(rml_->getModel()); + std::const_pointer_cast(scene_)->setPlanningSceneMsg(res.scene); + } + void initPlanner() { + assert(rml_); + assert(scene_); + planner_ = std::make_shared(rml_->getModel(), ros::NodeHandle("move_group")); + } +}; + + +PRIVATE_CLASS_IMPL(Task) +Task::Task(const std::string &name) + : SerialContainer(new TaskPrivate(this, name)) +{ } -Task::~Task(){ - subtasks_.clear(); - scene_.reset(); - planner_.reset(); -} - -void Task::clear(){ - subtasks_.clear(); +void Task::add(std::unique_ptr &&stage) { + if (!stage) + throw std::runtime_error("Task::add() failed: invalid stage pointer"); + if (!insert(std::move(stage))) + throw std::runtime_error(std::string("Task::add() failed for stage: ") + stage->getName()); } bool Task::plan(){ NewSolutionPublisher debug(*this); - bool computed= true; - while(ros::ok() && computed){ - computed= false; - for( SubTaskPtr& subtask : subtasks_ ){ - if( !subtask->canCompute() ) - continue; - std::cout << "Computing subtask '" << subtask->getName() << "':" << std::endl; - bool success= subtask->compute(); - computed= true; - std::cout << (success ? "succeeded" : "failed") << std::endl; - } - if(computed){ + while(ros::ok() && canCompute()) { + if (compute()) { debug.publish(); printState(); - } + } else + break; } return false; } -void Task::add( SubTaskPtr subtask ){ - subtask->setPlanningScene( scene_ ); - subtask->setPlanningPipeline( planner_ ); - - if( !subtasks_.empty() ){ - subtask->impl_->addPredecessor( subtasks_.back() ); - subtasks_.back()->impl_->addSuccessor( subtask ); - } - - subtasks_.push_back( subtask ); -} - const robot_state::RobotState& Task::getCurrentRobotState() const { - return scene_->getCurrentState(); + return pimpl_func()->scene_->getCurrentState(); } void Task::printState(){ - for( auto& st : subtasks_ ){ - std::cout - << st->impl_->getBeginning().size() << " -> " - << st->impl_->getTrajectories().size() - << " <- " << st->impl_->getEnd().size() - << " / " << st->getName() - << std::endl; - } + ContainerBase::StageCallback processor = [](const SubTask& stage, int depth) -> bool { + std::cout << std::string(2*depth, ' ') << stage << std::endl; + }; + traverseStages(processor); } namespace { @@ -120,7 +120,7 @@ bool traverseFullTrajectories( ret= cb(trace); } else if( start.end ){ - for( SubTrajectory* successor : start.end->nextTrajectories() ){ + for( SubTrajectory* successor : start.end->outgoingTrajectories() ){ if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ){ ret= false; break; @@ -135,11 +135,12 @@ bool traverseFullTrajectories( } bool Task::processSolutions(const Task::SolutionCallback& processor) { - const size_t nr_of_trajectories= subtasks_.size(); + const TaskPrivate::array_type& children = pimpl_func()->children(); + const size_t nr_of_trajectories = children.size(); std::vector trace; trace.reserve(nr_of_trajectories); - for(SubTrajectory& st : subtasks_.front()->impl_->trajectories_) - if( !traverseFullTrajectories(st, subtasks_.size(), processor, trace) ) + for(SubTrajectory& st : children.front()->pimpl_func()->trajectories_) + if( !traverseFullTrajectories(st, nr_of_trajectories, processor, trace) ) return false; return true; } diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index fea91497..341062e3 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -1,3 +1,16 @@ +############# +## Testing ## +############# + +## access to private headers +include_directories(${CMAKE_SOURCE_DIR}/src) + +## Add gtest based cpp test target and link libraries +if (CATKIN_ENABLE_TESTING) + catkin_add_gtest(${PROJECT_NAME}-container_test container.cpp) + target_link_libraries(${PROJECT_NAME}-container_test ${PROJECT_NAME} gtest_main ${catkin_LIBRARIES}) +endif() + add_executable(test_plan_current_state test_plan_current_state.cpp) target_link_libraries(test_plan_current_state ${PROJECT_NAME}_subtasks ${PROJECT_NAME}) diff --git a/src/test/container.cpp b/src/test/container.cpp new file mode 100644 index 00000000..1261fba4 --- /dev/null +++ b/src/test/container.cpp @@ -0,0 +1,135 @@ +#include +#include + +#include +#include + +namespace testing { namespace internal { +enum GTestColor { + COLOR_DEFAULT, + COLOR_RED, + COLOR_GREEN, + COLOR_YELLOW +}; +extern void ColoredPrintf(GTestColor color, const char* fmt, ...); +} } +#define PRINTF(...) do { \ + testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); \ +} while(0) + +namespace moveit { namespace task_constructor { + +class TestGenerator : public Generator { +public: + TestGenerator() : Generator("generator") {} + bool canCompute() const override { return true; } + bool compute() override { return false; } +}; + +class TestPropagatingForward : public PropagatingForward { +public: + TestPropagatingForward() : PropagatingForward("forwarder") {} + bool canCompute() const override { return true; } + bool compute() override { return false; } +}; + + +class SubTaskTest : public ::testing::Test { +protected: + void SetUp() {} + void TearDown() {} + + // accessors for private elements of SubTaskPrivate + const SubTaskPrivate* parent(const SubTaskPrivate* p) const { return p->parent_; } + const SubTaskPrivate* prev(const SubTaskPrivate* p) const { return p->predeccessor_; } + const SubTaskPrivate* next(const SubTaskPrivate* p) const { return p->successor_; } + + void validateOrder(const SerialContainerPrivate* container, const std::initializer_list &expected) { + size_t num = container->children_.size(); + ASSERT_TRUE(num == expected.size()) << "different list lengths"; + + // validate position() + EXPECT_EQ(container->children_.begin(), container->position(-(num+1))); + EXPECT_EQ(container->children_.end(), container->position(num)); + + // print order + for (auto it = container->children_.begin(), end = container->children_.end(); it != end; ++it) + PRINTF(" %p", (*it)->pimpl_func()); + PRINTF(" *** parent: %p ***\n", container); + + // validate order + const SubTaskPrivate* predeccessor = container; + const SubTaskPrivate* successor = container; + size_t pos = 0; + auto exp_it = expected.begin(); + for (auto it = container->children_.begin(), end = container->children_.end(); it != end; ++it, ++exp_it, ++pos) { + SubTaskPrivate *child = (*it)->pimpl_func(); + EXPECT_EQ(child, *exp_it) << "wrong order"; + EXPECT_EQ(child->parent_, container) << "wrong parent"; + EXPECT_EQ(it, container->position(pos)) << "bad forward position resolution"; + EXPECT_EQ(it, container->position(pos-num-1)) << "bad backward position resolution"; + EXPECT_EQ(prev(child), predeccessor) << "wrong link to predeccessor for child " << child; + if (successor != container) + EXPECT_EQ(child, successor) << "wrong link to successor for child " << predeccessor; + // store predeccessor and successor for next round + predeccessor = child; + successor = next(child); + } + EXPECT_EQ(successor, container); + } +}; + +TEST_F(SubTaskTest, interfaceFlags) { + std::unique_ptr g = std::make_unique(); + EXPECT_EQ(g->interfaceFlags(), SubTask::InterfaceFlags({SubTask::WRITES_NEXT_INPUT, SubTask::WRITES_PREV_OUTPUT})); +} + +#define VALIDATE(...) \ + PRINTF("*** validateOrder({" #__VA_ARGS__ "}) ***"); \ + validateOrder(cp, {__VA_ARGS__}); + +TEST_F(SubTaskTest, serialContainer) { + SerialContainer c("serial"); + SerialContainerPrivate *cp = c.pimpl_func(); + + EXPECT_TRUE(bool(cp->input_)); + EXPECT_TRUE(bool(cp->output_)); + EXPECT_EQ(prev(cp), nullptr); + EXPECT_EQ(next(cp), nullptr); + VALIDATE(); + + /***** inserting first stage *****/ + auto g = std::make_unique(); + GeneratorPrivate *gp = g->pimpl_func(); + ASSERT_TRUE(c.insert(std::move(g))); + EXPECT_FALSE(g); // ownership transferred to container + VALIDATE(gp); + +#if 0 + // inserting another generator should fail + g = std::make_unique(); + EXPECT_FALSE(c.insert(std::move(g))); + EXPECT_TRUE(bool(g)); // due to failure, pointer should be still valid +#endif + + /***** inserting second stage *****/ + auto f = std::make_unique(); + PropagatingForwardPrivate *fp = f->pimpl_func(); + ASSERT_TRUE(c.insert(std::move(f))); + EXPECT_FALSE(f); // ownership transferred to container + VALIDATE(gp, fp); + + /***** inserting third stage *****/ + auto f2 = std::make_unique(); + PropagatingForwardPrivate *fp2 = f2->pimpl_func(); +#if 0 + EXPECT_FALSE(c.insert(std::move(f2), 0)); // should fail at first position + EXPECT_TRUE(bool(f)); // ownership not transferred to container +#endif + // insert @2nd position + ASSERT_TRUE(c.insert(std::move(f2), 1)); + EXPECT_FALSE(f2); // ownership transferred to container + VALIDATE(gp, fp2, fp); +} + +} } diff --git a/src/test/test_plan_cartesian_forward.cpp b/src/test/test_plan_cartesian_forward.cpp index 29879f16..3822b6ce 100644 --- a/src/test/test_plan_cartesian_forward.cpp +++ b/src/test/test_plan_cartesian_forward.cpp @@ -12,10 +12,10 @@ int main(int argc, char** argv){ Task t; - t.add( std::make_shared("current state") ); + t.add( std::make_unique("current state") ); { - auto move= std::make_shared("lift object"); + auto move= std::make_unique("lift object"); move->setGroup("arm"); move->setLink("s_model_tool0"); move->setMinMaxDistance(0.05, 0.2); @@ -25,11 +25,11 @@ int main(int argc, char** argv){ direction.header.frame_id= "world"; direction.vector.x= 1.0; move->along(direction); - t.add(move); + t.add(std::move(move)); } { - auto move= std::make_shared("lift object"); + auto move= std::make_unique("lift object"); move->setGroup("arm"); move->setLink("s_model_tool0"); move->setMinDistance(0.01); @@ -40,7 +40,7 @@ int main(int argc, char** argv){ target.point.y= 0.7; target.point.z= 1.5; move->towards(target); - t.add(move); + t.add(std::move(move)); } t.plan(); diff --git a/src/test/test_plan_current_state.cpp b/src/test/test_plan_current_state.cpp index 067b722a..bf267448 100644 --- a/src/test/test_plan_current_state.cpp +++ b/src/test/test_plan_current_state.cpp @@ -11,7 +11,7 @@ int main(int argc, char** argv){ Task t; - t.add( std::make_shared("current state") ); + t.add( std::make_unique("current state") ); t.plan(); diff --git a/src/test/test_plan_generate_grasp_pose.cpp b/src/test/test_plan_generate_grasp_pose.cpp index 77b448d6..8af2c3bb 100644 --- a/src/test/test_plan_generate_grasp_pose.cpp +++ b/src/test/test_plan_generate_grasp_pose.cpp @@ -36,7 +36,7 @@ int main(int argc, char** argv){ Task t; - auto st= std::make_shared("generate grasp candidates"); + auto st= std::make_unique("generate grasp candidates"); st->setEndEffector("gripper"); //st->setGroup("arm"); @@ -45,7 +45,7 @@ int main(int argc, char** argv){ st->setAngleDelta(0.1); st->setGraspOffset(0.03); - t.add(st); + t.add(std::move(st)); t.plan(); diff --git a/src/test/test_plan_gripper.cpp b/src/test/test_plan_gripper.cpp index ac9b4735..dae255ff 100644 --- a/src/test/test_plan_gripper.cpp +++ b/src/test/test_plan_gripper.cpp @@ -14,24 +14,24 @@ int main(int argc, char** argv){ Task t; - t.add( std::make_shared("current state") ); + t.add( std::make_unique("current state") ); { - auto gripper= std::make_shared("close gripper"); + auto gripper= std::make_unique("close gripper"); gripper->setEndEffector("gripper"); gripper->setTo("closed"); - t.add( gripper ); + t.add(std::move( gripper )); } t.plan(); /*TODO currently not implemented in gripper*/ /* { - auto gripper= std::make_shared("close gripper"); + auto gripper= std::make_unique("close gripper"); gripper->setEndEffector("gripper"); gripper->setTo("closed"); - t.add( gripper ); + t.add(std::move( gripper )); } - t.add( std::make_shared("current state") ); + t.add( std::make_unique("current state") ); t.plan(); */ return 0;