mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
6a1eacb315
commit
bb06eda33c
@ -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)
|
||||
|
||||
44
include/moveit_task_constructor/container.h
Normal file
44
include/moveit_task_constructor/container.h
Normal 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);
|
||||
};
|
||||
|
||||
} }
|
||||
@ -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 ¬ify);
|
||||
// add a new InterfaceState, connect the trajectory (either incoming or outgoing) to the newly created state
|
||||
// and finally run the notify callback
|
||||
container_type::iterator add(const planning_scene::PlanningSceneConstPtr& ps, SubTrajectory* incoming, SubTrajectory* outgoing);
|
||||
|
||||
using container_type::value_type;
|
||||
using container_type::reference;
|
||||
using container_type::const_reference;
|
||||
|
||||
using container_type::iterator;
|
||||
using container_type::const_iterator;
|
||||
using container_type::reverse_iterator;
|
||||
using container_type::const_reverse_iterator;
|
||||
|
||||
using container_type::empty;
|
||||
using container_type::size;
|
||||
using container_type::clear;
|
||||
using container_type::front;
|
||||
using container_type::back;
|
||||
|
||||
using container_type::begin;
|
||||
using container_type::cbegin;
|
||||
using container_type::end;
|
||||
using container_type::cend;
|
||||
using container_type::rbegin;
|
||||
using container_type::crbegin;
|
||||
using container_type::rend;
|
||||
using container_type::crend;
|
||||
|
||||
private:
|
||||
const NotifyFunction notify_;
|
||||
};
|
||||
|
||||
|
||||
struct SubTrajectory {
|
||||
SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj)
|
||||
: trajectory(traj),
|
||||
@ -55,19 +102,20 @@ struct SubTrajectory {
|
||||
cost(0)
|
||||
{}
|
||||
|
||||
void setBeginning(const InterfaceState& state){
|
||||
inline void setStartState(const InterfaceState& state){
|
||||
assert(begin == NULL);
|
||||
begin= &state;
|
||||
state.next_trajectories_.push_back(this);
|
||||
state.outgoing_trajectories_.push_back(this);
|
||||
}
|
||||
|
||||
void setEnding(const InterfaceState& state){
|
||||
inline void setEndState(const InterfaceState& state){
|
||||
assert(end == NULL);
|
||||
end= &state;
|
||||
state.previous_trajectories_.push_back(this);
|
||||
state.incoming_trajectories_.push_back(this);
|
||||
}
|
||||
|
||||
robot_trajectory::RobotTrajectoryPtr trajectory;
|
||||
// TODO: trajectories could have a name, e.g. a generator could name its solutions
|
||||
const robot_trajectory::RobotTrajectoryPtr trajectory;
|
||||
const InterfaceState* begin;
|
||||
const InterfaceState* end;
|
||||
double cost;
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
} }
|
||||
|
||||
58
include/moveit_task_constructor/utils.h
Normal file
58
include/moveit_task_constructor/utils.h
Normal 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;
|
||||
@ -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
157
src/container.cpp
Normal 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
57
src/container_p.h
Normal 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;
|
||||
};
|
||||
|
||||
|
||||
} }
|
||||
@ -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>
|
||||
|
||||
@ -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})
|
||||
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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
33
src/demo/test.cpp
Normal 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
22
src/storage.cpp
Normal file
@ -0,0 +1,22 @@
|
||||
#include <moveit_task_constructor/storage.h>
|
||||
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
Interface::Interface(const Interface::NotifyFunction ¬ify)
|
||||
: notify_(notify)
|
||||
{}
|
||||
|
||||
Interface::iterator Interface::add(const planning_scene::PlanningSceneConstPtr& ps, SubTrajectory* incoming, SubTrajectory* outgoing) {
|
||||
assert(bool(ps));
|
||||
assert(bool(incoming) ^ bool(outgoing)); // either incoming or outgoing is set
|
||||
emplace_back(InterfaceState(ps));
|
||||
iterator back = --end();
|
||||
// adjust subtrajectories ...
|
||||
if (incoming) incoming->setEndState(*back);
|
||||
if (outgoing) outgoing->setStartState(*back);
|
||||
// ... before calling notify callback
|
||||
if (notify_) notify_(back);
|
||||
return back;
|
||||
}
|
||||
|
||||
} }
|
||||
202
src/subtask.cpp
202
src/subtask.cpp
@ -1,39 +1,71 @@
|
||||
#include "subtask_p.h"
|
||||
|
||||
// get correctly casted private impl pointer
|
||||
#define I(Class) Class##Private * const impl = reinterpret_cast<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,32 +120,33 @@ 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;
|
||||
}
|
||||
@ -132,87 +155,94 @@ void PropagatingAnyWay::sendBackward(const robot_trajectory::RobotTrajectoryPtr&
|
||||
const planning_scene::PlanningSceneConstPtr& from,
|
||||
const InterfaceState& to,
|
||||
double cost){
|
||||
SubTrajectory& trajectory = impl_->addTrajectory(t, cost);
|
||||
trajectory.setEnding(to);
|
||||
impl_->sendBackward(trajectory, from);
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
128
src/subtask_p.h
128
src/subtask_p.h
@ -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()
|
||||
|
||||
14
src/subtasks/CMakeLists.txt
Normal file
14
src/subtasks/CMakeLists.txt
Normal 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})
|
||||
@ -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();
|
||||
|
||||
@ -246,7 +247,7 @@ 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.model_id= scene()->getRobotModel()->getName();
|
||||
dt.trajectory.resize(1);
|
||||
trajectory.getRobotTrajectoryMsg(dt.trajectory[0]);
|
||||
dt.model_id= start.getRobotModel()->getName();
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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");
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
105
src/task.cpp
105
src/task.cpp
@ -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,13 +13,28 @@
|
||||
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
Task::Task(){
|
||||
class TaskPrivate : public SerialContainerPrivate {
|
||||
friend class Task;
|
||||
robot_model_loader::RobotModelLoaderPtr rml_;
|
||||
|
||||
public:
|
||||
TaskPrivate(Task* me, const std::string &name)
|
||||
: SerialContainerPrivate(me, name)
|
||||
{
|
||||
initModel();
|
||||
initScene();
|
||||
initPlanner();
|
||||
}
|
||||
|
||||
void initModel () {
|
||||
rml_.reset(new robot_model_loader::RobotModelLoader);
|
||||
if( !rml_->getModel() )
|
||||
throw Exception("Task failed to construct RobotModel");
|
||||
}
|
||||
void initScene() {
|
||||
assert(rml_);
|
||||
|
||||
ros::NodeHandle h;
|
||||
|
||||
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
|
||||
client.waitForExistence();
|
||||
|
||||
@ -40,69 +57,52 @@ Task::Task(){
|
||||
throw Exception("Task failed to acquire current PlanningScene");
|
||||
}
|
||||
|
||||
scene_.reset(new planning_scene::PlanningScene(rml_->getModel()));
|
||||
scene_->setPlanningSceneMsg(res.scene);
|
||||
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"));
|
||||
}
|
||||
};
|
||||
|
||||
planner_.reset(new 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;
|
||||
}
|
||||
|
||||
@ -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
135
src/test/container.cpp
Normal 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);
|
||||
}
|
||||
|
||||
} }
|
||||
@ -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();
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user