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
This commit is contained in:
Robert Haschke 2017-10-02 20:05:25 +02:00
parent 6a1eacb315
commit bb06eda33c
35 changed files with 1066 additions and 389 deletions

View File

@ -20,35 +20,24 @@ include_directories(
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
) )
add_compile_options(-std=c++14) set(CMAKE_CXX_STANDARD 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})
add_library(${PROJECT_NAME} add_library(${PROJECT_NAME}
src/storage.cpp
src/subtask.cpp src/subtask.cpp
src/container.cpp
src/task.cpp src/task.cpp
src/debug.cpp src/debug.cpp
src/subtask_p.h src/subtask_p.h
include/${PROJECT_NAME}/utils.h
include/${PROJECT_NAME}/storage.h include/${PROJECT_NAME}/storage.h
include/${PROJECT_NAME}/subtask.h include/${PROJECT_NAME}/subtask.h
include/${PROJECT_NAME}/container.h
include/${PROJECT_NAME}/task.h include/${PROJECT_NAME}/task.h
include/${PROJECT_NAME}/debug.h include/${PROJECT_NAME}/debug.h
) )
add_subdirectory(src/subtasks)
add_subdirectory(src/demo) add_subdirectory(src/demo)
add_subdirectory(src/test) add_subdirectory(src/test)

View File

@ -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<SubTask> value_type;
typedef std::function<bool(const SubTask&, int depth)> StageCallback;
typedef std::function<bool(const std::vector<SubTrajectory*>&)> 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);
};
} }

View File

