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

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 <vector>
#include <deque>
#include <list>
#include <cassert>
#include <functional>
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<SubTrajectory*> SubTrajectoryList;
typedef std::deque<SubTrajectory*> 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<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 {
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;

View File

@ -1,15 +1,17 @@
// copyright Michael 'v4hn' Goerner @ 2017
// copyright Robert Haschke @ 2017
#pragma once
#include <moveit_task_constructor/storage.h>
#include "utils.h"
#include <moveit/macros/class_forward.h>
#include <moveit/planning_scene/planning_scene.h>
#include <vector>
#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 {
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<InterfaceState> InterfaceStateList;
typedef std::pair<InterfaceState&, InterfaceState&> InterfaceStatePair;
MOVEIT_CLASS_FORWARD(Interface)
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 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<InterfaceFlag> 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<InterfaceState>::iterator&) {}
virtual void newOutputState(const std::list<InterfaceState>::iterator&) {}
planning_scene::PlanningSceneConstPtr scene_;
planning_pipeline::PlanningPipelinePtr planner_;
protected:
// 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 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<InterfaceState>::iterator& it);
void newOutputState(const std::list<InterfaceState>::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<InterfaceState>::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/PointStamped.h>
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);

View File

@ -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_;

View File

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

View File

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

View File

@ -4,9 +4,9 @@
#include <moveit_task_constructor/subtask.h>
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);

View File

@ -2,7 +2,7 @@
#pragma once
#include <moveit_task_constructor/storage.h>
#include "container.h"
#include <moveit/macros/class_forward.h>
@ -10,18 +10,6 @@
#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 {
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<bool(const std::vector<SubTrajectory*>&)> SolutionCallback;
Task();
~Task();
Task(const std::string &name = std::string());
void add( SubTaskPtr );
void add(std::unique_ptr<SubTask> &&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<SubTaskPtr> 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;
};
} }

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>
<run_depend>moveit_core</run_depend>
<test_depend>rosunit</test_depend>
</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/task.h>
#include <moveit_task_constructor/storage.h>
#include <ros/ros.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)
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;
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->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->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<subtasks::CartesianPositionMotion>("proceed to grasp pose");
auto move= std::make_unique<subtasks::CartesianPositionMotion>("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<subtasks::GenerateGraspPose>("generate grasp pose");
auto gengrasp= std::make_unique<subtasks::GenerateGraspPose>("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<subtasks::Gripper>("grasp");
auto move= std::make_unique<subtasks::Gripper>("grasp");
move->setEndEffector("left_gripper");
move->setTo("closed");
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->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();

View File

@ -41,25 +41,25 @@ int main(int argc, char** argv){
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->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->setPlannerId("RRTConnectkConfigDefault");
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->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<subtasks::GenerateGraspPose>("generate grasp pose");
auto gengrasp = std::make_unique<subtasks::GenerateGraspPose>("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<subtasks::Gripper>("grasp");
auto move = std::make_unique<subtasks::Gripper>("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<subtasks::CartesianPositionMotion>("lift object");
auto move = std::make_unique<subtasks::CartesianPositionMotion>("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();

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"
// get correctly casted private impl pointer
#define I(Class) Class##Private * const impl = reinterpret_cast<Class##Private*>(impl_)
#include <iostream>
#include <iomanip>
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<InterfaceState&, InterfaceState&> Connecting::fetchStatePair(){
I(Connecting);
InterfaceStatePair Connecting::fetchStatePair(){
IMPL(Connecting);
// TODO: implement this properly
return std::pair<InterfaceState&, InterfaceState&>
return std::pair<const InterfaceState&, const InterfaceState&>
(*impl->it_pairs_.first, *(impl->it_pairs_.second++));
}
void Connecting::connect(const robot_trajectory::RobotTrajectoryPtr& t, const InterfaceStatePair& state_pair, double cost)
{
moveit::task_constructor::SubTrajectory& trajectory = impl_->addTrajectory(t, cost);
trajectory.setBeginning(state_pair.first);
trajectory.setEnding(state_pair.second);
IMPL(Connecting);
SubTrajectory& trajectory = impl->addTrajectory(t, cost);
trajectory.setStartState(state_pair.first);
trajectory.setEndState(state_pair.second);
}

View File

@ -1,41 +1,51 @@
// copyright Robert Haschke @ 2017
#pragma once
#include <moveit_task_constructor/subtask.h>
#include <moveit_task_constructor/storage.h>
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<SubTrajectory>& 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<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 {
@ -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<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/storage.h>
#include <ros/ros.h>
#include <moveit_msgs/DisplayTrajectory.h>
@ -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);
}
} } }

View File

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

View File

@ -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");

View File

@ -1,4 +1,5 @@
#include <moveit_task_constructor/subtasks/gripper.h>
#include <moveit_task_constructor/storage.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
@ -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<moveit::planning_interface::MoveGroupInterface>(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

View File

@ -1,4 +1,5 @@
#include <moveit_task_constructor/subtasks/move.h>
#include <moveit_task_constructor/storage.h>
#include <moveit/planning_scene/planning_scene.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();
}
bool Move::compute(){
assert( scene_->getRobotModel() );
std::pair<InterfaceState&, InterfaceState&> 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

View File

@ -1,5 +1,7 @@
#include "subtask_p.h"
#include "container_p.h"
#include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/container.h>
#include <moveit_task_constructor/debug.h>
#include <ros/ros.h>
@ -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<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");
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<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(){
subtasks_.clear();
scene_.reset();
planner_.reset();
}
void Task::clear(){
subtasks_.clear();
void Task::add(std::unique_ptr<SubTask> &&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<SubTrajectory*> 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;
}

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)
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;
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->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<subtasks::CartesianPositionMotion>("lift object");
auto move= std::make_unique<subtasks::CartesianPositionMotion>("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();

View File

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

View File

@ -36,7 +36,7 @@ int main(int argc, char** argv){
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->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();

View File

@ -14,24 +14,24 @@ int main(int argc, char** argv){
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->setTo("closed");
t.add( gripper );
t.add(std::move( gripper ));
}
t.plan();
/*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->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();
*/
return 0;