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}
|
${catkin_INCLUDE_DIRS}
|
||||||
)
|
)
|
||||||
|
|
||||||
add_compile_options(-std=c++14)
|
set(CMAKE_CXX_STANDARD 14)
|
||||||
|
|
||||||
add_library(${PROJECT_NAME}_subtasks
|
|
||||||
src/subtasks/move.cpp
|
|
||||||
src/subtasks/current_state.cpp
|
|
||||||
src/subtasks/gripper.cpp
|
|
||||||
src/subtasks/generate_grasp_pose.cpp
|
|
||||||
src/subtasks/cartesian_position_motion.cpp
|
|
||||||
|
|
||||||
include/${PROJECT_NAME}/subtasks/move.h
|
|
||||||
include/${PROJECT_NAME}/subtasks/current_state.h
|
|
||||||
include/${PROJECT_NAME}/subtasks/gripper.h
|
|
||||||
include/${PROJECT_NAME}/subtasks/generate_grasp_pose.h
|
|
||||||
include/${PROJECT_NAME}/subtasks/cartesian_position_motion.h
|
|
||||||
)
|
|
||||||
target_link_libraries(${PROJECT_NAME}_subtasks ${catkin_LIBRARIES})
|
|
||||||
|
|
||||||
add_library(${PROJECT_NAME}
|
add_library(${PROJECT_NAME}
|
||||||
|
src/storage.cpp
|
||||||
src/subtask.cpp
|
src/subtask.cpp
|
||||||
|
src/container.cpp
|
||||||
src/task.cpp
|
src/task.cpp
|
||||||
src/debug.cpp
|
src/debug.cpp
|
||||||
|
|
||||||
src/subtask_p.h
|
src/subtask_p.h
|
||||||
|
include/${PROJECT_NAME}/utils.h
|
||||||
include/${PROJECT_NAME}/storage.h
|
include/${PROJECT_NAME}/storage.h
|
||||||
include/${PROJECT_NAME}/subtask.h
|
include/${PROJECT_NAME}/subtask.h
|
||||||
|
include/${PROJECT_NAME}/container.h
|
||||||
include/${PROJECT_NAME}/task.h
|
include/${PROJECT_NAME}/task.h
|
||||||
include/${PROJECT_NAME}/debug.h
|
include/${PROJECT_NAME}/debug.h
|
||||||
)
|
)
|
||||||
|
|
||||||
|
add_subdirectory(src/subtasks)
|
||||||
add_subdirectory(src/demo)
|
add_subdirectory(src/demo)
|
||||||
|
|
||||||
add_subdirectory(src/test)
|
add_subdirectory(src/test)
|
||||||
|
|||||||
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 <moveit/macros/class_forward.h>
|
||||||
|
|
||||||
#include <vector>
|
#include <deque>
|
||||||
|
#include <list>
|
||||||
#include <cassert>
|
#include <cassert>
|
||||||
|
#include <functional>
|
||||||
|
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene)
|
MOVEIT_CLASS_FORWARD(PlanningScene)
|
||||||
@ -19,34 +21,79 @@ namespace moveit { namespace task_constructor {
|
|||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(SubTrajectory)
|
MOVEIT_CLASS_FORWARD(SubTrajectory)
|
||||||
MOVEIT_CLASS_FORWARD(InterfaceState)
|
MOVEIT_CLASS_FORWARD(InterfaceState)
|
||||||
|
MOVEIT_CLASS_FORWARD(Interface)
|
||||||
|
MOVEIT_CLASS_FORWARD(SubTask)
|
||||||
|
|
||||||
|
|
||||||
|
/** InterfaceState describes a potential start or goal state for a planning stage.
|
||||||
|
*
|
||||||
|
* A start or goal state for planning is essentially defined by the state of a planning scene.
|
||||||
|
*/
|
||||||
class InterfaceState {
|
class InterfaceState {
|
||||||
friend class SubTrajectory;
|
friend class SubTrajectory;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
typedef std::vector<SubTrajectory*> SubTrajectoryList;
|
typedef std::deque<SubTrajectory*> SubTrajectoryList;
|
||||||
|
|
||||||
InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next)
|
InterfaceState(const planning_scene::PlanningSceneConstPtr& ps)
|
||||||
: state(ps)
|
: state(ps)
|
||||||
{
|
{}
|
||||||
if( previous )
|
|
||||||
previous_trajectories_.push_back(previous);
|
|
||||||
if( next )
|
|
||||||
next_trajectories_.push_back(next);
|
|
||||||
}
|
|
||||||
|
|
||||||
inline const SubTrajectoryList& previousTrajectories() const { return previous_trajectories_; }
|
inline const SubTrajectoryList& incomingTrajectories() const { return incoming_trajectories_; }
|
||||||
inline const SubTrajectoryList& nextTrajectories() const { return next_trajectories_; }
|
inline const SubTrajectoryList& outgoingTrajectories() const { return outgoing_trajectories_; }
|
||||||
|
|
||||||
public:
|
public:
|
||||||
planning_scene::PlanningSceneConstPtr state;
|
planning_scene::PlanningSceneConstPtr state;
|
||||||
double cost; // minimal costs
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
mutable SubTrajectoryList previous_trajectories_;
|
mutable SubTrajectoryList incoming_trajectories_;
|
||||||
mutable SubTrajectoryList next_trajectories_;
|
mutable SubTrajectoryList outgoing_trajectories_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
/** Interface provides a list of InterfaceStates available as input for a stage.
|
||||||
|
*
|
||||||
|
* This is essentially an adaptor to a container class, to allow for notification
|
||||||
|
* of the interface's owner when new states become available
|
||||||
|
*/
|
||||||
|
class Interface : protected std::list<InterfaceState> {
|
||||||
|
public:
|
||||||
|
typedef std::list<InterfaceState> container_type;
|
||||||
|
typedef std::function<void(const container_type::iterator&)> NotifyFunction;
|
||||||
|
Interface(const NotifyFunction ¬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 {
|
struct SubTrajectory {
|
||||||
SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj)
|
SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj)
|
||||||
: trajectory(traj),
|
: trajectory(traj),
|
||||||
@ -55,19 +102,20 @@ struct SubTrajectory {
|
|||||||
cost(0)
|
cost(0)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
void setBeginning(const InterfaceState& state){
|
inline void setStartState(const InterfaceState& state){
|
||||||
assert(begin == NULL);
|
assert(begin == NULL);
|
||||||
begin= &state;
|
begin= &state;
|
||||||
state.next_trajectories_.push_back(this);
|
state.outgoing_trajectories_.push_back(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
void setEnding(const InterfaceState& state){
|
inline void setEndState(const InterfaceState& state){
|
||||||
assert(end == NULL);
|
assert(end == NULL);
|
||||||
end= &state;
|
end= &state;
|
||||||
state.previous_trajectories_.push_back(this);
|
state.incoming_trajectories_.push_back(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryPtr trajectory;
|
// TODO: trajectories could have a name, e.g. a generator could name its solutions
|
||||||
|
const robot_trajectory::RobotTrajectoryPtr trajectory;
|
||||||
const InterfaceState* begin;
|
const InterfaceState* begin;
|
||||||
const InterfaceState* end;
|
const InterfaceState* end;
|
||||||
double cost;
|
double cost;
|
||||||
|
|||||||
@ -1,15 +1,17 @@
|
|||||||
// copyright Michael 'v4hn' Goerner @ 2017
|
// copyright Michael 'v4hn' Goerner @ 2017
|
||||||
|
// copyright Robert Haschke @ 2017
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <moveit_task_constructor/storage.h>
|
#include "utils.h"
|
||||||
|
|
||||||
#include <moveit/macros/class_forward.h>
|
#include <moveit/macros/class_forward.h>
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
|
||||||
|
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include <list>
|
#include <list>
|
||||||
#include <tuple>
|
|
||||||
|
#define PRIVATE_CLASS(Class) \
|
||||||
|
friend class Class##Private; \
|
||||||
|
Class##Private* pimpl_func(); \
|
||||||
|
const Class##Private* pimpl_func() const;
|
||||||
|
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene)
|
MOVEIT_CLASS_FORWARD(PlanningScene)
|
||||||
@ -19,63 +21,84 @@ namespace planning_pipeline {
|
|||||||
MOVEIT_CLASS_FORWARD(PlanningPipeline)
|
MOVEIT_CLASS_FORWARD(PlanningPipeline)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
namespace robot_trajectory {
|
||||||
|
MOVEIT_CLASS_FORWARD(RobotTrajectory)
|
||||||
|
}
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
typedef std::list<InterfaceState> InterfaceStateList;
|
MOVEIT_CLASS_FORWARD(Interface)
|
||||||
typedef std::pair<InterfaceState&, InterfaceState&> InterfaceStatePair;
|
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(SubTask)
|
MOVEIT_CLASS_FORWARD(SubTask)
|
||||||
typedef std::weak_ptr<SubTask> SubTaskWeakPtr;
|
MOVEIT_CLASS_FORWARD(SubTrajectory)
|
||||||
|
class InterfaceState;
|
||||||
|
typedef std::pair<const InterfaceState&, const InterfaceState&> InterfaceStatePair;
|
||||||
|
|
||||||
class Task;
|
|
||||||
|
class ContainerBasePrivate;
|
||||||
class SubTaskPrivate;
|
class SubTaskPrivate;
|
||||||
class SubTask {
|
class SubTask {
|
||||||
friend class SubTaskPrivate; // allow access to impl_
|
friend std::ostream& operator<<(std::ostream &os, const SubTask& stage);
|
||||||
friend class Task; // TODO: remove when we have SubTaskContainers
|
friend class SubTaskPrivate;
|
||||||
|
friend class ContainerBasePrivate;
|
||||||
protected:
|
|
||||||
SubTaskPrivate* const impl_;
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
enum PropagationType { FORWARD, BACKWARD, ANYWAY, GENERATOR, CONNECTING };
|
inline const SubTaskPrivate* pimpl_func() const { return pimpl_; }
|
||||||
|
inline SubTaskPrivate* pimpl_func() { return pimpl_; }
|
||||||
|
|
||||||
|
enum InterfaceFlag {
|
||||||
|
READS_INPUT = 0x01,
|
||||||
|
READS_OUTPUT = 0x02,
|
||||||
|
READS_MASK = READS_INPUT | READS_OUTPUT,
|
||||||
|
WRITES_NEXT_INPUT = 0x04,
|
||||||
|
WRITES_PREV_OUTPUT = 0x08,
|
||||||
|
WRITES_UNKNOWN = 0x10,
|
||||||
|
};
|
||||||
|
typedef Flags<InterfaceFlag> InterfaceFlags;
|
||||||
|
InterfaceFlags interfaceFlags() const;
|
||||||
|
|
||||||
~SubTask();
|
~SubTask();
|
||||||
const std::string& getName() const;
|
const std::string& getName() const;
|
||||||
|
|
||||||
virtual bool canCompute() = 0;
|
|
||||||
|
// TODO: results from { TIMEOUT, EXHAUSTED, FINISHED, WAITING }
|
||||||
|
virtual bool canCompute() const = 0;
|
||||||
virtual bool compute() = 0;
|
virtual bool compute() = 0;
|
||||||
|
|
||||||
void setPlanningScene(planning_scene::PlanningSceneConstPtr);
|
planning_scene::PlanningSceneConstPtr scene() const;
|
||||||
void setPlanningPipeline(planning_pipeline::PlanningPipelinePtr);
|
planning_pipeline::PlanningPipelinePtr planner() const;
|
||||||
|
void setPlanningScene(const planning_scene::PlanningSceneConstPtr&);
|
||||||
|
void setPlanningPipeline(const planning_pipeline::PlanningPipelinePtr&);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
/// can only instantiated by derived classes
|
/// can only instantiated by derived classes
|
||||||
SubTask(SubTaskPrivate *impl);
|
SubTask(SubTaskPrivate *impl);
|
||||||
|
|
||||||
/// methods called when a new InterfaceState was spawned
|
/// methods called when a new InterfaceState was spawned
|
||||||
virtual inline void newBeginning(const InterfaceStateList::iterator& it) {}
|
virtual void newInputState(const std::list<InterfaceState>::iterator&) {}
|
||||||
virtual inline void newEnd(const InterfaceStateList::iterator& it) {}
|
virtual void newOutputState(const std::list<InterfaceState>::iterator&) {}
|
||||||
|
|
||||||
planning_scene::PlanningSceneConstPtr scene_;
|
protected:
|
||||||
planning_pipeline::PlanningPipelinePtr planner_;
|
// TODO: use unique_ptr<SubTaskPrivate> and get rid of destructor
|
||||||
|
SubTaskPrivate* const pimpl_; // constness guarantees one initial write
|
||||||
};
|
};
|
||||||
|
std::ostream& operator<<(std::ostream &os, const SubTask& stage);
|
||||||
|
|
||||||
|
|
||||||
class PropagatingAnyWayPrivate;
|
class PropagatingAnyWayPrivate;
|
||||||
class PropagatingAnyWay : public SubTask {
|
class PropagatingAnyWay : public SubTask {
|
||||||
public:
|
public:
|
||||||
|
PRIVATE_CLASS(PropagatingAnyWay)
|
||||||
PropagatingAnyWay(const std::string& name);
|
PropagatingAnyWay(const std::string& name);
|
||||||
static inline constexpr PropagationType type() { return ANYWAY; }
|
|
||||||
|
|
||||||
bool hasBeginning() const;
|
bool hasBeginning() const;
|
||||||
InterfaceState& fetchStateBeginning();
|
const InterfaceState &fetchStateBeginning();
|
||||||
void sendForward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
|
void sendForward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
|
||||||
const InterfaceState& from,
|
const InterfaceState& from,
|
||||||
const planning_scene::PlanningSceneConstPtr& to,
|
const planning_scene::PlanningSceneConstPtr& to,
|
||||||
double cost = 0);
|
double cost = 0);
|
||||||
|
|
||||||
bool hasEnding() const;
|
bool hasEnding() const;
|
||||||
InterfaceState& fetchStateEnding();
|
const InterfaceState &fetchStateEnding();
|
||||||
void sendBackward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
|
void sendBackward(const robot_trajectory::RobotTrajectoryPtr& trajectory,
|
||||||
const planning_scene::PlanningSceneConstPtr& from,
|
const planning_scene::PlanningSceneConstPtr& from,
|
||||||
const InterfaceState& to,
|
const InterfaceState& to,
|
||||||
@ -83,65 +106,65 @@ public:
|
|||||||
|
|
||||||
protected:
|
protected:
|
||||||
// constructor for use in derived classes
|
// constructor for use in derived classes
|
||||||
inline PropagatingAnyWay(PropagatingAnyWayPrivate* impl);
|
PropagatingAnyWay(PropagatingAnyWayPrivate* impl);
|
||||||
|
|
||||||
// get informed when new beginnings and endings become available
|
// get informed when new beginnings and endings become available
|
||||||
void newBeginning(const InterfaceStateList::iterator& it);
|
void newInputState(const std::list<InterfaceState>::iterator& it);
|
||||||
void newEnd(const InterfaceStateList::iterator& it);
|
void newOutputState(const std::list<InterfaceState>::iterator& it);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class PropagatingForwardPrivate;
|
||||||
class PropagatingForward : public PropagatingAnyWay {
|
class PropagatingForward : public PropagatingAnyWay {
|
||||||
public:
|
public:
|
||||||
|
PRIVATE_CLASS(PropagatingForward)
|
||||||
PropagatingForward(const std::string& name);
|
PropagatingForward(const std::string& name);
|
||||||
static inline constexpr PropagationType type() { return FORWARD; }
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// restrict access to backward methods
|
// restrict access to backward methods
|
||||||
using PropagatingAnyWay::hasEnding;
|
using PropagatingAnyWay::hasEnding;
|
||||||
using PropagatingAnyWay::fetchStateEnding;
|
using PropagatingAnyWay::fetchStateEnding;
|
||||||
using PropagatingAnyWay::sendBackward;
|
using PropagatingAnyWay::sendBackward;
|
||||||
// don't care about new endings
|
|
||||||
inline void newEnd(const InterfaceStateList::iterator& it) {}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class PropagatingBackwardPrivate;
|
||||||
class PropagatingBackward : public PropagatingAnyWay {
|
class PropagatingBackward : public PropagatingAnyWay {
|
||||||
public:
|
public:
|
||||||
|
PRIVATE_CLASS(PropagatingBackward)
|
||||||
PropagatingBackward(const std::string& name);
|
PropagatingBackward(const std::string& name);
|
||||||
static inline constexpr PropagationType type() { return BACKWARD; }
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// restrict access to forward methods
|
// restrict access to forward methods
|
||||||
using PropagatingAnyWay::hasBeginning;
|
using PropagatingAnyWay::hasBeginning;
|
||||||
using PropagatingAnyWay::fetchStateBeginning;
|
using PropagatingAnyWay::fetchStateBeginning;
|
||||||
using PropagatingAnyWay::sendForward;
|
using PropagatingAnyWay::sendForward;
|
||||||
// don't care about new beginnings
|
|
||||||
inline void newBeginning(const InterfaceStateList::iterator& it) {}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class GeneratorPrivate;
|
||||||
class Generator : public SubTask {
|
class Generator : public SubTask {
|
||||||
public:
|
public:
|
||||||
|
PRIVATE_CLASS(Generator)
|
||||||
Generator(const std::string& name);
|
Generator(const std::string& name);
|
||||||
static inline constexpr PropagationType type() { return GENERATOR; }
|
|
||||||
|
|
||||||
void spawn(const planning_scene::PlanningSceneConstPtr &ps, double cost = 0);
|
void spawn(const planning_scene::PlanningSceneConstPtr &ps, double cost = 0);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class ConnectingPrivate;
|
||||||
class Connecting : public SubTask {
|
class Connecting : public SubTask {
|
||||||
public:
|
public:
|
||||||
|
PRIVATE_CLASS(Connecting)
|
||||||
Connecting(const std::string& name);
|
Connecting(const std::string& name);
|
||||||
static inline constexpr PropagationType type() { return CONNECTING; }
|
|
||||||
|
|
||||||
bool hasStatePair() const;
|
bool hasStatePair() const;
|
||||||
InterfaceStatePair fetchStatePair();
|
InterfaceStatePair fetchStatePair();
|
||||||
void connect(const robot_trajectory::RobotTrajectoryPtr&, const InterfaceStatePair&, double cost = 0);
|
void connect(const robot_trajectory::RobotTrajectoryPtr&, const InterfaceStatePair&, double cost = 0);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
virtual void newBeginning(const InterfaceStateList::iterator& it);
|
virtual void newInputState(const std::list<InterfaceState>::iterator& it);
|
||||||
virtual void newEnd(const InterfaceStateList::iterator& it);
|
virtual void newOutputState(const std::list<InterfaceState>::iterator& it);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -9,9 +9,10 @@
|
|||||||
#include <geometry_msgs/Vector3Stamped.h>
|
#include <geometry_msgs/Vector3Stamped.h>
|
||||||
#include <geometry_msgs/PointStamped.h>
|
#include <geometry_msgs/PointStamped.h>
|
||||||
|
|
||||||
namespace moveit { namespace planning_interface {
|
namespace moveit {
|
||||||
MOVEIT_CLASS_FORWARD(MoveGroupInterface);
|
namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface) }
|
||||||
} }
|
namespace core { MOVEIT_CLASS_FORWARD(RobotState) }
|
||||||
|
}
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
@ -19,9 +20,8 @@ class CartesianPositionMotion : public PropagatingAnyWay {
|
|||||||
public:
|
public:
|
||||||
CartesianPositionMotion(std::string name);
|
CartesianPositionMotion(std::string name);
|
||||||
|
|
||||||
virtual bool canCompute();
|
virtual bool canCompute() const override;
|
||||||
|
virtual bool compute() override;
|
||||||
virtual bool compute();
|
|
||||||
|
|
||||||
void setGroup(std::string group);
|
void setGroup(std::string group);
|
||||||
void setLink(std::string link);
|
void setLink(std::string link);
|
||||||
|
|||||||
@ -10,9 +10,8 @@ class CurrentState : public Generator {
|
|||||||
public:
|
public:
|
||||||
CurrentState(std::string name);
|
CurrentState(std::string name);
|
||||||
|
|
||||||
virtual bool canCompute();
|
virtual bool canCompute() const override;
|
||||||
|
virtual bool compute() override;
|
||||||
virtual bool compute();
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
bool ran_;
|
bool ran_;
|
||||||
|
|||||||
@ -18,9 +18,8 @@ class GenerateGraspPose : public Generator {
|
|||||||
public:
|
public:
|
||||||
GenerateGraspPose(std::string name);
|
GenerateGraspPose(std::string name);
|
||||||
|
|
||||||
virtual bool canCompute();
|
virtual bool canCompute() const override;
|
||||||
|
virtual bool compute() override;
|
||||||
virtual bool compute();
|
|
||||||
|
|
||||||
void setEndEffector(std::string eef);
|
void setEndEffector(std::string eef);
|
||||||
|
|
||||||
|
|||||||
@ -14,9 +14,8 @@ class Gripper : public PropagatingForward {
|
|||||||
public:
|
public:
|
||||||
Gripper(std::string name);
|
Gripper(std::string name);
|
||||||
|
|
||||||
virtual bool canCompute();
|
virtual bool canCompute() const override;
|
||||||
|
virtual bool compute() override;
|
||||||
virtual bool compute();
|
|
||||||
|
|
||||||
void setEndEffector(std::string eef);
|
void setEndEffector(std::string eef);
|
||||||
|
|
||||||
|
|||||||
@ -4,9 +4,9 @@
|
|||||||
|
|
||||||
#include <moveit_task_constructor/subtask.h>
|
#include <moveit_task_constructor/subtask.h>
|
||||||
|
|
||||||
namespace moveit { namespace planning_interface {
|
namespace moveit {
|
||||||
MOVEIT_CLASS_FORWARD(MoveGroupInterface)
|
namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface)}
|
||||||
} }
|
}
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace subtasks {
|
namespace moveit { namespace task_constructor { namespace subtasks {
|
||||||
|
|
||||||
@ -14,9 +14,8 @@ class Move : public Connecting {
|
|||||||
public:
|
public:
|
||||||
Move(std::string name);
|
Move(std::string name);
|
||||||
|
|
||||||
virtual bool canCompute();
|
virtual bool canCompute() const override;
|
||||||
|
virtual bool compute() override;
|
||||||
virtual bool compute();
|
|
||||||
|
|
||||||
void setGroup(std::string group);
|
void setGroup(std::string group);
|
||||||
void setLink(std::string link);
|
void setLink(std::string link);
|
||||||
|
|||||||
@ -2,7 +2,7 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <moveit_task_constructor/storage.h>
|
#include "container.h"
|
||||||
|
|
||||||
#include <moveit/macros/class_forward.h>
|
#include <moveit/macros/class_forward.h>
|
||||||
|
|
||||||
@ -10,18 +10,6 @@
|
|||||||
|
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
namespace planning_scene {
|
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene)
|
|
||||||
}
|
|
||||||
|
|
||||||
namespace robot_model_loader {
|
|
||||||
MOVEIT_CLASS_FORWARD(RobotModelLoader)
|
|
||||||
}
|
|
||||||
|
|
||||||
namespace planning_pipeline {
|
|
||||||
MOVEIT_CLASS_FORWARD(PlanningPipeline)
|
|
||||||
}
|
|
||||||
|
|
||||||
namespace moveit { namespace core {
|
namespace moveit { namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotState)
|
MOVEIT_CLASS_FORWARD(RobotState)
|
||||||
}}
|
}}
|
||||||
@ -29,35 +17,26 @@ namespace moveit { namespace core {
|
|||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(SubTask)
|
MOVEIT_CLASS_FORWARD(SubTask)
|
||||||
|
MOVEIT_CLASS_FORWARD(ContainerBase)
|
||||||
MOVEIT_CLASS_FORWARD(Task)
|
MOVEIT_CLASS_FORWARD(Task)
|
||||||
|
|
||||||
class Task {
|
class TaskPrivate;
|
||||||
|
class Task : protected SerialContainer {
|
||||||
|
PRIVATE_CLASS(Task)
|
||||||
public:
|
public:
|
||||||
typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback;
|
typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback;
|
||||||
|
|
||||||
Task();
|
Task(const std::string &name = std::string());
|
||||||
~Task();
|
|
||||||
|
|
||||||
void add( SubTaskPtr );
|
void add(std::unique_ptr<SubTask> &&stage);
|
||||||
|
using SerialContainer::clear;
|
||||||
|
|
||||||
bool plan();
|
bool plan();
|
||||||
|
|
||||||
bool processSolutions(const SolutionCallback &processor);
|
|
||||||
bool processSolutions(const SolutionCallback &processor) const;
|
|
||||||
|
|
||||||
const moveit::core::RobotState &getCurrentRobotState() const;
|
|
||||||
|
|
||||||
void printState();
|
void printState();
|
||||||
|
|
||||||
void clear();
|
const moveit::core::RobotState &getCurrentRobotState() const;
|
||||||
|
bool processSolutions(const SolutionCallback &processor);
|
||||||
protected:
|
bool processSolutions(const SolutionCallback &processor) const;
|
||||||
std::vector<SubTaskPtr> subtasks_;
|
|
||||||
|
|
||||||
planning_scene::PlanningScenePtr scene_;
|
|
||||||
robot_model_loader::RobotModelLoaderPtr rml_;
|
|
||||||
|
|
||||||
planning_pipeline::PlanningPipelinePtr planner_;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
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>
|
<build_depend>moveit_core</build_depend>
|
||||||
<run_depend>moveit_core</run_depend>
|
<run_depend>moveit_core</run_depend>
|
||||||
|
<test_depend>rosunit</test_depend>
|
||||||
</package>
|
</package>
|
||||||
|
|||||||
157
src/container.cpp
Normal file
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/debug.h>
|
||||||
#include <moveit_task_constructor/task.h>
|
#include <moveit_task_constructor/task.h>
|
||||||
|
#include <moveit_task_constructor/storage.h>
|
||||||
|
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||||
|
|||||||
@ -1,3 +1,6 @@
|
|||||||
|
add_executable(foo test.cpp)
|
||||||
|
target_link_libraries(foo)
|
||||||
|
|
||||||
add_executable(plan_pick_ur5 plan_pick_ur5.cpp)
|
add_executable(plan_pick_ur5 plan_pick_ur5.cpp)
|
||||||
target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_subtasks ${PROJECT_NAME})
|
target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_subtasks ${PROJECT_NAME})
|
||||||
|
|
||||||
|
|||||||
@ -43,26 +43,26 @@ int main(int argc, char** argv){
|
|||||||
|
|
||||||
Task t;
|
Task t;
|
||||||
|
|
||||||
t.add( std::make_shared<subtasks::CurrentState>("current state") );
|
t.add( std::make_unique<subtasks::CurrentState>("current state") );
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move= std::make_shared<subtasks::Gripper>("open gripper");
|
auto move= std::make_unique<subtasks::Gripper>("open gripper");
|
||||||
move->setEndEffector("left_gripper");
|
move->setEndEffector("left_gripper");
|
||||||
move->setTo("open");
|
move->setTo("open");
|
||||||
t.add(move);
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move= std::make_shared<subtasks::Move>("move to pre-grasp");
|
auto move= std::make_unique<subtasks::Move>("move to pre-grasp");
|
||||||
move->setGroup("left_arm");
|
move->setGroup("left_arm");
|
||||||
move->setLink("l_gripper_tool_frame");
|
move->setLink("l_gripper_tool_frame");
|
||||||
move->setPlannerId("RRTConnectkConfigDefault");
|
move->setPlannerId("RRTConnectkConfigDefault");
|
||||||
move->setTimeout(8.0);
|
move->setTimeout(8.0);
|
||||||
t.add(move);
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move= std::make_shared<subtasks::CartesianPositionMotion>("proceed to grasp pose");
|
auto move= std::make_unique<subtasks::CartesianPositionMotion>("proceed to grasp pose");
|
||||||
move->setGroup("left_arm");
|
move->setGroup("left_arm");
|
||||||
move->setLink("l_gripper_tool_frame");
|
move->setLink("l_gripper_tool_frame");
|
||||||
move->setMinMaxDistance(.03, 0.1);
|
move->setMinMaxDistance(.03, 0.1);
|
||||||
@ -71,11 +71,11 @@ int main(int argc, char** argv){
|
|||||||
geometry_msgs::PointStamped target;
|
geometry_msgs::PointStamped target;
|
||||||
target.header.frame_id= "object";
|
target.header.frame_id= "object";
|
||||||
move->towards(target);
|
move->towards(target);
|
||||||
t.add(move);
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto gengrasp= std::make_shared<subtasks::GenerateGraspPose>("generate grasp pose");
|
auto gengrasp= std::make_unique<subtasks::GenerateGraspPose>("generate grasp pose");
|
||||||
gengrasp->setEndEffector("left_gripper");
|
gengrasp->setEndEffector("left_gripper");
|
||||||
//gengrasp->setGroup("arm");
|
//gengrasp->setGroup("arm");
|
||||||
gengrasp->setLink("l_gripper_tool_frame");
|
gengrasp->setLink("l_gripper_tool_frame");
|
||||||
@ -83,19 +83,19 @@ int main(int argc, char** argv){
|
|||||||
gengrasp->setObject("object");
|
gengrasp->setObject("object");
|
||||||
gengrasp->setGraspOffset(.00);
|
gengrasp->setGraspOffset(.00);
|
||||||
gengrasp->setAngleDelta(.2);
|
gengrasp->setAngleDelta(.2);
|
||||||
t.add(gengrasp);
|
t.add(std::move(gengrasp));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move= std::make_shared<subtasks::Gripper>("grasp");
|
auto move= std::make_unique<subtasks::Gripper>("grasp");
|
||||||
move->setEndEffector("left_gripper");
|
move->setEndEffector("left_gripper");
|
||||||
move->setTo("closed");
|
move->setTo("closed");
|
||||||
move->graspObject("object");
|
move->graspObject("object");
|
||||||
t.add(move);
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move= std::make_shared<subtasks::CartesianPositionMotion>("lift object");
|
auto move= std::make_unique<subtasks::CartesianPositionMotion>("lift object");
|
||||||
move->setGroup("left_arm");
|
move->setGroup("left_arm");
|
||||||
move->setLink("l_gripper_tool_frame");
|
move->setLink("l_gripper_tool_frame");
|
||||||
move->setMinMaxDistance(0.03, 0.05);
|
move->setMinMaxDistance(0.03, 0.05);
|
||||||
@ -105,7 +105,7 @@ int main(int argc, char** argv){
|
|||||||
direction.header.frame_id= "base_link";
|
direction.header.frame_id= "base_link";
|
||||||
direction.vector.z= 1.0;
|
direction.vector.z= 1.0;
|
||||||
move->along(direction);
|
move->along(direction);
|
||||||
t.add(move);
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
t.plan();
|
t.plan();
|
||||||
|
|||||||
@ -41,25 +41,25 @@ int main(int argc, char** argv){
|
|||||||
|
|
||||||
Task t;
|
Task t;
|
||||||
|
|
||||||
t.add( std::make_shared<subtasks::CurrentState>("current state") );
|
t.add(std::make_unique<subtasks::CurrentState>("current state"));
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move= std::make_shared<subtasks::Gripper>("open gripper");
|
auto move = std::make_unique<subtasks::Gripper>("open gripper");
|
||||||
move->setEndEffector("gripper");
|
move->setEndEffector("gripper");
|
||||||
move->setTo("open");
|
move->setTo("open");
|
||||||
t.add(move);
|
t.add(std::move(std::move(move)));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move= std::make_shared<subtasks::Move>("move to pre-grasp");
|
auto move = std::make_unique<subtasks::Move>("move to pre-grasp");
|
||||||
move->setGroup("arm");
|
move->setGroup("arm");
|
||||||
move->setPlannerId("RRTConnectkConfigDefault");
|
move->setPlannerId("RRTConnectkConfigDefault");
|
||||||
move->setTimeout(8.0);
|
move->setTimeout(8.0);
|
||||||
t.add(move);
|
t.add(std::move(std::move(move)));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move= std::make_shared<subtasks::CartesianPositionMotion>("proceed to grasp pose");
|
auto move = std::make_unique<subtasks::CartesianPositionMotion>("proceed to grasp pose");
|
||||||
move->setGroup("arm");
|
move->setGroup("arm");
|
||||||
move->setLink("s_model_tool0");
|
move->setLink("s_model_tool0");
|
||||||
move->setMinMaxDistance(.03, 0.1);
|
move->setMinMaxDistance(.03, 0.1);
|
||||||
@ -68,11 +68,11 @@ int main(int argc, char** argv){
|
|||||||
geometry_msgs::PointStamped target;
|
geometry_msgs::PointStamped target;
|
||||||
target.header.frame_id= "object";
|
target.header.frame_id= "object";
|
||||||
move->towards(target);
|
move->towards(target);
|
||||||
t.add(move);
|
t.add(std::move(std::move(move)));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto gengrasp= std::make_shared<subtasks::GenerateGraspPose>("generate grasp pose");
|
auto gengrasp = std::make_unique<subtasks::GenerateGraspPose>("generate grasp pose");
|
||||||
gengrasp->setEndEffector("gripper");
|
gengrasp->setEndEffector("gripper");
|
||||||
//gengrasp->setGroup("arm");
|
//gengrasp->setGroup("arm");
|
||||||
gengrasp->setGripperGraspPose("open");
|
gengrasp->setGripperGraspPose("open");
|
||||||
@ -80,19 +80,19 @@ int main(int argc, char** argv){
|
|||||||
gengrasp->setGraspOffset(.03);
|
gengrasp->setGraspOffset(.03);
|
||||||
gengrasp->setAngleDelta(-.2);
|
gengrasp->setAngleDelta(-.2);
|
||||||
gengrasp->setMaxIKSolutions(8);
|
gengrasp->setMaxIKSolutions(8);
|
||||||
t.add(gengrasp);
|
t.add(std::move(std::move(gengrasp)));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move= std::make_shared<subtasks::Gripper>("grasp");
|
auto move = std::make_unique<subtasks::Gripper>("grasp");
|
||||||
move->setEndEffector("gripper");
|
move->setEndEffector("gripper");
|
||||||
move->setTo("closed");
|
move->setTo("closed");
|
||||||
move->graspObject("object");
|
move->graspObject("object");
|
||||||
t.add(move);
|
t.add(std::move(std::move(move)));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move= std::make_shared<subtasks::CartesianPositionMotion>("lift object");
|
auto move = std::make_unique<subtasks::CartesianPositionMotion>("lift object");
|
||||||
move->setGroup("arm");
|
move->setGroup("arm");
|
||||||
move->setLink("s_model_tool0");
|
move->setLink("s_model_tool0");
|
||||||
move->setMinMaxDistance(0.03, 0.05);
|
move->setMinMaxDistance(0.03, 0.05);
|
||||||
@ -102,7 +102,7 @@ int main(int argc, char** argv){
|
|||||||
direction.header.frame_id= "world";
|
direction.header.frame_id= "world";
|
||||||
direction.vector.z= 1.0;
|
direction.vector.z= 1.0;
|
||||||
move->along(direction);
|
move->along(direction);
|
||||||
t.add(move);
|
t.add(std::move(std::move(move)));
|
||||||
}
|
}
|
||||||
|
|
||||||
t.plan();
|
t.plan();
|
||||||
|
|||||||
33
src/demo/test.cpp
Normal file
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"
|
#include "subtask_p.h"
|
||||||
|
#include <iostream>
|
||||||
// get correctly casted private impl pointer
|
#include <iomanip>
|
||||||
#define I(Class) Class##Private * const impl = reinterpret_cast<Class##Private*>(impl_)
|
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
SubTask::SubTask(SubTaskPrivate *impl)
|
SubTask::SubTask(SubTaskPrivate *impl)
|
||||||
: impl_(impl)
|
: pimpl_(impl)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
SubTask::InterfaceFlags SubTask::interfaceFlags() const
|
||||||
|
{
|
||||||
|
SubTask::InterfaceFlags result = pimpl_->interfaceFlags();
|
||||||
|
result &= ~SubTask::InterfaceFlags(SubTask::READS_MASK);
|
||||||
|
result |= pimpl_->SubTaskPrivate::interfaceFlags();
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
SubTask::~SubTask()
|
SubTask::~SubTask()
|
||||||
{
|
{
|
||||||
delete impl_;
|
delete pimpl_;
|
||||||
}
|
}
|
||||||
|
|
||||||
const std::string& SubTask::getName() const {
|
const std::string& SubTask::getName() const {
|
||||||
return impl_->name_;
|
return pimpl_->name_;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SubTask::setPlanningScene(planning_scene::PlanningSceneConstPtr scene){
|
std::ostream& operator<<(std::ostream &os, const SubTask& stage) {
|
||||||
scene_= scene;
|
os << *stage.pimpl_func();
|
||||||
|
return os;
|
||||||
|
}
|
||||||
|
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
|
||||||
|
// inputs
|
||||||
|
for (const InterfacePtr &i : {stage.prevOutput(), stage.input_}) {
|
||||||
|
os << std::setw(3);
|
||||||
|
if (i) os << i->size();
|
||||||
|
else os << "-";
|
||||||
|
}
|
||||||
|
// trajectories
|
||||||
|
os << " -> " << std::setw(3) << stage.trajectories_.size() << " <- ";
|
||||||
|
// outputs
|
||||||
|
for (const InterfacePtr &i : {stage.output_, stage.nextInput()}) {
|
||||||
|
os << std::setw(3);
|
||||||
|
if (i) os << i->size();
|
||||||
|
else os << "-";
|
||||||
|
}
|
||||||
|
// name
|
||||||
|
os << " / " << stage.name_;
|
||||||
|
return os;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SubTask::setPlanningPipeline(planning_pipeline::PlanningPipelinePtr planner){
|
planning_scene::PlanningSceneConstPtr SubTask::scene() const
|
||||||
planner_= planner;
|
{
|
||||||
|
return pimpl_->scene_;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
planning_pipeline::PlanningPipelinePtr SubTask::planner() const
|
||||||
void SubTaskPrivate::addPredecessor(SubTaskPtr prev_task){
|
{
|
||||||
predecessor_= SubTaskWeakPtr(prev_task);
|
return pimpl_->planner_;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SubTaskPrivate::addSuccessor(SubTaskPtr next_task){
|
void SubTask::setPlanningScene(const planning_scene::PlanningSceneConstPtr &scene){
|
||||||
successor_= next_task;
|
pimpl_->scene_= scene;
|
||||||
|
}
|
||||||
|
|
||||||
|
void SubTask::setPlanningPipeline(const planning_pipeline::PlanningPipelinePtr &planner){
|
||||||
|
pimpl_->planner_= planner;
|
||||||
}
|
}
|
||||||
|
|
||||||
SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
|
SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
|
||||||
@ -41,37 +73,27 @@ SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajec
|
|||||||
return trajectories_.back();
|
return trajectories_.back();
|
||||||
}
|
}
|
||||||
|
|
||||||
void SubTaskPrivate::sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps){
|
SubTask::InterfaceFlags SubTaskPrivate::interfaceFlags() const
|
||||||
if( successor_ ){
|
{
|
||||||
std::cout << "sending state forward to successor" << std::endl;
|
// the base class provides the read flags
|
||||||
traj.end= successor_->impl_->newBeginning(ps, &traj);
|
SubTask::InterfaceFlags f;
|
||||||
}
|
if (input_) f |= SubTask::READS_INPUT;
|
||||||
|
if (output_) f |= SubTask::READS_OUTPUT;
|
||||||
|
return f;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SubTaskPrivate::sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps){
|
void SubTaskPrivate::sendForward(SubTrajectory& trajectory, const planning_scene::PlanningSceneConstPtr& ps){
|
||||||
if( !predecessor_.expired() ){
|
std::cout << "sending state to start" << std::endl;
|
||||||
std::cout << "sending state backward to predecessor" << std::endl;
|
nextInput()->add(ps, &trajectory, NULL);
|
||||||
traj.begin= predecessor_.lock()->impl_->newEnd(ps, &traj);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
InterfaceState* SubTaskPrivate::newBeginning(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_end){
|
void SubTaskPrivate::sendBackward(SubTrajectory& trajectory, const planning_scene::PlanningSceneConstPtr& ps){
|
||||||
assert( bool(ps) );
|
std::cout << "sending state to end" << std::endl;
|
||||||
beginnings_.push_back( InterfaceState(ps, old_end, NULL) );
|
prevOutput()->add(ps, NULL, &trajectory);
|
||||||
|
|
||||||
me_->newBeginning(--beginnings_.end());
|
|
||||||
return &beginnings_.back();
|
|
||||||
}
|
|
||||||
|
|
||||||
InterfaceState* SubTaskPrivate::newEnd(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_beginning){
|
|
||||||
assert( bool(ps) );
|
|
||||||
endings_.push_back( InterfaceState(ps, NULL, old_beginning) );
|
|
||||||
|
|
||||||
me_->newEnd(--endings_.end());
|
|
||||||
return &endings_.back();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
PRIVATE_CLASS_IMPL(PropagatingAnyWay)
|
||||||
PropagatingAnyWay::PropagatingAnyWay(const std::string &name)
|
PropagatingAnyWay::PropagatingAnyWay(const std::string &name)
|
||||||
: SubTask(new PropagatingAnyWayPrivate(this, name))
|
: SubTask(new PropagatingAnyWayPrivate(this, name))
|
||||||
{}
|
{}
|
||||||
@ -79,17 +101,17 @@ PropagatingAnyWay::PropagatingAnyWay(const std::string &name)
|
|||||||
PropagatingAnyWay::PropagatingAnyWay(PropagatingAnyWayPrivate *impl) : SubTask(impl) {}
|
PropagatingAnyWay::PropagatingAnyWay(PropagatingAnyWayPrivate *impl) : SubTask(impl) {}
|
||||||
|
|
||||||
bool PropagatingAnyWay::hasBeginning() const{
|
bool PropagatingAnyWay::hasBeginning() const{
|
||||||
I(PropagatingAnyWay);
|
IMPL(const PropagatingAnyWay);
|
||||||
return impl->it_beginnings_ != impl->beginnings_.end();
|
return impl->next_input_ != impl->input_->end();
|
||||||
}
|
}
|
||||||
|
|
||||||
InterfaceState& PropagatingAnyWay::fetchStateBeginning(){
|
const InterfaceState& PropagatingAnyWay::fetchStateBeginning(){
|
||||||
I(PropagatingAnyWay);
|
IMPL(PropagatingAnyWay);
|
||||||
if(impl->it_beginnings_ == impl->beginnings_.end())
|
if (!hasBeginning())
|
||||||
throw std::runtime_error("no new state for beginning available");
|
throw std::runtime_error("no new state for beginning available");
|
||||||
|
|
||||||
InterfaceState& state= *impl->it_beginnings_;
|
const InterfaceState& state= *impl->next_input_;
|
||||||
++impl->it_beginnings_;
|
++impl->next_input_;
|
||||||
|
|
||||||
return state;
|
return state;
|
||||||
}
|
}
|
||||||
@ -98,32 +120,33 @@ void PropagatingAnyWay::sendForward(const robot_trajectory::RobotTrajectoryPtr&
|
|||||||
const InterfaceState& from,
|
const InterfaceState& from,
|
||||||
const planning_scene::PlanningSceneConstPtr& to,
|
const planning_scene::PlanningSceneConstPtr& to,
|
||||||
double cost){
|
double cost){
|
||||||
SubTrajectory &trajectory = impl_->addTrajectory(t, cost);
|
IMPL(PropagatingAnyWay);
|
||||||
trajectory.setBeginning(from);
|
SubTrajectory &trajectory = impl->addTrajectory(t, cost);
|
||||||
impl_->sendForward(trajectory, to);
|
trajectory.setStartState(from);
|
||||||
|
impl->sendForward(trajectory, to);
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingAnyWay::newBeginning(const InterfaceStateList::iterator &it)
|
void PropagatingAnyWay::newInputState(const Interface::iterator &it)
|
||||||
{
|
{
|
||||||
I(PropagatingAnyWay);
|
IMPL(PropagatingAnyWay);
|
||||||
// we just appended a state to the list, but the iterator doesn't see it anymore
|
// we just appended a state to the list, but the iterator doesn't see it anymore
|
||||||
// so let's point it at the new one
|
// so let's point it at the new one
|
||||||
if( impl->it_beginnings_ == impl->beginnings_.end() )
|
if( impl->next_input_ == impl->input_->end() )
|
||||||
--impl->it_beginnings_;
|
--impl->next_input_;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool PropagatingAnyWay::hasEnding() const{
|
bool PropagatingAnyWay::hasEnding() const{
|
||||||
I(PropagatingAnyWay);
|
IMPL(const PropagatingAnyWay);
|
||||||
return impl->it_endings_ != impl->endings_.end();
|
return impl->next_output_ != impl->output_->end();
|
||||||
}
|
}
|
||||||
|
|
||||||
InterfaceState& PropagatingAnyWay::fetchStateEnding(){
|
const InterfaceState& PropagatingAnyWay::fetchStateEnding(){
|
||||||
I(PropagatingAnyWay);
|
IMPL(PropagatingAnyWay);
|
||||||
if(impl->it_endings_ == impl->endings_.end())
|
if(!hasEnding())
|
||||||
throw std::runtime_error("no new state for ending available");
|
throw std::runtime_error("no new state for ending available");
|
||||||
|
|
||||||
InterfaceState& state= *impl->it_endings_;
|
const InterfaceState& state= *impl->next_output_;
|
||||||
++impl->it_endings_;
|
++impl->next_output_;
|
||||||
|
|
||||||
return state;
|
return state;
|
||||||
}
|
}
|
||||||
@ -132,87 +155,94 @@ void PropagatingAnyWay::sendBackward(const robot_trajectory::RobotTrajectoryPtr&
|
|||||||
const planning_scene::PlanningSceneConstPtr& from,
|
const planning_scene::PlanningSceneConstPtr& from,
|
||||||
const InterfaceState& to,
|
const InterfaceState& to,
|
||||||
double cost){
|
double cost){
|
||||||
SubTrajectory& trajectory = impl_->addTrajectory(t, cost);
|
IMPL(PropagatingAnyWay);
|
||||||
trajectory.setEnding(to);
|
SubTrajectory& trajectory = impl->addTrajectory(t, cost);
|
||||||
impl_->sendBackward(trajectory, from);
|
trajectory.setEndState(to);
|
||||||
|
impl->sendBackward(trajectory, from);
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingAnyWay::newEnd(const InterfaceStateList::iterator &it)
|
void PropagatingAnyWay::newOutputState(const Interface::iterator &it)
|
||||||
{
|
{
|
||||||
I(PropagatingAnyWay);
|
IMPL(PropagatingAnyWay);
|
||||||
// we just appended a state to the list, but the iterator doesn't see it anymore
|
// we just appended a state to the list, but the iterator doesn't see it anymore
|
||||||
// so let's point it at the new one
|
// so let's point it at the new one
|
||||||
if( impl->it_endings_ == impl->endings_.end() )
|
if( impl->next_output_ == impl->output_->end() )
|
||||||
--impl->it_endings_;
|
--impl->next_output_;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
PRIVATE_CLASS_IMPL(PropagatingForward)
|
||||||
PropagatingForward::PropagatingForward(const std::string& name)
|
PropagatingForward::PropagatingForward(const std::string& name)
|
||||||
: PropagatingAnyWay(new PropagatingForwardPrivate(this, name))
|
: PropagatingAnyWay(new PropagatingForwardPrivate(this, name))
|
||||||
{}
|
{}
|
||||||
|
|
||||||
|
|
||||||
|
PRIVATE_CLASS_IMPL(PropagatingBackward)
|
||||||
PropagatingBackward::PropagatingBackward(const std::string &name)
|
PropagatingBackward::PropagatingBackward(const std::string &name)
|
||||||
: PropagatingAnyWay(new PropagatingBackwardPrivate(this, name))
|
: PropagatingAnyWay(new PropagatingBackwardPrivate(this, name))
|
||||||
{}
|
{}
|
||||||
|
|
||||||
|
|
||||||
|
PRIVATE_CLASS_IMPL(Generator)
|
||||||
Generator::Generator(const std::string &name)
|
Generator::Generator(const std::string &name)
|
||||||
: SubTask(new SubTaskPrivate(this, name))
|
: SubTask(new GeneratorPrivate(this, name))
|
||||||
{}
|
{}
|
||||||
|
|
||||||
void Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost)
|
void Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost)
|
||||||
{
|
{
|
||||||
|
IMPL(Generator);
|
||||||
// empty trajectory ref -> this node only produces states
|
// empty trajectory ref -> this node only produces states
|
||||||
robot_trajectory::RobotTrajectoryPtr dummy;
|
robot_trajectory::RobotTrajectoryPtr dummy;
|
||||||
moveit::task_constructor::SubTrajectory& trajectory = impl_->addTrajectory(dummy, cost);
|
SubTrajectory& trajectory = impl->addTrajectory(dummy, cost);
|
||||||
|
|
||||||
std::cout << "spawning state" << std::endl;
|
std::cout << "spawning state" << std::endl;
|
||||||
impl_->sendBackward(trajectory, ps);
|
impl->sendBackward(trajectory, ps);
|
||||||
impl_->sendForward(trajectory, ps);
|
impl->sendForward(trajectory, ps);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
PRIVATE_CLASS_IMPL(Connecting)
|
||||||
Connecting::Connecting(const std::string &name)
|
Connecting::Connecting(const std::string &name)
|
||||||
: SubTask(new ConnectingPrivate(this, name))
|
: SubTask(new ConnectingPrivate(this, name))
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
void Connecting::newBeginning(const InterfaceStateList::iterator& it)
|
void Connecting::newInputState(const Interface::iterator& it)
|
||||||
{
|
{
|
||||||
I(Connecting);
|
IMPL(Connecting);
|
||||||
// TODO: need to handle the pairs iterator
|
// TODO: need to handle the pairs iterator
|
||||||
if( impl->it_pairs_.first == impl->beginnings_.end() )
|
if( impl->it_pairs_.first == impl->input_->end() )
|
||||||
--impl->it_pairs_.first;
|
--impl->it_pairs_.first;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Connecting::newEnd(const InterfaceStateList::iterator& it)
|
void Connecting::newOutputState(const Interface::iterator& it)
|
||||||
{
|
{
|
||||||
I(Connecting);
|
IMPL(Connecting);
|
||||||
// TODO: need to handle the pairs iterator properly
|
// TODO: need to handle the pairs iterator properly
|
||||||
if( impl->it_pairs_.second == impl->endings_.end() )
|
if( impl->it_pairs_.second == impl->output_->end() )
|
||||||
--impl->it_pairs_.second;
|
--impl->it_pairs_.second;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Connecting::hasStatePair() const{
|
bool Connecting::hasStatePair() const{
|
||||||
I(Connecting);
|
IMPL(const Connecting);
|
||||||
// TODO: implement this properly
|
// TODO: implement this properly
|
||||||
return impl->it_pairs_.first != impl->beginnings_.end() &&
|
return impl->it_pairs_.first != impl->input_->end() &&
|
||||||
impl->it_pairs_.second != impl->endings_.end();
|
impl->it_pairs_.second != impl->output_->end();
|
||||||
}
|
}
|
||||||
|
|
||||||
std::pair<InterfaceState&, InterfaceState&> Connecting::fetchStatePair(){
|
InterfaceStatePair Connecting::fetchStatePair(){
|
||||||
I(Connecting);
|
IMPL(Connecting);
|
||||||
// TODO: implement this properly
|
// TODO: implement this properly
|
||||||
return std::pair<InterfaceState&, InterfaceState&>
|
return std::pair<const InterfaceState&, const InterfaceState&>
|
||||||
(*impl->it_pairs_.first, *(impl->it_pairs_.second++));
|
(*impl->it_pairs_.first, *(impl->it_pairs_.second++));
|
||||||
}
|
}
|
||||||
|
|
||||||
void Connecting::connect(const robot_trajectory::RobotTrajectoryPtr& t, const InterfaceStatePair& state_pair, double cost)
|
void Connecting::connect(const robot_trajectory::RobotTrajectoryPtr& t, const InterfaceStatePair& state_pair, double cost)
|
||||||
{
|
{
|
||||||
moveit::task_constructor::SubTrajectory& trajectory = impl_->addTrajectory(t, cost);
|
IMPL(Connecting);
|
||||||
trajectory.setBeginning(state_pair.first);
|
SubTrajectory& trajectory = impl->addTrajectory(t, cost);
|
||||||
trajectory.setEnding(state_pair.second);
|
trajectory.setStartState(state_pair.first);
|
||||||
|
trajectory.setEndState(state_pair.second);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
128
src/subtask_p.h
128
src/subtask_p.h
@ -1,41 +1,51 @@
|
|||||||
|
// copyright Robert Haschke @ 2017
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <moveit_task_constructor/subtask.h>
|
#include <moveit_task_constructor/subtask.h>
|
||||||
|
#include <moveit_task_constructor/storage.h>
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
class SubTaskPrivate {
|
class SubTaskPrivate {
|
||||||
|
friend class SubTask;
|
||||||
|
friend class SubTaskTest; // allow unit tests
|
||||||
|
// ContainerBase will maintain the double-linked list of interfaces
|
||||||
|
friend class ContainerBasePrivate;
|
||||||
|
friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
inline SubTaskPrivate(SubTask* me, const std::string& name)
|
inline SubTaskPrivate(SubTask* me, const std::string& name)
|
||||||
: me_(me), name_(name)
|
: me_(me), name_(name), parent_(nullptr), predeccessor_(nullptr), successor_(nullptr)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
inline const InterfaceStateList& getBeginning() const { return beginnings_; }
|
|
||||||
inline const InterfaceStateList& getEnd() const { return endings_; }
|
|
||||||
inline const std::list<SubTrajectory>& getTrajectories() const { return trajectories_; }
|
|
||||||
|
|
||||||
void addPredecessor(SubTaskPtr);
|
|
||||||
void addSuccessor(SubTaskPtr);
|
|
||||||
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
|
SubTrajectory& addTrajectory(const robot_trajectory::RobotTrajectoryPtr &, double cost);
|
||||||
|
|
||||||
|
virtual SubTask::InterfaceFlags interfaceFlags() const;
|
||||||
void sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
|
void sendForward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
|
||||||
void sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
|
void sendBackward(SubTrajectory& traj, const planning_scene::PlanningSceneConstPtr& ps);
|
||||||
|
|
||||||
InterfaceState* newBeginning(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
|
|
||||||
InterfaceState* newEnd(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
SubTask* const me_; // pointer to owning instance
|
SubTask* const me_; // associated/owning SubTask instance
|
||||||
const std::string name_;
|
const std::string name_;
|
||||||
|
|
||||||
// avoid shared pointers back: why? to allow proper deallocation?
|
planning_scene::PlanningSceneConstPtr scene_;
|
||||||
SubTaskWeakPtr predecessor_;
|
planning_pipeline::PlanningPipelinePtr planner_;
|
||||||
SubTaskPtr successor_;
|
|
||||||
|
|
||||||
InterfaceStateList beginnings_;
|
InterfacePtr input_;
|
||||||
InterfaceStateList endings_;
|
InterfacePtr output_;
|
||||||
std::list<SubTrajectory> trajectories_;
|
std::list<SubTrajectory> trajectories_;
|
||||||
|
|
||||||
|
const InterfacePtr prevOutput() const { return predeccessor_ ? predeccessor_->output_ : InterfacePtr(); }
|
||||||
|
const InterfacePtr nextInput() const { return successor_ ? successor_->input_ : InterfacePtr(); }
|
||||||
|
|
||||||
|
private:
|
||||||
|
// items accessed by ContainerBasePrivate only to maintain hierarchy
|
||||||
|
SubTaskPrivate* parent_;
|
||||||
|
SubTaskPrivate* predeccessor_;
|
||||||
|
SubTaskPrivate* successor_;
|
||||||
};
|
};
|
||||||
|
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
|
||||||
|
|
||||||
|
|
||||||
class PropagatingAnyWayPrivate : public SubTaskPrivate {
|
class PropagatingAnyWayPrivate : public SubTaskPrivate {
|
||||||
@ -44,19 +54,67 @@ class PropagatingAnyWayPrivate : public SubTaskPrivate {
|
|||||||
public:
|
public:
|
||||||
inline PropagatingAnyWayPrivate(PropagatingAnyWay *me, const std::string &name)
|
inline PropagatingAnyWayPrivate(PropagatingAnyWay *me, const std::string &name)
|
||||||
: SubTaskPrivate(me, name)
|
: SubTaskPrivate(me, name)
|
||||||
, it_beginnings_ (beginnings_.begin())
|
{
|
||||||
, it_endings_ (endings_.begin())
|
input_.reset(new Interface([me](const Interface::iterator& it) { me->newInputState(it); }));
|
||||||
{}
|
output_.reset(new Interface(std::bind(&PropagatingAnyWay::newOutputState, me, std::placeholders::_1)));
|
||||||
|
next_input_ = input_->begin();
|
||||||
|
next_output_ = output_->end();
|
||||||
|
}
|
||||||
|
|
||||||
|
SubTask::InterfaceFlags interfaceFlags() const override {
|
||||||
|
return SubTask::InterfaceFlags({SubTask::READS_INPUT, SubTask::WRITES_NEXT_INPUT,
|
||||||
|
SubTask::READS_OUTPUT, SubTask::WRITES_PREV_OUTPUT,
|
||||||
|
SubTask::WRITES_UNKNOWN});
|
||||||
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
InterfaceStateList::iterator it_beginnings_;
|
Interface::const_iterator next_input_;
|
||||||
InterfaceStateList::iterator it_endings_;
|
Interface::const_iterator next_output_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
// for now, we use the same implementation for the reduced versions too
|
class PropagatingForwardPrivate : public PropagatingAnyWayPrivate {
|
||||||
typedef PropagatingAnyWayPrivate PropagatingForwardPrivate;
|
public:
|
||||||
typedef PropagatingAnyWayPrivate PropagatingBackwardPrivate;
|
inline PropagatingForwardPrivate(PropagatingForward *me, const std::string &name)
|
||||||
|
: PropagatingAnyWayPrivate(me, name)
|
||||||
|
{
|
||||||
|
// indicate, that we don't accept new states from output interface
|
||||||
|
output_.reset();
|
||||||
|
next_output_ = Interface::iterator();
|
||||||
|
}
|
||||||
|
|
||||||
|
SubTask::InterfaceFlags interfaceFlags() const override {
|
||||||
|
return SubTask::InterfaceFlags({SubTask::READS_INPUT, SubTask::WRITES_NEXT_INPUT});
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class PropagatingBackwardPrivate : public PropagatingAnyWayPrivate {
|
||||||
|
public:
|
||||||
|
inline PropagatingBackwardPrivate(PropagatingBackward *me, const std::string &name)
|
||||||
|
: PropagatingAnyWayPrivate(me, name)
|
||||||
|
{
|
||||||
|
// indicate, that we don't accept new states from input interface
|
||||||
|
input_.reset();
|
||||||
|
next_input_ = Interface::iterator();
|
||||||
|
}
|
||||||
|
|
||||||
|
SubTask::InterfaceFlags interfaceFlags() const override {
|
||||||
|
return SubTask::InterfaceFlags({SubTask::READS_OUTPUT, SubTask::WRITES_PREV_OUTPUT});
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class GeneratorPrivate : public SubTaskPrivate {
|
||||||
|
public:
|
||||||
|
inline GeneratorPrivate(Generator *me, const std::string &name)
|
||||||
|
: SubTaskPrivate(me, name)
|
||||||
|
{}
|
||||||
|
|
||||||
|
SubTask::InterfaceFlags interfaceFlags() const override {
|
||||||
|
return SubTask::InterfaceFlags({SubTask::WRITES_NEXT_INPUT, SubTask::WRITES_PREV_OUTPUT});
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
class ConnectingPrivate : public SubTaskPrivate {
|
class ConnectingPrivate : public SubTaskPrivate {
|
||||||
@ -65,11 +123,27 @@ class ConnectingPrivate : public SubTaskPrivate {
|
|||||||
public:
|
public:
|
||||||
inline ConnectingPrivate(Connecting *me, const std::string &name)
|
inline ConnectingPrivate(Connecting *me, const std::string &name)
|
||||||
: SubTaskPrivate(me, name)
|
: SubTaskPrivate(me, name)
|
||||||
, it_pairs_(beginnings_.begin(), endings_.begin())
|
{
|
||||||
{}
|
input_.reset(new Interface(std::bind(&Connecting::newInputState, me, std::placeholders::_1)));
|
||||||
|
output_.reset(new Interface(std::bind(&Connecting::newOutputState, me, std::placeholders::_1)));
|
||||||
|
it_pairs_ = std::make_pair(input_->begin(), output_->begin());
|
||||||
|
}
|
||||||
|
|
||||||
|
SubTask::InterfaceFlags interfaceFlags() const override {
|
||||||
|
return SubTask::InterfaceFlags({SubTask::READS_INPUT, SubTask::READS_OUTPUT});
|
||||||
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::pair<InterfaceStateList::iterator, InterfaceStateList::iterator> it_pairs_;
|
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|
||||||
|
|
||||||
|
// implement pimpl_func()
|
||||||
|
#define PRIVATE_CLASS_IMPL(Class) \
|
||||||
|
inline Class##Private* Class::pimpl_func() { return static_cast<Class##Private*>(pimpl_); } \
|
||||||
|
inline const Class##Private* Class::pimpl_func() const { return static_cast<const Class##Private*>(pimpl_); } \
|
||||||
|
|
||||||
|
// get correctly casted private impl pointer
|
||||||
|
#define IMPL(Class) Class##Private * const impl = pimpl_func()
|
||||||
|
|||||||
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/subtasks/cartesian_position_motion.h>
|
||||||
|
#include <moveit_task_constructor/storage.h>
|
||||||
|
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
#include <moveit_msgs/DisplayTrajectory.h>
|
#include <moveit_msgs/DisplayTrajectory.h>
|
||||||
@ -58,7 +59,7 @@ void CartesianPositionMotion::setCartesianStepSize(double distance){
|
|||||||
step_size_= distance;
|
step_size_= distance;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool CartesianPositionMotion::canCompute(){
|
bool CartesianPositionMotion::canCompute() const{
|
||||||
return hasBeginning() || hasEnding();
|
return hasBeginning() || hasEnding();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -83,7 +84,7 @@ bool CartesianPositionMotion::compute(){
|
|||||||
bool CartesianPositionMotion::_computeFromBeginning(){
|
bool CartesianPositionMotion::_computeFromBeginning(){
|
||||||
assert( hasBeginning() );
|
assert( hasBeginning() );
|
||||||
|
|
||||||
moveit::task_constructor::InterfaceState& beginning= fetchStateBeginning();
|
const InterfaceState& beginning= fetchStateBeginning();
|
||||||
planning_scene::PlanningScenePtr result_scene = beginning.state->diff();
|
planning_scene::PlanningScenePtr result_scene = beginning.state->diff();
|
||||||
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
||||||
|
|
||||||
@ -173,7 +174,7 @@ bool CartesianPositionMotion::_computeFromBeginning(){
|
|||||||
bool CartesianPositionMotion::_computeFromEnding(){
|
bool CartesianPositionMotion::_computeFromEnding(){
|
||||||
assert( hasEnding() );
|
assert( hasEnding() );
|
||||||
|
|
||||||
moveit::task_constructor::InterfaceState& ending= fetchStateEnding();
|
const InterfaceState& ending= fetchStateEnding();
|
||||||
planning_scene::PlanningScenePtr result_scene = ending.state->diff();
|
planning_scene::PlanningScenePtr result_scene = ending.state->diff();
|
||||||
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
||||||
|
|
||||||
@ -246,7 +247,7 @@ bool CartesianPositionMotion::_computeFromEnding(){
|
|||||||
void CartesianPositionMotion::_publishTrajectory(const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start){
|
void CartesianPositionMotion::_publishTrajectory(const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start){
|
||||||
moveit_msgs::DisplayTrajectory dt;
|
moveit_msgs::DisplayTrajectory dt;
|
||||||
robot_state::robotStateToRobotStateMsg(start, dt.trajectory_start);
|
robot_state::robotStateToRobotStateMsg(start, dt.trajectory_start);
|
||||||
dt.model_id= scene_->getRobotModel()->getName();
|
dt.model_id= scene()->getRobotModel()->getName();
|
||||||
dt.trajectory.resize(1);
|
dt.trajectory.resize(1);
|
||||||
trajectory.getRobotTrajectoryMsg(dt.trajectory[0]);
|
trajectory.getRobotTrajectoryMsg(dt.trajectory[0]);
|
||||||
dt.model_id= start.getRobotModel()->getName();
|
dt.model_id= start.getRobotModel()->getName();
|
||||||
|
|||||||
@ -8,14 +8,13 @@ CurrentState::CurrentState(std::string name)
|
|||||||
ran_= false;
|
ran_= false;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool CurrentState::canCompute(){
|
bool CurrentState::canCompute() const{
|
||||||
return !ran_;
|
return !ran_;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool CurrentState::compute(){
|
bool CurrentState::compute(){
|
||||||
ran_= true;
|
ran_= true;
|
||||||
assert( scene_ );
|
spawn(scene());
|
||||||
spawn(scene_);
|
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -70,7 +70,7 @@ void GenerateGraspPose::setMaxIKSolutions(uint32_t n){
|
|||||||
max_ik_solutions_= n;
|
max_ik_solutions_= n;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool GenerateGraspPose::canCompute(){
|
bool GenerateGraspPose::canCompute() const{
|
||||||
return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI;
|
return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -102,9 +102,9 @@ namespace {
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool GenerateGraspPose::compute(){
|
bool GenerateGraspPose::compute(){
|
||||||
assert( scene_->getRobotModel()->hasEndEffector(eef_) && "The specified end effector is not defined in the srdf" );
|
assert(scene()->getRobotModel()->hasEndEffector(eef_) && "The specified end effector is not defined in the srdf");
|
||||||
|
|
||||||
planning_scene::PlanningScenePtr grasp_scene = scene_->diff();
|
planning_scene::PlanningScenePtr grasp_scene = scene()->diff();
|
||||||
robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst();
|
robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst();
|
||||||
|
|
||||||
const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef_);
|
const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef_);
|
||||||
@ -122,7 +122,7 @@ bool GenerateGraspPose::compute(){
|
|||||||
const moveit::core::GroupStateValidityCallbackFn is_valid=
|
const moveit::core::GroupStateValidityCallbackFn is_valid=
|
||||||
std::bind(
|
std::bind(
|
||||||
&isValid,
|
&isValid,
|
||||||
scene_,
|
scene(),
|
||||||
ignore_collisions_,
|
ignore_collisions_,
|
||||||
&previous_solutions_,
|
&previous_solutions_,
|
||||||
std::placeholders::_1,
|
std::placeholders::_1,
|
||||||
@ -130,7 +130,7 @@ bool GenerateGraspPose::compute(){
|
|||||||
std::placeholders::_3);
|
std::placeholders::_3);
|
||||||
|
|
||||||
geometry_msgs::Pose object_pose, grasp_pose;
|
geometry_msgs::Pose object_pose, grasp_pose;
|
||||||
const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(object_);
|
const Eigen::Affine3d object_pose_eigen= scene()->getFrameTransform(object_);
|
||||||
if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
|
if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
|
||||||
throw std::runtime_error("requested object does not exist or could not be retrieved");
|
throw std::runtime_error("requested object does not exist or could not be retrieved");
|
||||||
|
|
||||||
|
|||||||
@ -1,4 +1,5 @@
|
|||||||
#include <moveit_task_constructor/subtasks/gripper.h>
|
#include <moveit_task_constructor/subtasks/gripper.h>
|
||||||
|
#include <moveit_task_constructor/storage.h>
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/robot_state/conversions.h>
|
#include <moveit/robot_state/conversions.h>
|
||||||
@ -36,16 +37,16 @@ void Gripper::graspObject(std::string grasp_object){
|
|||||||
grasp_object_= grasp_object;
|
grasp_object_= grasp_object;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Gripper::canCompute(){
|
bool Gripper::canCompute() const{
|
||||||
return hasBeginning();
|
return hasBeginning();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Gripper::compute(){
|
bool Gripper::compute(){
|
||||||
assert( scene_->getRobotModel() );
|
assert(scene()->getRobotModel());
|
||||||
|
|
||||||
if(!mgi_){
|
if(!mgi_){
|
||||||
assert( scene_->getRobotModel()->hasEndEffector(eef_) && "no end effector with that name defined in srdf" );
|
assert(scene()->getRobotModel()->hasEndEffector(eef_) && "no end effector with that name defined in srdf");
|
||||||
const moveit::core::JointModelGroup* jmg= scene_->getRobotModel()->getEndEffector(eef_);
|
const moveit::core::JointModelGroup* jmg= scene()->getRobotModel()->getEndEffector(eef_);
|
||||||
const std::string group_name= jmg->getName();
|
const std::string group_name= jmg->getName();
|
||||||
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_name);
|
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_name);
|
||||||
|
|
||||||
@ -68,7 +69,7 @@ bool Gripper::compute(){
|
|||||||
}
|
}
|
||||||
|
|
||||||
::planning_interface::MotionPlanResponse res;
|
::planning_interface::MotionPlanResponse res;
|
||||||
if(!planner_->generatePlan(scene, req, res))
|
if(!planner()->generatePlan(scene, req, res))
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
// set end state
|
// set end state
|
||||||
|
|||||||
@ -1,4 +1,5 @@
|
|||||||
#include <moveit_task_constructor/subtasks/move.h>
|
#include <moveit_task_constructor/subtasks/move.h>
|
||||||
|
#include <moveit_task_constructor/storage.h>
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/robot_state/conversions.h>
|
#include <moveit/robot_state/conversions.h>
|
||||||
@ -43,14 +44,12 @@ void Move::setTo(std::string named_target){
|
|||||||
}
|
}
|
||||||
*/
|
*/
|
||||||
|
|
||||||
bool Move::canCompute(){
|
bool Move::canCompute() const{
|
||||||
return hasStatePair();
|
return hasStatePair();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Move::compute(){
|
bool Move::compute(){
|
||||||
assert( scene_->getRobotModel() );
|
InterfaceStatePair state_pair= fetchStatePair();
|
||||||
|
|
||||||
std::pair<InterfaceState&, InterfaceState&> state_pair= fetchStatePair();
|
|
||||||
|
|
||||||
mgi_->setJointValueTarget(state_pair.second.state->getCurrentState());
|
mgi_->setJointValueTarget(state_pair.second.state->getCurrentState());
|
||||||
if( !planner_id_.empty() )
|
if( !planner_id_.empty() )
|
||||||
@ -62,7 +61,7 @@ bool Move::compute(){
|
|||||||
|
|
||||||
ros::Duration(4.0).sleep();
|
ros::Duration(4.0).sleep();
|
||||||
::planning_interface::MotionPlanResponse res;
|
::planning_interface::MotionPlanResponse res;
|
||||||
if(!planner_->generatePlan(state_pair.first.state, req, res))
|
if(!planner()->generatePlan(state_pair.first.state, req, res))
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
// finish subtask
|
// finish subtask
|
||||||
|
|||||||
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/task.h>
|
||||||
|
#include <moveit_task_constructor/container.h>
|
||||||
#include <moveit_task_constructor/debug.h>
|
#include <moveit_task_constructor/debug.h>
|
||||||
|
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
@ -11,13 +13,28 @@
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
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);
|
rml_.reset(new robot_model_loader::RobotModelLoader);
|
||||||
if( !rml_->getModel() )
|
if( !rml_->getModel() )
|
||||||
throw Exception("Task failed to construct RobotModel");
|
throw Exception("Task failed to construct RobotModel");
|
||||||
|
}
|
||||||
|
void initScene() {
|
||||||
|
assert(rml_);
|
||||||
|
|
||||||
ros::NodeHandle h;
|
ros::NodeHandle h;
|
||||||
|
|
||||||
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
|
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
|
||||||
client.waitForExistence();
|
client.waitForExistence();
|
||||||
|
|
||||||
@ -40,69 +57,52 @@ Task::Task(){
|
|||||||
throw Exception("Task failed to acquire current PlanningScene");
|
throw Exception("Task failed to acquire current PlanningScene");
|
||||||
}
|
}
|
||||||
|
|
||||||
scene_.reset(new planning_scene::PlanningScene(rml_->getModel()));
|
scene_ = std::make_shared<planning_scene::PlanningScene>(rml_->getModel());
|
||||||
scene_->setPlanningSceneMsg(res.scene);
|
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(){
|
void Task::add(std::unique_ptr<SubTask> &&stage) {
|
||||||
subtasks_.clear();
|
if (!stage)
|
||||||
scene_.reset();
|
throw std::runtime_error("Task::add() failed: invalid stage pointer");
|
||||||
planner_.reset();
|
if (!insert(std::move(stage)))
|
||||||
}
|
throw std::runtime_error(std::string("Task::add() failed for stage: ") + stage->getName());
|
||||||
|
|
||||||
void Task::clear(){
|
|
||||||
subtasks_.clear();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Task::plan(){
|
bool Task::plan(){
|
||||||
NewSolutionPublisher debug(*this);
|
NewSolutionPublisher debug(*this);
|
||||||
|
|
||||||
bool computed= true;
|
while(ros::ok() && canCompute()) {
|
||||||
while(ros::ok() && computed){
|
if (compute()) {
|
||||||
computed= false;
|
|
||||||
for( SubTaskPtr& subtask : subtasks_ ){
|
|
||||||
if( !subtask->canCompute() )
|
|
||||||
continue;
|
|
||||||
std::cout << "Computing subtask '" << subtask->getName() << "':" << std::endl;
|
|
||||||
bool success= subtask->compute();
|
|
||||||
computed= true;
|
|
||||||
std::cout << (success ? "succeeded" : "failed") << std::endl;
|
|
||||||
}
|
|
||||||
if(computed){
|
|
||||||
debug.publish();
|
debug.publish();
|
||||||
printState();
|
printState();
|
||||||
}
|
} else
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Task::add( SubTaskPtr subtask ){
|
|
||||||
subtask->setPlanningScene( scene_ );
|
|
||||||
subtask->setPlanningPipeline( planner_ );
|
|
||||||
|
|
||||||
if( !subtasks_.empty() ){
|
|
||||||
subtask->impl_->addPredecessor( subtasks_.back() );
|
|
||||||
subtasks_.back()->impl_->addSuccessor( subtask );
|
|
||||||
}
|
|
||||||
|
|
||||||
subtasks_.push_back( subtask );
|
|
||||||
}
|
|
||||||
|
|
||||||
const robot_state::RobotState& Task::getCurrentRobotState() const {
|
const robot_state::RobotState& Task::getCurrentRobotState() const {
|
||||||
return scene_->getCurrentState();
|
return pimpl_func()->scene_->getCurrentState();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Task::printState(){
|
void Task::printState(){
|
||||||
for( auto& st : subtasks_ ){
|
ContainerBase::StageCallback processor = [](const SubTask& stage, int depth) -> bool {
|
||||||
std::cout
|
std::cout << std::string(2*depth, ' ') << stage << std::endl;
|
||||||
<< st->impl_->getBeginning().size() << " -> "
|
};
|
||||||
<< st->impl_->getTrajectories().size()
|
traverseStages(processor);
|
||||||
<< " <- " << st->impl_->getEnd().size()
|
|
||||||
<< " / " << st->getName()
|
|
||||||
<< std::endl;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
@ -120,7 +120,7 @@ bool traverseFullTrajectories(
|
|||||||
ret= cb(trace);
|
ret= cb(trace);
|
||||||
}
|
}
|
||||||
else if( start.end ){
|
else if( start.end ){
|
||||||
for( SubTrajectory* successor : start.end->nextTrajectories() ){
|
for( SubTrajectory* successor : start.end->outgoingTrajectories() ){
|
||||||
if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ){
|
if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ){
|
||||||
ret= false;
|
ret= false;
|
||||||
break;
|
break;
|
||||||
@ -135,11 +135,12 @@ bool traverseFullTrajectories(
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool Task::processSolutions(const Task::SolutionCallback& processor) {
|
bool Task::processSolutions(const Task::SolutionCallback& processor) {
|
||||||
const size_t nr_of_trajectories= subtasks_.size();
|
const TaskPrivate::array_type& children = pimpl_func()->children();
|
||||||
|
const size_t nr_of_trajectories = children.size();
|
||||||
std::vector<SubTrajectory*> trace;
|
std::vector<SubTrajectory*> trace;
|
||||||
trace.reserve(nr_of_trajectories);
|
trace.reserve(nr_of_trajectories);
|
||||||
for(SubTrajectory& st : subtasks_.front()->impl_->trajectories_)
|
for(SubTrajectory& st : children.front()->pimpl_func()->trajectories_)
|
||||||
if( !traverseFullTrajectories(st, subtasks_.size(), processor, trace) )
|
if( !traverseFullTrajectories(st, nr_of_trajectories, processor, trace) )
|
||||||
return false;
|
return false;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,3 +1,16 @@
|
|||||||
|
#############
|
||||||
|
## Testing ##
|
||||||
|
#############
|
||||||
|
|
||||||
|
## access to private headers
|
||||||
|
include_directories(${CMAKE_SOURCE_DIR}/src)
|
||||||
|
|
||||||
|
## Add gtest based cpp test target and link libraries
|
||||||
|
if (CATKIN_ENABLE_TESTING)
|
||||||
|
catkin_add_gtest(${PROJECT_NAME}-container_test container.cpp)
|
||||||
|
target_link_libraries(${PROJECT_NAME}-container_test ${PROJECT_NAME} gtest_main ${catkin_LIBRARIES})
|
||||||
|
endif()
|
||||||
|
|
||||||
add_executable(test_plan_current_state test_plan_current_state.cpp)
|
add_executable(test_plan_current_state test_plan_current_state.cpp)
|
||||||
target_link_libraries(test_plan_current_state ${PROJECT_NAME}_subtasks ${PROJECT_NAME})
|
target_link_libraries(test_plan_current_state ${PROJECT_NAME}_subtasks ${PROJECT_NAME})
|
||||||
|
|
||||||
|
|||||||
135
src/test/container.cpp
Normal file
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;
|
Task t;
|
||||||
|
|
||||||
t.add( std::make_shared<subtasks::CurrentState>("current state") );
|
t.add( std::make_unique<subtasks::CurrentState>("current state") );
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move= std::make_shared<subtasks::CartesianPositionMotion>("lift object");
|
auto move= std::make_unique<subtasks::CartesianPositionMotion>("lift object");
|
||||||
move->setGroup("arm");
|
move->setGroup("arm");
|
||||||
move->setLink("s_model_tool0");
|
move->setLink("s_model_tool0");
|
||||||
move->setMinMaxDistance(0.05, 0.2);
|
move->setMinMaxDistance(0.05, 0.2);
|
||||||
@ -25,11 +25,11 @@ int main(int argc, char** argv){
|
|||||||
direction.header.frame_id= "world";
|
direction.header.frame_id= "world";
|
||||||
direction.vector.x= 1.0;
|
direction.vector.x= 1.0;
|
||||||
move->along(direction);
|
move->along(direction);
|
||||||
t.add(move);
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move= std::make_shared<subtasks::CartesianPositionMotion>("lift object");
|
auto move= std::make_unique<subtasks::CartesianPositionMotion>("lift object");
|
||||||
move->setGroup("arm");
|
move->setGroup("arm");
|
||||||
move->setLink("s_model_tool0");
|
move->setLink("s_model_tool0");
|
||||||
move->setMinDistance(0.01);
|
move->setMinDistance(0.01);
|
||||||
@ -40,7 +40,7 @@ int main(int argc, char** argv){
|
|||||||
target.point.y= 0.7;
|
target.point.y= 0.7;
|
||||||
target.point.z= 1.5;
|
target.point.z= 1.5;
|
||||||
move->towards(target);
|
move->towards(target);
|
||||||
t.add(move);
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
t.plan();
|
t.plan();
|
||||||
|
|||||||
@ -11,7 +11,7 @@ int main(int argc, char** argv){
|
|||||||
|
|
||||||
Task t;
|
Task t;
|
||||||
|
|
||||||
t.add( std::make_shared<subtasks::CurrentState>("current state") );
|
t.add( std::make_unique<subtasks::CurrentState>("current state") );
|
||||||
|
|
||||||
t.plan();
|
t.plan();
|
||||||
|
|
||||||
|
|||||||
@ -36,7 +36,7 @@ int main(int argc, char** argv){
|
|||||||
|
|
||||||
Task t;
|
Task t;
|
||||||
|
|
||||||
auto st= std::make_shared<subtasks::GenerateGraspPose>("generate grasp candidates");
|
auto st= std::make_unique<subtasks::GenerateGraspPose>("generate grasp candidates");
|
||||||
|
|
||||||
st->setEndEffector("gripper");
|
st->setEndEffector("gripper");
|
||||||
//st->setGroup("arm");
|
//st->setGroup("arm");
|
||||||
@ -45,7 +45,7 @@ int main(int argc, char** argv){
|
|||||||
st->setAngleDelta(0.1);
|
st->setAngleDelta(0.1);
|
||||||
st->setGraspOffset(0.03);
|
st->setGraspOffset(0.03);
|
||||||
|
|
||||||
t.add(st);
|
t.add(std::move(st));
|
||||||
|
|
||||||
t.plan();
|
t.plan();
|
||||||
|
|
||||||
|
|||||||
@ -14,24 +14,24 @@ int main(int argc, char** argv){
|
|||||||
|
|
||||||
Task t;
|
Task t;
|
||||||
|
|
||||||
t.add( std::make_shared<subtasks::CurrentState>("current state") );
|
t.add( std::make_unique<subtasks::CurrentState>("current state") );
|
||||||
{
|
{
|
||||||
auto gripper= std::make_shared<subtasks::Gripper>("close gripper");
|
auto gripper= std::make_unique<subtasks::Gripper>("close gripper");
|
||||||
gripper->setEndEffector("gripper");
|
gripper->setEndEffector("gripper");
|
||||||
gripper->setTo("closed");
|
gripper->setTo("closed");
|
||||||
t.add( gripper );
|
t.add(std::move( gripper ));
|
||||||
}
|
}
|
||||||
t.plan();
|
t.plan();
|
||||||
|
|
||||||
/*TODO currently not implemented in gripper*/
|
/*TODO currently not implemented in gripper*/
|
||||||
/*
|
/*
|
||||||
{
|
{
|
||||||
auto gripper= std::make_shared<subtasks::Gripper>("close gripper");
|
auto gripper= std::make_unique<subtasks::Gripper>("close gripper");
|
||||||
gripper->setEndEffector("gripper");
|
gripper->setEndEffector("gripper");
|
||||||
gripper->setTo("closed");
|
gripper->setTo("closed");
|
||||||
t.add( gripper );
|
t.add(std::move( gripper ));
|
||||||
}
|
}
|
||||||
t.add( std::make_shared<subtasks::CurrentState>("current state") );
|
t.add( std::make_unique<subtasks::CurrentState>("current state") );
|
||||||
t.plan();
|
t.plan();
|
||||||
*/
|
*/
|
||||||
return 0;
|
return 0;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user