@ -4,8 +4,10 @@
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <vector> #include <deque>
#include <list>
#include <cassert> #include <cassert>
#include <functional>
namespace planning_scene { namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene) MOVEIT_CLASS_FORWARD(PlanningScene)
@ -19,34 +21,79 @@ namespace moveit { namespace task_constructor {
MOVEIT_CLASS_FORWARD(SubTrajectory) MOVEIT_CLASS_FORWARD(SubTrajectory)
MOVEIT_CLASS_FORWARD(InterfaceState) 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 { class InterfaceState {
friend class SubTrajectory; friend class SubTrajectory;
public: public:
typedef std::vector<SubTrajectory*> SubTrajectoryList; typedef std::deque<SubTrajectory*> SubTrajectoryList;
InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next) InterfaceState(const planning_scene::PlanningSceneConstPtr& ps)
: state(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& incomingTrajectories() const { return incoming_trajectories_; }
inline const SubTrajectoryList& nextTrajectories() const { return next_trajectories_; } inline const SubTrajectoryList& outgoingTrajectories() const { return outgoing_trajectories_; }
public: public:
planning_scene::PlanningSceneConstPtr state; planning_scene::PlanningSceneConstPtr state;
double cost; // minimal costs
private: private:
mutable SubTrajectoryList previous_trajectories_; mutable SubTrajectoryList incoming_trajectories_;
mutable SubTrajectoryList next_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<InterfaceState> {
public:
typedef std::list<InterfaceState> container_type;
typedef std::function<void(const container_type::iterator&)> NotifyFunction;
Interface(const NotifyFunction &notify);
// 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 { struct SubTrajectory {
SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj) SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj)
: trajectory(traj), : trajectory(traj),
@ -55,19 +102,20 @@ struct SubTrajectory {
cost(0) cost(0)
{} {}
void setBeginning(const InterfaceState& state){ inline void setStartState(const InterfaceState& state){
assert(begin == NULL); assert(begin == NULL);
begin= &state; 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); assert(end == NULL);
end= &state; 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* begin;
const InterfaceState* end; const InterfaceState* end;
double cost; double cost;

View File

@ -1,15 +1,17 @@
// copyright Michael 'v4hn' Goerner @ 2017 // copyright Michael 'v4hn' Goerner @ 2017
// copyright Robert Haschke @ 2017
#pragma once #pragma once
#include <moveit_task_constructor/storage.h> #include "utils.h"
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <moveit/planning_scene/planning_scene.h>
#include <vector> #include <vector>
#include <list> #include <list>
#include <tuple>
#define PRIVATE_CLASS(Class) \
friend class Class##Private; \
Class##Private* pimpl_func(); \
const Class##Private* pimpl_func() const;
namespace planning_scene { namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene) MOVEIT_CLASS_FORWARD(PlanningScene)
@ -19,63 +21,84 @@ namespace planning_pipeline {
MOVEIT_CLASS_FORWARD(PlanningPipeline) MOVEIT_CLASS_FORWARD(PlanningPipeline)
} }
namespace robot_trajectory {
MOVEIT_CLASS_FORWARD(RobotTrajectory)
}
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
typedef std::list<InterfaceState> InterfaceStateList; MOVEIT_CLASS_FORWARD(Interface)
typedef std::pair<InterfaceState&, InterfaceState&> InterfaceStatePair;
MOVEIT_CLASS_FORWARD(SubTask) MOVEIT_CLASS_FORWARD(SubTask)
typedef std::weak_ptr<SubTask> SubTaskWeakPtr; MOVEIT_CLASS_FORWARD(SubTrajectory)
class InterfaceState;
typedef std::pair<const InterfaceState&, const InterfaceState&> InterfaceStatePair;
class Task;
class ContainerBasePrivate;
class SubTaskPrivate; class SubTaskPrivate;
class SubTask { class SubTask {
friend class SubTaskPrivate; // allow access to impl_ friend std::ostream& operator<<(std::ostream &os, const SubTask& stage);
friend class Task; // TODO: remove when we have SubTaskContainers friend class SubTaskPrivate;
friend class ContainerBasePrivate;
protected:
SubTaskPrivate* const impl_;
public: 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<InterfaceFlag> InterfaceFlags;
InterfaceFlags interfaceFlags() const;
~SubTask(); ~SubTask();
const std::string& getName() const; 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; virtual bool compute() = 0;
void setPlanningScene(planning_scene::PlanningSceneConstPtr); planning_scene::PlanningSceneConstPtr scene() const;
void setPlanningPipeline(planning_pipeline::PlanningPipelinePtr); planning_pipeline::PlanningPipelinePtr planner() const;
void setPlanningScene(const planning_scene::PlanningSceneConstPtr&);
void setPlanningPipeline(const planning_pipeline::PlanningPipelinePtr&);
protected: protected:
/// can only instantiated by derived classes /// can only instantiated by derived classes
SubTask(SubTaskPrivate *impl); SubTask(SubTaskPrivate *impl);
/// methods called when a new InterfaceState was spawned /// methods called when a new InterfaceState was spawned
virtual inline void newBeginning(const InterfaceStateList::iterator& it) {} virtual void newInputState(const std::list<InterfaceState>::iterator&) {}
virtual inline void newEnd(const InterfaceStateList::iterator& it) {} virtual void newOutputState(const std::list<InterfaceState>::iterator&) {}
planning_scene::PlanningSceneConstPtr scene_; protected:
planning_pipeline::PlanningPipelinePtr planner_; // TODO: use unique_ptr<SubTaskPrivate> 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 PropagatingAnyWayPrivate;
class PropagatingAnyWay : public SubTask { class PropagatingAnyWay : public SubTask {
public: public:
PRIVATE_CLASS(PropagatingAnyWay)
PropagatingAnyWay(const std::string& name); PropagatingAnyWay(const std::string& name);
static inline constexpr PropagationType type() { return ANYWAY; }
bool hasBeginning() const; bool hasBeginning() const;
InterfaceState& fetchStateBeginning(); const InterfaceState &fetchStateBeginning();
void sendForward(const robot_trajectory::RobotTrajectoryPtr& trajectory, void sendForward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
const InterfaceState& from, const InterfaceState& from,
const planning_scene::PlanningSceneConstPtr& to, const planning_scene::PlanningSceneConstPtr& to,
double cost = 0); double cost = 0);
bool hasEnding() const; bool hasEnding() const;
InterfaceState& fetchStateEnding(); const InterfaceState &fetchStateEnding();
void sendBackward(const robot_trajectory::RobotTrajectoryPtr& trajectory, void sendBackward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& from,
const InterfaceState& to, const InterfaceState& to,
@ -83,65 +106,65 @@ public:
protected: protected:
// constructor for use in derived classes // constructor for use in derived classes
inline PropagatingAnyWay(PropagatingAnyWayPrivate* impl); PropagatingAnyWay(PropagatingAnyWayPrivate* impl);
// get informed when new beginnings and endings become available // get informed when new beginnings and endings become available
void newBeginning(const InterfaceStateList::iterator& it); void newInputState(const std::list<InterfaceState>::iterator& it);
void newEnd(const InterfaceStateList::iterator& it); void newOutputState(const std::list<InterfaceState>::iterator& it);
}; };
class PropagatingForwardPrivate;
class PropagatingForward : public PropagatingAnyWay { class PropagatingForward : public PropagatingAnyWay {
public: public:
PRIVATE_CLASS(PropagatingForward)
PropagatingForward(const std::string& name); PropagatingForward(const std::string& name);
static inline constexpr PropagationType type() { return FORWARD; }
private: private:
// restrict access to backward methods // restrict access to backward methods
using PropagatingAnyWay::hasEnding; using PropagatingAnyWay::hasEnding;
using PropagatingAnyWay::fetchStateEnding; using PropagatingAnyWay::fetchStateEnding;
using PropagatingAnyWay::sendBackward; using PropagatingAnyWay::sendBackward;
// don't care about new endings
inline void newEnd(const InterfaceStateList::iterator& it) {}
}; };
class PropagatingBackwardPrivate;
class PropagatingBackward : public PropagatingAnyWay { class PropagatingBackward : public PropagatingAnyWay {
public: public:
PRIVATE_CLASS(PropagatingBackward)
PropagatingBackward(const std::string& name); PropagatingBackward(const std::string& name);
static inline constexpr PropagationType type() { return BACKWARD; }
private: private:
// restrict access to forward methods // restrict access to forward methods
using PropagatingAnyWay::hasBeginning; using PropagatingAnyWay::hasBeginning;
using PropagatingAnyWay::fetchStateBeginning; using PropagatingAnyWay::fetchStateBeginning;
using PropagatingAnyWay::sendForward; using PropagatingAnyWay::sendForward;
// don't care about new beginnings
inline void newBeginning(const InterfaceStateList::iterator& it) {}
}; };
class GeneratorPrivate;
class Generator : public SubTask { class Generator : public SubTask {
public: public:
PRIVATE_CLASS(Generator)
Generator(const std::string& name); Generator(const std::string& name);
static inline constexpr PropagationType type() { return GENERATOR; }
void spawn(const planning_scene::PlanningSceneConstPtr &ps, double cost = 0); void spawn(const planning_scene::PlanningSceneConstPtr &ps, double cost = 0);
}; };
class ConnectingPrivate;
class Connecting : public SubTask { class Connecting : public SubTask {
public: public:
PRIVATE_CLASS(Connecting)
Connecting(const std::string& name); Connecting(const std::string& name);
static inline constexpr PropagationType type() { return CONNECTING; }
bool hasStatePair() const; bool hasStatePair() const;
InterfaceStatePair fetchStatePair(); InterfaceStatePair fetchStatePair();
void connect(const robot_trajectory::RobotTrajectoryPtr&, const InterfaceStatePair&, double cost = 0); void connect(const robot_trajectory::RobotTrajectoryPtr&, const InterfaceStatePair&, double cost = 0);
protected: protected:
virtual void newBeginning(const InterfaceStateList::iterator& it); virtual void newInputState(const std::list<InterfaceState>::iterator& it);
virtual void newEnd(const InterfaceStateList::iterator& it); virtual void newOutputState(const std::list<InterfaceState>::iterator& it);
}; };

View File

@ -9,9 +9,10 @@
#include <geometry_msgs/Vector3Stamped.h> #include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/PointStamped.h> #include <geometry_msgs/PointStamped.h>
namespace moveit { namespace planning_interface { namespace moveit {
MOVEIT_CLASS_FORWARD(MoveGroupInterface); namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface) }
} } namespace core { MOVEIT_CLASS_FORWARD(RobotState) }
}
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {
@ -19,9 +20,8 @@ class CartesianPositionMotion : public PropagatingAnyWay {
public: public:
CartesianPositionMotion(std::string name); CartesianPositionMotion(std::string name);
virtual bool canCompute(); virtual bool canCompute() const override;
virtual bool compute() override;
virtual bool compute();
void setGroup(std::string group); void setGroup(std::string group);
void setLink(std::string link); void setLink(std::string link);

View File

@ -10,9 +10,8 @@ class CurrentState : public Generator {
public: public:
CurrentState(std::string name); CurrentState(std::string name);
virtual bool canCompute(); virtual bool canCompute() const override;
virtual bool compute() override;
virtual bool compute();
protected: protected:
bool ran_; bool ran_;

View File

@ -18,9 +18,8 @@ class GenerateGraspPose : public Generator {
public: public:
GenerateGraspPose(std::string name); GenerateGraspPose(std::string name);
virtual bool canCompute(); virtual bool canCompute() const override;
virtual bool compute() override;
virtual bool compute();
void setEndEffector(std::string eef); void setEndEffector(std::string eef);

View File

@ -14,9 +14,8 @@ class Gripper : public PropagatingForward {
public: public:
Gripper(std::string name); Gripper(std::string name);
virtual bool canCompute(); virtual bool canCompute() const override;
virtual bool compute() override;
virtual bool compute();
void setEndEffector(std::string eef); void setEndEffector(std::string eef);

View File

@ -4,9 +4,9 @@
#include <moveit_task_constructor/subtask.h> #include <moveit_task_constructor/subtask.h>
namespace moveit { namespace planning_interface { namespace moveit {
MOVEIT_CLASS_FORWARD(MoveGroupInterface) namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface)}
} } }
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace subtasks {
@ -14,9 +14,8 @@ class Move : public Connecting {
public: public:
Move(std::string name); Move(std::string name);
virtual bool canCompute(); virtual bool canCompute() const override;
virtual bool compute() override;
virtual bool compute();
void setGroup(std::string group); void setGroup(std::string group);
void setLink(std::string link); void setLink(std::string link);

View File

@ -2,7 +2,7 @@
#pragma once #pragma once
#include <moveit_task_constructor/storage.h> #include "container.h"
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
@ -10,18 +10,6 @@
#include <vector> #include <vector>
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 { namespace moveit { namespace core {
MOVEIT_CLASS_FORWARD(RobotState) MOVEIT_CLASS_FORWARD(RobotState)
}} }}
@ -29,35 +17,26 @@ namespace moveit { namespace core {
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
MOVEIT_CLASS_FORWARD(SubTask) MOVEIT_CLASS_FORWARD(SubTask)
MOVEIT_CLASS_FORWARD(ContainerBase)
MOVEIT_CLASS_FORWARD(Task) MOVEIT_CLASS_FORWARD(Task)
class Task { class TaskPrivate;
class Task : protected SerialContainer {
PRIVATE_CLASS(Task)
public: public:
typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback; typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback;
Task(); Task(const std::string &name = std::string());
~Task();
void add( SubTaskPtr ); void add(std::unique_ptr<SubTask> &&stage);
using SerialContainer::clear;
bool plan(); bool plan();
bool processSolutions(const SolutionCallback &processor);
bool processSolutions(const SolutionCallback &processor) const;
const moveit::core::RobotState &getCurrentRobotState() const;
void printState(); void printState();
void clear(); const moveit::core::RobotState &getCurrentRobotState() const;
bool processSolutions(const SolutionCallback &processor);
protected: bool processSolutions(const SolutionCallback &processor) const;
std::vector<SubTaskPtr> subtasks_;
planning_scene::PlanningScenePtr scene_;
robot_model_loader::RobotModelLoaderPtr rml_;
planning_pipeline::PlanningPipelinePtr planner_;
}; };
} } } }

View File

@ -0,0 +1,58 @@
#pragma once
#include <type_traits>
#include <initializer_list>
/** template class to compose flags from enums in a type-safe fashion */
template<typename Enum>
class Flags
{
static_assert((sizeof(Enum) <= sizeof(int)),
"Flags uses an int as storage, this enum will overflow!");
public:
typedef typename std::conditional<std::is_unsigned<Enum>::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<Enum> 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<Enum>::const_iterator it,
typename std::initializer_list<Enum>::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<Enum> Flags;

View File

@ -9,5 +9,5 @@
<build_depend>moveit_core</build_depend> <build_depend>moveit_core</build_depend>
<run_depend>moveit_core</run_depend> <run_depend>moveit_core</run_depend>
<test_depend>rosunit</test_depend>
</package> </package>

157
src/container.cpp Normal file
View File

@ -0,0 +1,157 @@
#include "container_p.h"
#include <memory>
#include <iostream>
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<ContainerBase*>(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;
}
} }

57
src/container_p.h Normal file
View File

@ -0,0 +1,57 @@
#pragma once
#include <moveit_task_constructor/container.h>
#include "subtask_p.h"
namespace moveit { namespace task_constructor {
class ContainerBasePrivate : public SubTaskPrivate
{
public:
typedef ContainerBase::value_type value_type;
typedef std::vector<value_type> 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;
};
} }

View File

@ -1,5 +1,6 @@
#include <moveit_task_constructor/debug.h> #include <moveit_task_constructor/debug.h>
#include <moveit_task_constructor/task.h> #include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/storage.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <moveit/robot_trajectory/robot_trajectory.h> #include <moveit/robot_trajectory/robot_trajectory.h>

View File

@ -1,3 +1,6 @@
add_executable(foo test.cpp)
target_link_libraries(foo)
add_executable(plan_pick_ur5 plan_pick_ur5.cpp) add_executable(plan_pick_ur5 plan_pick_ur5.cpp)
target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_subtasks ${PROJECT_NAME}) target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_subtasks ${PROJECT_NAME})

View File

@ -43,26 +43,26 @@ int main(int argc, char** argv){
Task t; Task t;
t.add( std::make_shared<subtasks::CurrentState>("current state") ); t.add( std::make_unique<subtasks::CurrentState>("current state") );
{ {
auto move= std::make_shared<subtasks::Gripper>("open gripper"); auto move= std::make_unique<subtasks::Gripper>("open gripper");
move->setEndEffector("left_gripper"); move->setEndEffector("left_gripper");
move->setTo("open"); move->setTo("open");
t.add(move); t.add(std::move(move));
} }
{ {
auto move= std::make_shared<subtasks::Move>("move to pre-grasp"); auto move= std::make_unique<subtasks::Move>("move to pre-grasp");
move->setGroup("left_arm"); move->setGroup("left_arm");
move->setLink("l_gripper_tool_frame"); move->setLink("l_gripper_tool_frame");
move->setPlannerId("RRTConnectkConfigDefault"); move->setPlannerId("RRTConnectkConfigDefault");
move->setTimeout(8.0); move->setTimeout(8.0);
t.add(move); t.add(std::move(move));
} }
{ {
auto move= std::make_shared<subtasks::CartesianPositionMotion>("proceed to grasp pose"); auto move= std::make_unique<subtasks::CartesianPositionMotion>("proceed to grasp pose");
move->setGroup("left_arm"); move->setGroup("left_arm");
move->setLink("l_gripper_tool_frame"); move->setLink("l_gripper_tool_frame");
move->setMinMaxDistance(.03, 0.1); move->setMinMaxDistance(.03, 0.1);
@ -71,11 +71,11 @@ int main(int argc, char** argv){
geometry_msgs::PointStamped target; geometry_msgs::PointStamped target;
target.header.frame_id= "object"; target.header.frame_id= "object";
move->towards(target); move->towards(target);
t.add(move); t.add(std::move(move));
} }
{ {
auto gengrasp= std::make_shared<subtasks::GenerateGraspPose>("generate grasp pose"); auto gengrasp= std::make_unique<subtasks::GenerateGraspPose>("generate grasp pose");
gengrasp->setEndEffector("left_gripper"); gengrasp->setEndEffector("left_gripper");
//gengrasp->setGroup("arm"); //gengrasp->setGroup("arm");
gengrasp->setLink("l_gripper_tool_frame"); gengrasp->setLink("l_gripper_tool_frame");
@ -83,19 +83,19 @@ int main(int argc, char** argv){
gengrasp->setObject("object"); gengrasp->setObject("object");
gengrasp->setGraspOffset(.00); gengrasp->setGraspOffset(.00);
gengrasp->setAngleDelta(.2); gengrasp->setAngleDelta(.2);
t.add(gengrasp); t.add(std::move(gengrasp));
} }
{ {
auto move= std::make_shared<subtasks::Gripper>("grasp"); auto move= std::make_unique<subtasks::Gripper>("grasp");
move->setEndEffector("left_gripper"); move->setEndEffector("left_gripper");
move->setTo("closed"); move->setTo("closed");
move->graspObject("object"); move->graspObject("object");
t.add(move); t.add(std::move(move));
} }
{ {
auto move= std::make_shared<subtasks::CartesianPositionMotion>("lift object"); auto move= std::make_unique<subtasks::CartesianPositionMotion>("lift object");
move->setGroup("left_arm"); move->setGroup("left_arm");
move->setLink("l_gripper_tool_frame"); move->setLink("l_gripper_tool_frame");
move->setMinMaxDistance(0.03, 0.05); move->setMinMaxDistance(0.03, 0.05);
@ -105,7 +105,7 @@ int main(int argc, char** argv){
direction.header.frame_id= "base_link"; direction.header.frame_id= "base_link";
direction.vector.z= 1.0; direction.vector.z= 1.0;
move->along(direction); move->along(direction);
t.add(move); t.add(std::move(move));
} }
t.plan(); t.plan();

View File

@ -41,25 +41,25 @@ int main(int argc, char** argv){
Task t; Task t;
t.add( std::make_shared<subtasks::CurrentState>("current state") ); t.add(std::make_unique<subtasks::CurrentState>("current state"));
{ {
auto move= std::make_shared<subtasks::Gripper>("open gripper"); auto move = std::make_unique<subtasks::Gripper>("open gripper");
move->setEndEffector("gripper"); move->setEndEffector("gripper");
move->setTo("open"); move->setTo("open");
t.add(move); t.add(std::move(std::move(move)));
} }
{ {
auto move= std::make_shared<subtasks::Move>("move to pre-grasp"); auto move = std::make_unique<subtasks::Move>("move to pre-grasp");
move->setGroup("arm"); move->setGroup("arm");
move->setPlannerId("RRTConnectkConfigDefault"); move->setPlannerId("RRTConnectkConfigDefault");
move->setTimeout(8.0); move->setTimeout(8.0);
t.add(move); t.add(std::move(std::move(move)));
} }
{ {
auto move= std::make_shared<subtasks::CartesianPositionMotion>("proceed to grasp pose"); auto move = std::make_unique<subtasks::CartesianPositionMotion>("proceed to grasp pose");
move->setGroup("arm"); move->setGroup("arm");
move->setLink("s_model_tool0"); move->setLink("s_model_tool0");
move->setMinMaxDistance(.03, 0.1); move->setMinMaxDistance(.03, 0.1);
@ -68,11 +68,11 @@ int main(int argc, char** argv){
geometry_msgs::PointStamped target; geometry_msgs::PointStamped target;
target.header.frame_id= "object"; target.header.frame_id= "object";
move->towards(target); move->towards(target);
t.add(move); t.add(std::move(std::move(move)));
} }
{ {
auto gengrasp= std::make_shared<subtasks::GenerateGraspPose>("generate grasp pose"); auto gengrasp = std::make_unique<subtasks::GenerateGraspPose>("generate grasp pose");
gengrasp->setEndEffector("gripper"); gengrasp->setEndEffector("gripper");
//gengrasp->setGroup("arm"); //gengrasp->setGroup("arm");
gengrasp->setGripperGraspPose("open"); gengrasp->setGripperGraspPose("open");
@ -80,19 +80,19 @@ int main(int argc, char** argv){
gengrasp->setGraspOffset(.03); gengrasp->setGraspOffset(.03);
gengrasp->setAngleDelta(-.2); gengrasp->setAngleDelta(-.2);
gengrasp->setMaxIKSolutions(8); gengrasp->setMaxIKSolutions(8);
t.add(gengrasp); t.add(std::move(std::move(gengrasp)));
} }
{ {
auto move= std::make_shared<subtasks::Gripper>("grasp"); auto move = std::make_unique<subtasks::Gripper>("grasp");
move->setEndEffector("gripper"); move->setEndEffector("gripper");
move->setTo("closed"); move->setTo("closed");
move->graspObject("object"); move->graspObject("object");
t.add(move); t.add(std::move(std::move(move)));
} }
{ {
auto move= std::make_shared<subtasks::CartesianPositionMotion>("lift object"); auto move = std::make_unique<subtasks::CartesianPositionMotion>("lift object");
move->setGroup("arm"); move->setGroup("arm");
move->setLink("s_model_tool0"); move->setLink("s_model_tool0");
move->setMinMaxDistance(0.03, 0.05); move->setMinMaxDistance(0.03, 0.05);
@ -102,7 +102,7 @@ int main(int argc, char** argv){
direction.header.frame_id= "world"; direction.header.frame_id= "world";
direction.vector.z= 1.0; direction.vector.z= 1.0;
move->along(direction); move->along(direction);
t.add(move); t.add(std::move(std::move(move)));
} }
t.plan(); t.plan();

33
src/demo/test.cpp Normal file
View File

@ -0,0 +1,33 @@
#include <iostream>
#include <memory>
void overloaded( std::unique_ptr<int> const &arg ) {
std::cout << " by lvalue " << arg.get() << std::endl;
}
void overloaded( std::unique_ptr<int> && arg ) {
std::unique_ptr<int> 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<int> &&arg ) {
std::cout << "- via std::forward: ";
overloaded( std::forward<std::unique_ptr<int>&>( 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<int> &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<int>(5));
std::cout << "initial caller passes lvalue:\n";
auto x = std::make_unique<int>(5);
forwarding( x );
}

22
src/storage.cpp Normal file
View File

@ -0,0 +1,22 @@
#include <moveit_task_constructor/storage.h>
namespace moveit { namespace task_constructor {
Interface::Interface(const Interface::NotifyFunction &notify)
: 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;
}
} }

View File

@ -1,39 +1,71 @@
#include "subtask_p.h" #include "subtask_p.h"
#include <iostream>
// get correctly casted private impl pointer #include <iomanip>
#define I(Class) Class##Private * const impl = reinterpret_cast<Class##Private*>(impl_)
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
SubTask::SubTask(SubTaskPrivate *impl) 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() SubTask::~SubTask()
{ {
delete impl_; delete pimpl_;
} }
const std::string& SubTask::getName() const { const std::string& SubTask::getName() const {
return impl_->name_; return pimpl_->name_;
} }
void SubTask::setPlanningScene(planning_scene::PlanningSceneConstPtr scene){ std::ostream& operator<<(std::ostream &os, const SubTask& stage) {
scene_= scene; 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){ planning_scene::PlanningSceneConstPtr SubTask::scene() const
planner_= planner; {
return pimpl_->scene_;
} }
planning_pipeline::PlanningPipelinePtr SubTask::planner() const
void SubTaskPrivate::addPredecessor(SubTaskPtr prev_task){ {
predecessor_= SubTaskWeakPtr(prev_task); return pimpl_->planner_;
} }
void SubTaskPrivate::addSuccessor(SubTaskPtr next_task){ void SubTask::setPlanningScene(const planning_scene::PlanningSceneConstPtr &scene){
successor_= next_task; pimpl_->scene_= scene;
}
void SubTask::setPlanningPipeline(const planning_pipeline::PlanningPipelinePtr &planner){
pimpl_->planner_= planner;
} }
SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){ SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
@ -41,37 +73,27 @@ SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajec
return trajectories_.back(); return trajectories_.back();
} }
void SubTaskPrivate::sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps){ SubTask::InterfaceFlags SubTaskPrivate::interfaceFlags() const
if( successor_ ){ {
std::cout << "sending state forward to successor" << std::endl; // the base class provides the read flags
traj.end= successor_->impl_->newBeginning(ps, &traj); 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){ void SubTaskPrivate::sendForward(SubTrajectory& trajectory, const planning_scene::PlanningSceneConstPtr& ps){
if( !predecessor_.expired() ){ std::cout << "sending state to start" << std::endl;
std::cout << "sending state backward to predecessor" << std::endl; nextInput()->add(ps, &trajectory, NULL);
traj.begin= predecessor_.lock()->impl_->newEnd(ps, &traj);
}
} }
InterfaceState* SubTaskPrivate::newBeginning(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_end){ void SubTaskPrivate::sendBackward(SubTrajectory& trajectory, const planning_scene::PlanningSceneConstPtr& ps){
assert( bool(ps) ); std::cout << "sending state to end" << std::endl;
beginnings_.push_back( InterfaceState(ps, old_end, NULL) ); prevOutput()->add(ps, NULL, &trajectory);
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();
} }
PRIVATE_CLASS_IMPL(PropagatingAnyWay)
PropagatingAnyWay::PropagatingAnyWay(const std::string &name) PropagatingAnyWay::PropagatingAnyWay(const std::string &name)
: SubTask(new PropagatingAnyWayPrivate(this, name)) : SubTask(new PropagatingAnyWayPrivate(this, name))
{} {}
@ -79,17 +101,17 @@ PropagatingAnyWay::PropagatingAnyWay(const std::string &name)
PropagatingAnyWay::PropagatingAnyWay(PropagatingAnyWayPrivate *impl) : SubTask(impl) {} PropagatingAnyWay::PropagatingAnyWay(PropagatingAnyWayPrivate *impl) : SubTask(impl) {}
bool PropagatingAnyWay::hasBeginning() const{ bool PropagatingAnyWay::hasBeginning() const{
I(PropagatingAnyWay); IMPL(const PropagatingAnyWay);
return impl->it_beginnings_ != impl->beginnings_.end(); return impl->next_input_ != impl->input_->end();
} }
InterfaceState& PropagatingAnyWay::fetchStateBeginning(){ const InterfaceState& PropagatingAnyWay::fetchStateBeginning(){
I(PropagatingAnyWay); IMPL(PropagatingAnyWay);
if(impl->it_beginnings_ == impl->beginnings_.end()) if (!hasBeginning())
throw std::runtime_error("no new state for beginning available"); throw std::runtime_error("no new state for beginning available");
InterfaceState& state= *impl->it_beginnings_; const InterfaceState& state= *impl->next_input_;
++impl->it_beginnings_; ++impl->next_input_;
return state; return state;
} }
@ -98,121 +120,129 @@ void PropagatingAnyWay::sendForward(const robot_trajectory::RobotTrajectoryPtr&
const InterfaceState& from, const InterfaceState& from,
const planning_scene::PlanningSceneConstPtr& to, const planning_scene::PlanningSceneConstPtr& to,
double cost){ double cost){
SubTrajectory &trajectory = impl_->addTrajectory(t, cost); IMPL(PropagatingAnyWay);
trajectory.setBeginning(from); SubTrajectory &trajectory = impl->addTrajectory(t, cost);
impl_->sendForward(trajectory, to); 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 // 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 // so let's point it at the new one
if( impl->it_beginnings_ == impl->beginnings_.end() ) if( impl->next_input_ == impl->input_->end() )
--impl->it_beginnings_; --impl->next_input_;
} }
bool PropagatingAnyWay::hasEnding() const{ bool PropagatingAnyWay::hasEnding() const{
I(PropagatingAnyWay); IMPL(const PropagatingAnyWay);
return impl->it_endings_ != impl->endings_.end(); return impl->next_output_ != impl->output_->end();
} }
InterfaceState& PropagatingAnyWay::fetchStateEnding(){ const InterfaceState& PropagatingAnyWay::fetchStateEnding(){
I(PropagatingAnyWay); IMPL(PropagatingAnyWay);
if(impl->it_endings_ == impl->endings_.end()) if(!hasEnding())
throw std::runtime_error("no new state for ending available"); throw std::runtime_error("no new state for ending available");
InterfaceState& state= *impl->it_endings_; const InterfaceState& state= *impl->next_output_;
++impl->it_endings_; ++impl->next_output_;
return state; return state;
} }
void PropagatingAnyWay::sendBackward(const robot_trajectory::RobotTrajectoryPtr& t, void PropagatingAnyWay::sendBackward(const robot_trajectory::RobotTrajectoryPtr& t,
const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& from,
const InterfaceState& to, const InterfaceState& to,
double cost){ double cost){
SubTrajectory& trajectory = impl_->addTrajectory(t, cost); IMPL(PropagatingAnyWay);
trajectory.setEnding(to); SubTrajectory& trajectory = impl->addTrajectory(t, cost);
impl_->sendBackward(trajectory, from); 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 // 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 // so let's point it at the new one
if( impl->it_endings_ == impl->endings_.end() ) if( impl->next_output_ == impl->output_->end() )
--impl->it_endings_; --impl->next_output_;
} }
PRIVATE_CLASS_IMPL(PropagatingForward)
PropagatingForward::PropagatingForward(const std::string& name) PropagatingForward::PropagatingForward(const std::string& name)
: PropagatingAnyWay(new PropagatingForwardPrivate(this, name)) : PropagatingAnyWay(new PropagatingForwardPrivate(this, name))
{} {}
PRIVATE_CLASS_IMPL(PropagatingBackward)
PropagatingBackward::PropagatingBackward(const std::string &name) PropagatingBackward::PropagatingBackward(const std::string &name)
: PropagatingAnyWay(new PropagatingBackwardPrivate(this, name)) : PropagatingAnyWay(new PropagatingBackwardPrivate(this, name))
{} {}
PRIVATE_CLASS_IMPL(Generator)
Generator::Generator(const std::string &name) 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) void Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost)
{ {
IMPL(Generator);
// empty trajectory ref -> this node only produces states // empty trajectory ref -> this node only produces states
robot_trajectory::RobotTrajectoryPtr dummy; 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; std::cout << "spawning state" << std::endl;
impl_->sendBackward(trajectory, ps); impl->sendBackward(trajectory, ps);
impl_->sendForward(trajectory, ps); impl->sendForward(trajectory, ps);
} }
PRIVATE_CLASS_IMPL(Connecting)
Connecting::Connecting(const std::string &name) Connecting::Connecting(const std::string &name)
: SubTask(new ConnectingPrivate(this, 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 // 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; --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 // 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; --impl->it_pairs_.second;
} }
bool Connecting::hasStatePair() const{ bool Connecting::hasStatePair() const{
I(Connecting); IMPL(const Connecting);
// TODO: implement this properly // TODO: implement this properly
return impl->it_pairs_.first != impl->beginnings_.end() && return impl->it_pairs_.first != impl->input_->end() &&
impl->it_pairs_.second != impl->endings_.end(); impl->it_pairs_.second != impl->output_->end();
} }
std::pair<InterfaceState&, InterfaceState&> Connecting::fetchStatePair(){ InterfaceStatePair Connecting::fetchStatePair(){
I(Connecting); IMPL(Connecting);
// TODO: implement this properly // TODO: implement this properly
return std::pair<InterfaceState&, InterfaceState&> return std::pair<const InterfaceState&, const InterfaceState&>
(*impl->it_pairs_.first, *(impl->it_pairs_.second++)); (*impl->it_pairs_.first, *(impl->it_pairs_.second++));
} }
void Connecting::connect(const robot_trajectory::RobotTrajectoryPtr& t, const InterfaceStatePair& state_pair, double cost) void Connecting::connect(const robot_trajectory::RobotTrajectoryPtr& t, const InterfaceStatePair& state_pair, double cost)
{ {
moveit::task_constructor::SubTrajectory& trajectory = impl_->addTrajectory(t, cost); IMPL(Connecting);
trajectory.setBeginning(state_pair.first); SubTrajectory& trajectory = impl->addTrajectory(t, cost);
trajectory.setEnding(state_pair.second); trajectory.setStartState(state_pair.first);
trajectory.setEndState(state_pair.second);
} }

View File

@ -1,41 +1,51 @@
// copyright Robert Haschke @ 2017
#pragma once #pragma once
#include <moveit_task_constructor/subtask.h> #include <moveit_task_constructor/subtask.h>
#include <moveit_task_constructor/storage.h>
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
class SubTaskPrivate { 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: public:
inline SubTaskPrivate(SubTask* me, const std::string& name) 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<SubTrajectory>& getTrajectories() const { return trajectories_; }
void addPredecessor(SubTaskPtr);
void addSuccessor(SubTaskPtr);
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost); SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
virtual SubTask::InterfaceFlags interfaceFlags() const;
void sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps); void sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
void sendBackward(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: public:
SubTask* const me_; // pointer to owning instance SubTask* const me_; // associated/owning SubTask instance
const std::string name_; const std::string name_;
// avoid shared pointers back: why? to allow proper deallocation? planning_scene::PlanningSceneConstPtr scene_;
SubTaskWeakPtr predecessor_; planning_pipeline::PlanningPipelinePtr planner_;
SubTaskPtr successor_;
InterfaceStateList beginnings_; InterfacePtr input_;
InterfaceStateList endings_; InterfacePtr output_;
std::list<SubTrajectory> trajectories_; std::list<SubTrajectory> 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 { class PropagatingAnyWayPrivate : public SubTaskPrivate {
@ -44,19 +54,67 @@ class PropagatingAnyWayPrivate : public SubTaskPrivate {
public: public:
inline PropagatingAnyWayPrivate(PropagatingAnyWay *me, const std::string &name) inline PropagatingAnyWayPrivate(PropagatingAnyWay *me, const std::string &name)
: SubTaskPrivate(me, 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: protected:
InterfaceStateList::iterator it_beginnings_; Interface::const_iterator next_input_;
InterfaceStateList::iterator it_endings_; Interface::const_iterator next_output_;
}; };
// for now, we use the same implementation for the reduced versions too class PropagatingForwardPrivate : public PropagatingAnyWayPrivate {
typedef PropagatingAnyWayPrivate PropagatingForwardPrivate; public:
typedef PropagatingAnyWayPrivate PropagatingBackwardPrivate; 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 { class ConnectingPrivate : public SubTaskPrivate {
@ -65,11 +123,27 @@ class ConnectingPrivate : public SubTaskPrivate {
public: public:
inline ConnectingPrivate(Connecting *me, const std::string &name) inline ConnectingPrivate(Connecting *me, const std::string &name)
: SubTaskPrivate(me, 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: private:
std::pair<InterfaceStateList::iterator, InterfaceStateList::iterator> it_pairs_; std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
}; };
} } } }
// implement pimpl_func()
#define PRIVATE_CLASS_IMPL(Class) \
inline Class##Private* Class::pimpl_func() { return static_cast<Class##Private*>(pimpl_); } \
inline const Class##Private* Class::pimpl_func() const { return static_cast<const Class##Private*>(pimpl_); } \
// get correctly casted private impl pointer
#define IMPL(Class) Class##Private * const impl = pimpl_func()

View File

@ -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})

View File

@ -1,4 +1,5 @@
#include <moveit_task_constructor/subtasks/cartesian_position_motion.h> #include <moveit_task_constructor/subtasks/cartesian_position_motion.h>
#include <moveit_task_constructor/storage.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <moveit_msgs/DisplayTrajectory.h> #include <moveit_msgs/DisplayTrajectory.h>
@ -58,7 +59,7 @@ void CartesianPositionMotion::setCartesianStepSize(double distance){
step_size_= distance; step_size_= distance;
} }
bool CartesianPositionMotion::canCompute(){ bool CartesianPositionMotion::canCompute() const{
return hasBeginning() || hasEnding(); return hasBeginning() || hasEnding();
} }
@ -83,7 +84,7 @@ bool CartesianPositionMotion::compute(){
bool CartesianPositionMotion::_computeFromBeginning(){ bool CartesianPositionMotion::_computeFromBeginning(){
assert( hasBeginning() ); assert( hasBeginning() );
moveit::task_constructor::InterfaceState& beginning= fetchStateBeginning(); const InterfaceState& beginning= fetchStateBeginning();
planning_scene::PlanningScenePtr result_scene = beginning.state->diff(); planning_scene::PlanningScenePtr result_scene = beginning.state->diff();
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst(); robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
@ -173,7 +174,7 @@ bool CartesianPositionMotion::_computeFromBeginning(){
bool CartesianPositionMotion::_computeFromEnding(){ bool CartesianPositionMotion::_computeFromEnding(){
assert( hasEnding() ); assert( hasEnding() );
moveit::task_constructor::InterfaceState& ending= fetchStateEnding(); const InterfaceState& ending= fetchStateEnding();
planning_scene::PlanningScenePtr result_scene = ending.state->diff(); planning_scene::PlanningScenePtr result_scene = ending.state->diff();
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst(); 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){ void CartesianPositionMotion::_publishTrajectory(const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start){
moveit_msgs::DisplayTrajectory dt; moveit_msgs::DisplayTrajectory dt;
robot_state::robotStateToRobotStateMsg(start, dt.trajectory_start); robot_state::robotStateToRobotStateMsg(start, dt.trajectory_start);
dt.model_id= scene_->getRobotModel()->getName(); dt.model_id= scene()->getRobotModel()->getName();
dt.trajectory.resize(1); dt.trajectory.resize(1);
trajectory.getRobotTrajectoryMsg(dt.trajectory[0]); trajectory.getRobotTrajectoryMsg(dt.trajectory[0]);
dt.model_id= start.getRobotModel()->getName(); dt.model_id= start.getRobotModel()->getName();
pub.publish(dt); pub.publish(dt);
} }
} } } } } }

View File

@ -8,14 +8,13 @@ CurrentState::CurrentState(std::string name)
ran_= false; ran_= false;
} }
bool CurrentState::canCompute(){ bool CurrentState::canCompute() const{
return !ran_; return !ran_;
} }
bool CurrentState::compute(){ bool CurrentState::compute(){
ran_= true; ran_= true;
assert( scene_ ); spawn(scene());
spawn(scene_);
return true; return true;
} }

View File

@ -70,7 +70,7 @@ void GenerateGraspPose::setMaxIKSolutions(uint32_t n){
max_ik_solutions_= n; max_ik_solutions_= n;
} }
bool GenerateGraspPose::canCompute(){ bool GenerateGraspPose::canCompute() const{
return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI; return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI;
} }
@ -102,9 +102,9 @@ namespace {
} }
bool GenerateGraspPose::compute(){ 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(); robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst();
const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef_); const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef_);
@ -122,7 +122,7 @@ bool GenerateGraspPose::compute(){
const moveit::core::GroupStateValidityCallbackFn is_valid= const moveit::core::GroupStateValidityCallbackFn is_valid=
std::bind( std::bind(
&isValid, &isValid,
scene_, scene(),
ignore_collisions_, ignore_collisions_,
&previous_solutions_, &previous_solutions_,
std::placeholders::_1, std::placeholders::_1,
@ -130,7 +130,7 @@ bool GenerateGraspPose::compute(){
std::placeholders::_3); std::placeholders::_3);
geometry_msgs::Pose object_pose, grasp_pose; 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()) 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"); throw std::runtime_error("requested object does not exist or could not be retrieved");

View File

@ -1,4 +1,5 @@
#include <moveit_task_constructor/subtasks/gripper.h> #include <moveit_task_constructor/subtasks/gripper.h>
#include <moveit_task_constructor/storage.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h> #include <moveit/robot_state/conversions.h>
@ -36,16 +37,16 @@ void Gripper::graspObject(std::string grasp_object){
grasp_object_= grasp_object; grasp_object_= grasp_object;
} }
bool Gripper::canCompute(){ bool Gripper::canCompute() const{
return hasBeginning(); return hasBeginning();
} }
bool Gripper::compute(){ bool Gripper::compute(){
assert( scene_->getRobotModel() ); assert(scene()->getRobotModel());
if(!mgi_){ if(!mgi_){
assert( scene_->getRobotModel()->hasEndEffector(eef_) && "no end effector with that name defined in srdf" ); assert(scene()->getRobotModel()->hasEndEffector(eef_) && "no end effector with that name defined in srdf");
const moveit::core::JointModelGroup* jmg= scene_->getRobotModel()->getEndEffector(eef_); const moveit::core::JointModelGroup* jmg= scene()->getRobotModel()->getEndEffector(eef_);
const std::string group_name= jmg->getName(); const std::string group_name= jmg->getName();
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_name); mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_name);
@ -68,7 +69,7 @@ bool Gripper::compute(){
} }
::planning_interface::MotionPlanResponse res; ::planning_interface::MotionPlanResponse res;
if(!planner_->generatePlan(scene, req, res)) if(!planner()->generatePlan(scene, req, res))
return false; return false;
// set end state // set end state

View File

@ -1,4 +1,5 @@
#include <moveit_task_constructor/subtasks/move.h> #include <moveit_task_constructor/subtasks/move.h>
#include <moveit_task_constructor/storage.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h> #include <moveit/robot_state/conversions.h>
@ -43,14 +44,12 @@ void Move::setTo(std::string named_target){
} }
*/ */
bool Move::canCompute(){ bool Move::canCompute() const{
return hasStatePair(); return hasStatePair();
} }
bool Move::compute(){ bool Move::compute(){
assert( scene_->getRobotModel() ); InterfaceStatePair state_pair= fetchStatePair();
std::pair<InterfaceState&, InterfaceState&> state_pair= fetchStatePair();
mgi_->setJointValueTarget(state_pair.second.state->getCurrentState()); mgi_->setJointValueTarget(state_pair.second.state->getCurrentState());
if( !planner_id_.empty() ) if( !planner_id_.empty() )
@ -62,7 +61,7 @@ bool Move::compute(){
ros::Duration(4.0).sleep(); ros::Duration(4.0).sleep();
::planning_interface::MotionPlanResponse res; ::planning_interface::MotionPlanResponse res;
if(!planner_->generatePlan(state_pair.first.state, req, res)) if(!planner()->generatePlan(state_pair.first.state, req, res))
return false; return false;
// finish subtask // finish subtask

View File

@ -1,5 +1,7 @@
#include "subtask_p.h" #include "container_p.h"
#include <moveit_task_constructor/task.h> #include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/container.h>
#include <moveit_task_constructor/debug.h> #include <moveit_task_constructor/debug.h>
#include <ros/ros.h> #include <ros/ros.h>
@ -11,98 +13,96 @@
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
Task::Task(){ class TaskPrivate : public SerialContainerPrivate {
rml_.reset(new robot_model_loader::RobotModelLoader); friend class Task;
if( !rml_->getModel() ) robot_model_loader::RobotModelLoaderPtr rml_;
throw Exception("Task failed to construct RobotModel");
ros::NodeHandle h; public:
TaskPrivate(Task* me, const std::string &name)
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene"); : SerialContainerPrivate(me, name)
client.waitForExistence(); {
initModel();
moveit_msgs::GetPlanningScene::Request req; initScene();
moveit_msgs::GetPlanningScene::Response res; initPlanner();
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_.reset(new planning_scene::PlanningScene(rml_->getModel())); void initModel () {
scene_->setPlanningSceneMsg(res.scene); 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<moveit_msgs::GetPlanningScene>("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<planning_scene::PlanningScene>(rml_->getModel());
std::const_pointer_cast<planning_scene::PlanningScene>(scene_)->setPlanningSceneMsg(res.scene);
}
void initPlanner() {
assert(rml_);
assert(scene_);
planner_ = std::make_shared<planning_pipeline::PlanningPipeline>(rml_->getModel(), ros::NodeHandle("move_group"));
}
};
PRIVATE_CLASS_IMPL(Task)
Task::Task(const std::string &name)
: SerialContainer(new TaskPrivate(this, name))
{
} }
Task::~Task(){ void Task::add(std::unique_ptr<SubTask> &&stage) {
subtasks_.clear(); if (!stage)
scene_.reset(); throw std::runtime_error("Task::add() failed: invalid stage pointer");
planner_.reset(); if (!insert(std::move(stage)))
} throw std::runtime_error(std::string("Task::add() failed for stage: ") + stage->getName());
void Task::clear(){
subtasks_.clear();
} }
bool Task::plan(){ bool Task::plan(){
NewSolutionPublisher debug(*this); NewSolutionPublisher debug(*this);
bool computed= true; while(ros::ok() && canCompute()) {
while(ros::ok() && computed){ if (compute()) {
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){
debug.publish(); debug.publish();
printState(); printState();
} } else
break;
} }
return false; 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 { const robot_state::RobotState& Task::getCurrentRobotState() const {
return scene_->getCurrentState(); return pimpl_func()->scene_->getCurrentState();
} }
void Task::printState(){ void Task::printState(){
for( auto& st : subtasks_ ){ ContainerBase::StageCallback processor = [](const SubTask& stage, int depth) -> bool {
std::cout std::cout << std::string(2*depth, ' ') << stage << std::endl;
<< st->impl_->getBeginning().size() << " -> " };
<< st->impl_->getTrajectories().size() traverseStages(processor);
<< " <- " << st->impl_->getEnd().size()
<< " / " << st->getName()
<< std::endl;
}
} }
namespace { namespace {
@ -120,7 +120,7 @@ bool traverseFullTrajectories(
ret= cb(trace); ret= cb(trace);
} }
else if( start.end ){ else if( start.end ){
for( SubTrajectory* successor : start.end->nextTrajectories() ){ for( SubTrajectory* successor : start.end->outgoingTrajectories() ){
if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ){ if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ){
ret= false; ret= false;
break; break;
@ -135,11 +135,12 @@ bool traverseFullTrajectories(
} }
bool Task::processSolutions(const Task::SolutionCallback& processor) { 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<SubTrajectory*> trace; std::vector<SubTrajectory*> trace;
trace.reserve(nr_of_trajectories); trace.reserve(nr_of_trajectories);
for(SubTrajectory& st : subtasks_.front()->impl_->trajectories_) for(SubTrajectory& st : children.front()->pimpl_func()->trajectories_)
if( !traverseFullTrajectories(st, subtasks_.size(), processor, trace) ) if( !traverseFullTrajectories(st, nr_of_trajectories, processor, trace) )
return false; return false;
return true; return true;
} }

View File

@ -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) add_executable(test_plan_current_state test_plan_current_state.cpp)
target_link_libraries(test_plan_current_state ${PROJECT_NAME}_subtasks ${PROJECT_NAME}) target_link_libraries(test_plan_current_state ${PROJECT_NAME}_subtasks ${PROJECT_NAME})

135
src/test/container.cpp Normal file
View File

@ -0,0 +1,135 @@
#include <container_p.h>
#include <subtask_p.h>
#include <gtest/gtest.h>
#include <initializer_list>
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<SubTaskPrivate*> &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<Generator> g = std::make_unique<TestGenerator>();
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<TestGenerator>();
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<TestGenerator>();
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<TestPropagatingForward>();
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<TestPropagatingForward>();
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);
}
} }

View File

@ -12,10 +12,10 @@ int main(int argc, char** argv){
Task t; Task t;
t.add( std::make_shared<subtasks::CurrentState>("current state") ); t.add( std::make_unique<subtasks::CurrentState>("current state") );
{ {
auto move= std::make_shared<subtasks::CartesianPositionMotion>("lift object"); auto move= std::make_unique<subtasks::CartesianPositionMotion>("lift object");
move->setGroup("arm"); move->setGroup("arm");
move->setLink("s_model_tool0"); move->setLink("s_model_tool0");
move->setMinMaxDistance(0.05, 0.2); move->setMinMaxDistance(0.05, 0.2);
@ -25,11 +25,11 @@ int main(int argc, char** argv){
direction.header.frame_id= "world"; direction.header.frame_id= "world";
direction.vector.x= 1.0; direction.vector.x= 1.0;
move->along(direction); move->along(direction);
t.add(move); t.add(std::move(move));
} }
{ {
auto move= std::make_shared<subtasks::CartesianPositionMotion>("lift object"); auto move= std::make_unique<subtasks::CartesianPositionMotion>("lift object");
move->setGroup("arm"); move->setGroup("arm");
move->setLink("s_model_tool0"); move->setLink("s_model_tool0");
move->setMinDistance(0.01); move->setMinDistance(0.01);
@ -40,7 +40,7 @@ int main(int argc, char** argv){
target.point.y= 0.7; target.point.y= 0.7;
target.point.z= 1.5; target.point.z= 1.5;
move->towards(target); move->towards(target);
t.add(move); t.add(std::move(move));
} }
t.plan(); t.plan();

View File

@ -11,7 +11,7 @@ int main(int argc, char** argv){
Task t; Task t;
t.add( std::make_shared<subtasks::CurrentState>("current state") ); t.add( std::make_unique<subtasks::CurrentState>("current state") );
t.plan(); t.plan();

View File

@ -36,7 +36,7 @@ int main(int argc, char** argv){
Task t; Task t;
auto st= std::make_shared<subtasks::GenerateGraspPose>("generate grasp candidates"); auto st= std::make_unique<subtasks::GenerateGraspPose>("generate grasp candidates");
st->setEndEffector("gripper"); st->setEndEffector("gripper");
//st->setGroup("arm"); //st->setGroup("arm");
@ -45,7 +45,7 @@ int main(int argc, char** argv){
st->setAngleDelta(0.1); st->setAngleDelta(0.1);
st->setGraspOffset(0.03); st->setGraspOffset(0.03);
t.add(st); t.add(std::move(st));
t.plan(); t.plan();

View File

@ -14,24 +14,24 @@ int main(int argc, char** argv){
Task t; Task t;
t.add( std::make_shared<subtasks::CurrentState>("current state") ); t.add( std::make_unique<subtasks::CurrentState>("current state") );
{ {
auto gripper= std::make_shared<subtasks::Gripper>("close gripper"); auto gripper= std::make_unique<subtasks::Gripper>("close gripper");
gripper->setEndEffector("gripper"); gripper->setEndEffector("gripper");
gripper->setTo("closed"); gripper->setTo("closed");
t.add( gripper ); t.add(std::move( gripper ));
} }
t.plan(); t.plan();
/*TODO currently not implemented in gripper*/ /*TODO currently not implemented in gripper*/
/* /*
{ {
auto gripper= std::make_shared<subtasks::Gripper>("close gripper"); auto gripper= std::make_unique<subtasks::Gripper>("close gripper");
gripper->setEndEffector("gripper"); gripper->setEndEffector("gripper");
gripper->setTo("closed"); gripper->setTo("closed");
t.add( gripper ); t.add(std::move( gripper ));
} }
t.add( std::make_shared<subtasks::CurrentState>("current state") ); t.add( std::make_unique<subtasks::CurrentState>("current state") );
t.plan(); t.plan();
*/ */
return 0; return 0;