renamed subtask -> stage

This commit is contained in:
Robert Haschke 2017-10-16 08:51:01 +02:00
parent 3cc112fe2c
commit a32007613e
31 changed files with 176 additions and 176 deletions

View File

@ -24,21 +24,21 @@ set(CMAKE_CXX_STANDARD 14)
add_library(${PROJECT_NAME} add_library(${PROJECT_NAME}
src/storage.cpp src/storage.cpp
src/subtask.cpp src/stage.cpp
src/container.cpp src/container.cpp
src/task.cpp src/task.cpp
src/debug.cpp src/debug.cpp
src/subtask_p.h src/stage_p.h
src/container_p.h src/container_p.h
include/${PROJECT_NAME}/utils.h include/${PROJECT_NAME}/utils.h
include/${PROJECT_NAME}/storage.h include/${PROJECT_NAME}/storage.h
include/${PROJECT_NAME}/subtask.h include/${PROJECT_NAME}/stage.h
include/${PROJECT_NAME}/container.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/stages)
add_subdirectory(src/demo) add_subdirectory(src/demo)
add_subdirectory(src/test) add_subdirectory(src/test)

View File

@ -1,17 +1,17 @@
#pragma once #pragma once
#include "subtask.h" #include "stage.h"
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
class ContainerBasePrivate; class ContainerBasePrivate;
class ContainerBase : public SubTask class ContainerBase : public Stage
{ {
public: public:
PRIVATE_CLASS(ContainerBase) PRIVATE_CLASS(ContainerBase)
typedef SubTask::pointer value_type; typedef Stage::pointer value_type;
typedef std::function<bool(const SubTask&, int depth)> StageCallback; typedef std::function<bool(const Stage&, int depth)> StageCallback;
typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback; typedef std::function<bool(const std::vector<SubTrajectory*>&)> SolutionCallback;
virtual bool canInsert(const value_type& stage, int before = -1) const = 0; virtual bool canInsert(const value_type& stage, int before = -1) const = 0;

View File

@ -28,39 +28,39 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory)
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
MOVEIT_CLASS_FORWARD(Interface) MOVEIT_CLASS_FORWARD(Interface)
MOVEIT_CLASS_FORWARD(SubTask) MOVEIT_CLASS_FORWARD(Stage)
MOVEIT_CLASS_FORWARD(SubTrajectory) MOVEIT_CLASS_FORWARD(SubTrajectory)
class InterfaceState; class InterfaceState;
typedef std::pair<const InterfaceState&, const InterfaceState&> InterfaceStatePair; typedef std::pair<const InterfaceState&, const InterfaceState&> InterfaceStatePair;
class SubTaskPrivate; class StagePrivate;
class SubTask { class Stage {
friend std::ostream& operator<<(std::ostream &os, const SubTask& stage); friend std::ostream& operator<<(std::ostream &os, const Stage& stage);
public: public:
PRIVATE_CLASS(SubTask) PRIVATE_CLASS(Stage)
typedef std::unique_ptr<SubTask> pointer; typedef std::unique_ptr<Stage> pointer;
~SubTask(); ~Stage();
/// initialize stage once before planning /// initialize stage once before planning
virtual bool init(const planning_scene::PlanningSceneConstPtr& scene); virtual bool init(const planning_scene::PlanningSceneConstPtr& scene);
const std::string& getName() const; const std::string& getName() const;
protected: protected:
/// SubTask can only be instantiated through derived classes /// Stage can only be instantiated through derived classes
SubTask(SubTaskPrivate *impl); Stage(StagePrivate *impl);
protected: protected:
// TODO: use unique_ptr<SubTaskPrivate> and get rid of destructor // TODO: use unique_ptr<StagePrivate> and get rid of destructor
SubTaskPrivate* const pimpl_; // constness guarantees one initial write StagePrivate* const pimpl_; // constness guarantees one initial write
}; };
std::ostream& operator<<(std::ostream &os, const SubTask& stage); std::ostream& operator<<(std::ostream &os, const Stage& stage);
class PropagatingEitherWayPrivate; class PropagatingEitherWayPrivate;
class PropagatingEitherWay : public SubTask { class PropagatingEitherWay : public Stage {
public: public:
PRIVATE_CLASS(PropagatingEitherWay) PRIVATE_CLASS(PropagatingEitherWay)
PropagatingEitherWay(const std::string& name); PropagatingEitherWay(const std::string& name);
@ -114,7 +114,7 @@ private:
class GeneratorPrivate; class GeneratorPrivate;
class Generator : public SubTask { class Generator : public Stage {
public: public:
PRIVATE_CLASS(Generator) PRIVATE_CLASS(Generator)
Generator(const std::string& name); Generator(const std::string& name);
@ -126,7 +126,7 @@ public:
class ConnectingPrivate; class ConnectingPrivate;
class Connecting : public SubTask { class Connecting : public Stage {
public: public:
PRIVATE_CLASS(Connecting) PRIVATE_CLASS(Connecting)
Connecting(const std::string& name); Connecting(const std::string& name);

View File

@ -2,7 +2,7 @@
#pragma once #pragma once
#include <moveit_task_constructor/subtask.h> #include <moveit_task_constructor/stage.h>
#include <ros/ros.h> #include <ros/ros.h>
@ -14,7 +14,7 @@ namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface) }
namespace core { MOVEIT_CLASS_FORWARD(RobotState) } namespace core { MOVEIT_CLASS_FORWARD(RobotState) }
} }
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace stages {
class CartesianPositionMotion : public PropagatingEitherWay { class CartesianPositionMotion : public PropagatingEitherWay {
public: public:

View File

@ -2,9 +2,9 @@
#pragma once #pragma once
#include <moveit_task_constructor/subtask.h> #include <moveit_task_constructor/stage.h>
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace stages {
class CurrentState : public Generator { class CurrentState : public Generator {
public: public:

View File

@ -2,7 +2,7 @@
#pragma once #pragma once
#include <moveit_task_constructor/subtask.h> #include <moveit_task_constructor/stage.h>
#include <ros/ros.h> #include <ros/ros.h>
@ -12,7 +12,7 @@ namespace moveit { namespace planning_interface {
MOVEIT_CLASS_FORWARD(MoveGroupInterface); MOVEIT_CLASS_FORWARD(MoveGroupInterface);
} } } }
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace stages {
class GenerateGraspPose : public Generator { class GenerateGraspPose : public Generator {
public: public:

View File

@ -2,13 +2,13 @@
#pragma once #pragma once
#include <moveit_task_constructor/subtask.h> #include <moveit_task_constructor/stage.h>
namespace moveit { namespace planning_interface { namespace moveit { namespace planning_interface {
MOVEIT_CLASS_FORWARD(MoveGroupInterface) MOVEIT_CLASS_FORWARD(MoveGroupInterface)
} } } }
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace stages {
class Gripper : public PropagatingEitherWay { class Gripper : public PropagatingEitherWay {
public: public:

View File

@ -2,13 +2,13 @@
#pragma once #pragma once
#include <moveit_task_constructor/subtask.h> #include <moveit_task_constructor/stage.h>
namespace moveit { namespace moveit {
namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface)} namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface)}
} }
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace stages {
class Move : public Connecting { class Move : public Connecting {
public: public:

View File

@ -22,7 +22,7 @@ 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(Interface)
MOVEIT_CLASS_FORWARD(SubTask) MOVEIT_CLASS_FORWARD(Stage)
/** InterfaceState describes a potential start or goal state for a planning stage. /** InterfaceState describes a potential start or goal state for a planning stage.

View File

@ -14,7 +14,7 @@ namespace moveit { namespace core {
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
MOVEIT_CLASS_FORWARD(SubTask) MOVEIT_CLASS_FORWARD(Stage)
MOVEIT_CLASS_FORWARD(ContainerBase) MOVEIT_CLASS_FORWARD(ContainerBase)
MOVEIT_CLASS_FORWARD(Task) MOVEIT_CLASS_FORWARD(Task)
@ -28,7 +28,7 @@ public:
const std::string &planning_plugin_param_name = "planning_plugin", const std::string &planning_plugin_param_name = "planning_plugin",
const std::string &adapter_plugins_param_name = "request_adapters"); const std::string &adapter_plugins_param_name = "request_adapters");
void add(SubTask::pointer &&stage); void add(Stage::pointer &&stage);
using SerialContainer::clear; using SerialContainer::clear;
bool plan(); bool plan();

View File

@ -19,8 +19,8 @@ ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int before)
return position; return position;
} }
inline bool ContainerBasePrivate::canInsert(const SubTask &stage) const { inline bool ContainerBasePrivate::canInsert(const Stage &stage) const {
const SubTaskPrivate* impl = stage.pimpl(); const StagePrivate* impl = stage.pimpl();
return impl->parent() == nullptr // re-parenting is not supported return impl->parent() == nullptr // re-parenting is not supported
&& impl->trajectories().empty(); // existing trajectories would become invalid && impl->trajectories().empty(); // existing trajectories would become invalid
} }
@ -36,17 +36,17 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
return true; return true;
} }
ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate::value_type &&subtask, ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate::value_type &&stage,
ContainerBasePrivate::const_iterator pos) { ContainerBasePrivate::const_iterator pos) {
SubTaskPrivate *impl = subtask->pimpl(); StagePrivate *impl = stage->pimpl();
ContainerBasePrivate::iterator it = children_.insert(pos, std::move(subtask)); ContainerBasePrivate::iterator it = children_.insert(pos, std::move(stage));
impl->setHierarchy(this, it); impl->setHierarchy(this, it);
return it; return it;
} }
ContainerBase::ContainerBase(ContainerBasePrivate *impl) ContainerBase::ContainerBase(ContainerBasePrivate *impl)
: SubTask(impl) : Stage(impl)
{ {
} }
PIMPL_FUNCTIONS(ContainerBase) PIMPL_FUNCTIONS(ContainerBase)
@ -85,7 +85,7 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s
ends_.reset(new Interface(Interface::NotifyFunction())); ends_.reset(new Interface(Interface::NotifyFunction()));
} }
SubTaskPrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const { StagePrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
InterfaceFlags f; InterfaceFlags f;
if (children().empty()) return f; if (children().empty()) return f;
f |= children().front()->pimpl()->announcedFlags() & INPUT_IF_MASK; f |= children().front()->pimpl()->announcedFlags() & INPUT_IF_MASK;
@ -93,7 +93,7 @@ SubTaskPrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
return f; return f;
} }
inline bool SerialContainerPrivate::canInsert(const SubTask &stage, ContainerBasePrivate::const_iterator before) const { inline bool SerialContainerPrivate::canInsert(const Stage &stage, ContainerBasePrivate::const_iterator before) const {
if (!ContainerBasePrivate::canInsert(stage)) if (!ContainerBasePrivate::canInsert(stage))
return false; return false;
return true; return true;
@ -105,24 +105,24 @@ ContainerBasePrivate::iterator SerialContainerPrivate::insert(value_type &&stage
bool at_begin = (before == children().begin()); bool at_begin = (before == children().begin());
bool at_end = (before == children().end()); bool at_end = (before == children().end());
SubTaskPrivate *cur = stage->pimpl(); StagePrivate *cur = stage->pimpl();
/* set pointer cache (prev_ends_ and next_starts_) of prev, current, and next stage */ /* set pointer cache (prev_ends_ and next_starts_) of prev, current, and next stage */
if (children().empty()) { // first child inserted if (children().empty()) { // first child inserted
cur->setPrevEnds(this->starts()); cur->setPrevEnds(this->starts());
cur->setNextStarts(this->ends()); cur->setNextStarts(this->ends());
} else if (at_begin) { } else if (at_begin) {
SubTaskPrivate *next = (*before)->pimpl(); StagePrivate *next = (*before)->pimpl();
cur->setPrevEnds(this->starts()); cur->setPrevEnds(this->starts());
cur->setNextStarts(next->starts()); cur->setNextStarts(next->starts());
next->setPrevEnds(cur->ends()); next->setPrevEnds(cur->ends());
} else if (at_end) { } else if (at_end) {
SubTaskPrivate *prev = (*this->prev(before))->pimpl(); StagePrivate *prev = (*this->prev(before))->pimpl();
prev->setNextStarts(cur->starts()); prev->setNextStarts(cur->starts());
cur->setPrevEnds(prev->ends()); cur->setPrevEnds(prev->ends());
cur->setNextStarts(this->ends()); cur->setNextStarts(this->ends());
} else { } else {
SubTaskPrivate *prev = (*this->prev(before))->pimpl(); StagePrivate *prev = (*this->prev(before))->pimpl();
SubTaskPrivate *next = (*before)->pimpl(); StagePrivate *next = (*before)->pimpl();
prev->setNextStarts(cur->starts()); prev->setNextStarts(cur->starts());
cur->setPrevEnds(prev->ends()); cur->setPrevEnds(prev->ends());
cur->setNextStarts(next->starts()); cur->setNextStarts(next->starts());
@ -152,21 +152,21 @@ SerialContainer::SerialContainer(const std::string &name)
{} {}
PIMPL_FUNCTIONS(SerialContainer) PIMPL_FUNCTIONS(SerialContainer)
bool SerialContainer::canInsert(const value_type& subtask, int before) const bool SerialContainer::canInsert(const value_type& stage, int before) const
{ {
auto impl = pimpl(); auto impl = pimpl();
return impl->canInsert(*subtask, impl->position(before)); return impl->canInsert(*stage, impl->position(before));
} }
bool SerialContainer::insert(value_type&& subtask, int before) bool SerialContainer::insert(value_type&& stage, int before)
{ {
auto impl = pimpl(); auto impl = pimpl();
ContainerBasePrivate::const_iterator where = impl->position(before); ContainerBasePrivate::const_iterator where = impl->position(before);
if (!impl->canInsert(*subtask, where)) if (!impl->canInsert(*stage, where))
return false; return false;
impl->insert(std::move(subtask), where); impl->insert(std::move(stage), where);
return true; return true;
} }
@ -181,7 +181,7 @@ bool SerialContainerPrivate::compute()
for(const auto& stage : children()) { for(const auto& stage : children()) {
if(!stage->pimpl()->canCompute()) if(!stage->pimpl()->canCompute())
continue; continue;
std::cout << "Computing subtask '" << stage->getName() << "':" << std::endl; std::cout << "Computing stage '" << stage->getName() << "':" << std::endl;
bool success = stage->pimpl()->compute(); bool success = stage->pimpl()->compute();
computed = true; computed = true;
std::cout << (success ? "succeeded" : "failed") << std::endl; std::cout << (success ? "succeeded" : "failed") << std::endl;

View File

@ -1,31 +1,31 @@
#pragma once #pragma once
#include <moveit_task_constructor/container.h> #include <moveit_task_constructor/container.h>
#include "subtask_p.h" #include "stage_p.h"
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
class ContainerBasePrivate : public SubTaskPrivate class ContainerBasePrivate : public StagePrivate
{ {
friend class ContainerBase; friend class ContainerBase;
public: public:
typedef ContainerBase::value_type value_type; typedef ContainerBase::value_type value_type;
typedef SubTaskPrivate::container_type container_type; typedef StagePrivate::container_type container_type;
typedef container_type::iterator iterator; typedef container_type::iterator iterator;
typedef container_type::const_iterator const_iterator; typedef container_type::const_iterator const_iterator;
inline const container_type& children() const { return children_; } inline const container_type& children() const { return children_; }
const_iterator position(int before = -1) const; const_iterator position(int before = -1) const;
bool canInsert(const SubTask& stage) const; bool canInsert(const Stage& stage) const;
virtual iterator insert(value_type &&subtask, const_iterator pos); virtual iterator insert(value_type &&stage, const_iterator pos);
bool traverseStages(const ContainerBase::StageCallback &processor, int depth) const; bool traverseStages(const ContainerBase::StageCallback &processor, int depth) const;
protected: protected:
ContainerBasePrivate(ContainerBase *me, const std::string &name) ContainerBasePrivate(ContainerBase *me, const std::string &name)
: SubTaskPrivate(me, name) : StagePrivate(me, name)
{} {}
private: private:
@ -38,7 +38,7 @@ public:
SerialContainerPrivate(SerialContainer* me, const std::string &name); SerialContainerPrivate(SerialContainer* me, const std::string &name);
InterfaceFlags announcedFlags() const override; InterfaceFlags announcedFlags() const override;
inline bool canInsert(const SubTask& stage, const_iterator before) const; inline bool canInsert(const Stage& stage, const_iterator before) const;
virtual iterator insert(value_type &&stage, const_iterator before) override; virtual iterator insert(value_type &&stage, const_iterator before) override;
bool canCompute() const override; bool canCompute() const override;

View File

@ -2,7 +2,7 @@ add_executable(foo test.cpp)
target_link_libraries(foo) 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}_stages ${PROJECT_NAME})
add_executable(plan_pick_trixi plan_pick_trixi.cpp) add_executable(plan_pick_trixi plan_pick_trixi.cpp)
target_link_libraries(plan_pick_trixi ${PROJECT_NAME}_subtasks ${PROJECT_NAME}) target_link_libraries(plan_pick_trixi ${PROJECT_NAME}_stages ${PROJECT_NAME})

View File

@ -1,11 +1,11 @@
#include <moveit_task_constructor/task.h> #include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/debug.h> #include <moveit_task_constructor/debug.h>
#include <moveit_task_constructor/subtasks/current_state.h> #include <moveit_task_constructor/stages/current_state.h>
#include <moveit_task_constructor/subtasks/gripper.h> #include <moveit_task_constructor/stages/gripper.h>
#include <moveit_task_constructor/subtasks/move.h> #include <moveit_task_constructor/stages/move.h>
#include <moveit_task_constructor/subtasks/generate_grasp_pose.h> #include <moveit_task_constructor/stages/generate_grasp_pose.h>
#include <moveit_task_constructor/subtasks/cartesian_position_motion.h> #include <moveit_task_constructor/stages/cartesian_position_motion.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <moveit_msgs/CollisionObject.h> #include <moveit_msgs/CollisionObject.h>
@ -43,17 +43,17 @@ int main(int argc, char** argv){
Task t; Task t;
t.add( std::make_unique<subtasks::CurrentState>("current state") ); t.add( std::make_unique<stages::CurrentState>("current state") );
{ {
auto move= std::make_unique<subtasks::Gripper>("open gripper"); auto move= std::make_unique<stages::Gripper>("open gripper");
move->setEndEffector("left_gripper"); move->setEndEffector("left_gripper");
move->setTo("open"); move->setTo("open");
t.add(std::move(move)); t.add(std::move(move));
} }
{ {
auto move= std::make_unique<subtasks::Move>("move to pre-grasp"); auto move= std::make_unique<stages::Move>("move to pre-grasp");
move->setGroup("left_arm"); move->setGroup("left_arm");
move->setPlannerId("RRTConnectkConfigDefault"); move->setPlannerId("RRTConnectkConfigDefault");
move->setTimeout(8.0); move->setTimeout(8.0);
@ -61,7 +61,7 @@ int main(int argc, char** argv){
} }
{ {
auto move= std::make_unique<subtasks::CartesianPositionMotion>("proceed to grasp pose"); auto move= std::make_unique<stages::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);
@ -74,7 +74,7 @@ int main(int argc, char** argv){
} }
{ {
auto gengrasp= std::make_unique<subtasks::GenerateGraspPose>("generate grasp pose"); auto gengrasp= std::make_unique<stages::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");
@ -86,7 +86,7 @@ int main(int argc, char** argv){
} }
{ {
auto move= std::make_unique<subtasks::Gripper>("grasp"); auto move= std::make_unique<stages::Gripper>("grasp");
move->setEndEffector("left_gripper"); move->setEndEffector("left_gripper");
move->setTo("closed"); move->setTo("closed");
move->graspObject("object"); move->graspObject("object");
@ -94,7 +94,7 @@ int main(int argc, char** argv){
} }
{ {
auto move= std::make_unique<subtasks::CartesianPositionMotion>("lift object"); auto move= std::make_unique<stages::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);

View File

@ -1,11 +1,11 @@
#include <moveit_task_constructor/task.h> #include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/debug.h> #include <moveit_task_constructor/debug.h>
#include <moveit_task_constructor/subtasks/current_state.h> #include <moveit_task_constructor/stages/current_state.h>
#include <moveit_task_constructor/subtasks/gripper.h> #include <moveit_task_constructor/stages/gripper.h>
#include <moveit_task_constructor/subtasks/move.h> #include <moveit_task_constructor/stages/move.h>
#include <moveit_task_constructor/subtasks/generate_grasp_pose.h> #include <moveit_task_constructor/stages/generate_grasp_pose.h>
#include <moveit_task_constructor/subtasks/cartesian_position_motion.h> #include <moveit_task_constructor/stages/cartesian_position_motion.h>
#include <ros/ros.h> #include <ros/ros.h>
@ -41,17 +41,17 @@ int main(int argc, char** argv){
Task t; Task t;
t.add(std::make_unique<subtasks::CurrentState>("current state")); t.add(std::make_unique<stages::CurrentState>("current state"));
{ {
auto move = std::make_unique<subtasks::Gripper>("open gripper"); auto move = std::make_unique<stages::Gripper>("open gripper");
move->setEndEffector("gripper"); move->setEndEffector("gripper");
move->setTo("open"); move->setTo("open");
t.add(std::move(std::move(move))); t.add(std::move(std::move(move)));
} }
{ {
auto move = std::make_unique<subtasks::Move>("move to pre-grasp"); auto move = std::make_unique<stages::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);
@ -59,7 +59,7 @@ int main(int argc, char** argv){
} }
{ {
auto move = std::make_unique<subtasks::CartesianPositionMotion>("proceed to grasp pose"); auto move = std::make_unique<stages::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);
@ -72,7 +72,7 @@ int main(int argc, char** argv){
} }
{ {
auto gengrasp = std::make_unique<subtasks::GenerateGraspPose>("generate grasp pose"); auto gengrasp = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
gengrasp->setEndEffector("gripper"); gengrasp->setEndEffector("gripper");
//gengrasp->setGroup("arm"); //gengrasp->setGroup("arm");
gengrasp->setGripperGraspPose("open"); gengrasp->setGripperGraspPose("open");
@ -84,7 +84,7 @@ int main(int argc, char** argv){
} }
{ {
auto move = std::make_unique<subtasks::Gripper>("grasp"); auto move = std::make_unique<stages::Gripper>("grasp");
move->setEndEffector("gripper"); move->setEndEffector("gripper");
move->setTo("closed"); move->setTo("closed");
move->graspObject("object"); move->graspObject("object");
@ -92,7 +92,7 @@ int main(int argc, char** argv){
} }
{ {
auto move = std::make_unique<subtasks::CartesianPositionMotion>("lift object"); auto move = std::make_unique<stages::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);

View File

@ -1,4 +1,4 @@
#include "subtask_p.h" #include "stage_p.h"
#include "container_p.h" #include "container_p.h"
#include <iostream> #include <iostream>
#include <iomanip> #include <iomanip>
@ -6,42 +6,42 @@
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
SubTask::SubTask(SubTaskPrivate *impl) Stage::Stage(StagePrivate *impl)
: pimpl_(impl) : pimpl_(impl)
{ {
} }
SubTask::~SubTask() Stage::~Stage()
{ {
delete pimpl_; delete pimpl_;
} }
bool SubTask::init(const planning_scene::PlanningSceneConstPtr &scene) bool Stage::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
} }
const std::string& SubTask::getName() const { const std::string& Stage::getName() const {
return pimpl_->name_; return pimpl_->name_;
} }
std::ostream& operator<<(std::ostream &os, const SubTask& stage) { std::ostream& operator<<(std::ostream &os, const Stage& stage) {
os << *stage.pimpl(); os << *stage.pimpl();
return os; return os;
} }
template<SubTaskPrivate::InterfaceFlag own, SubTaskPrivate::InterfaceFlag other> template<StagePrivate::InterfaceFlag own, StagePrivate::InterfaceFlag other>
const char* direction(const SubTaskPrivate& stage) { const char* direction(const StagePrivate& stage) {
SubTaskPrivate::InterfaceFlags f = stage.deducedFlags(); StagePrivate::InterfaceFlags f = stage.deducedFlags();
bool own_if = f & own; bool own_if = f & own;
bool other_if = f & other; bool other_if = f & other;
bool reverse = own & SubTaskPrivate::INPUT_IF_MASK; bool reverse = own & StagePrivate::INPUT_IF_MASK;
if (own_if && other_if) return "<>"; if (own_if && other_if) return "<>";
if (!own_if && !other_if) return "--"; if (!own_if && !other_if) return "--";
if (other_if ^ reverse) return "->"; if (other_if ^ reverse) return "->";
return "<-"; return "<-";
}; };
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) { std::ostream& operator<<(std::ostream &os, const StagePrivate& stage) {
// starts // starts
for (const Interface* i : {stage.prev_ends_, stage.starts_.get()}) { for (const Interface* i : {stage.prev_ends_, stage.starts_.get()}) {
os << std::setw(3); os << std::setw(3);
@ -49,9 +49,9 @@ std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
else os << "-"; else os << "-";
} }
// trajectories // trajectories
os << std::setw(5) << direction<SubTaskPrivate::READS_START, SubTaskPrivate::WRITES_PREV_END>(stage) os << std::setw(5) << direction<StagePrivate::READS_START, StagePrivate::WRITES_PREV_END>(stage)
<< std::setw(3) << stage.trajectories_.size() << std::setw(3) << stage.trajectories_.size()
<< std::setw(5) << direction<SubTaskPrivate::READS_END, SubTaskPrivate::WRITES_NEXT_START>(stage); << std::setw(5) << direction<StagePrivate::READS_END, StagePrivate::WRITES_NEXT_START>(stage);
// ends // ends
for (const Interface* i : {stage.ends_.get(), stage.next_starts_}) { for (const Interface* i : {stage.ends_.get(), stage.next_starts_}) {
os << std::setw(3); os << std::setw(3);
@ -64,17 +64,17 @@ std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
} }
SubTaskPrivate::SubTaskPrivate(SubTask *me, const std::string &name) StagePrivate::StagePrivate(Stage *me, const std::string &name)
: me_(me), name_(name), parent_(nullptr), prev_ends_(nullptr), next_starts_(nullptr) : me_(me), name_(name), parent_(nullptr), prev_ends_(nullptr), next_starts_(nullptr)
{} {}
SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){ SubTrajectory& StagePrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
trajectories_.emplace_back(trajectory); trajectories_.emplace_back(trajectory);
SubTrajectory& back = trajectories_.back(); SubTrajectory& back = trajectories_.back();
return back; return back;
} }
SubTaskPrivate::InterfaceFlags SubTaskPrivate::interfaceFlags() const StagePrivate::InterfaceFlags StagePrivate::interfaceFlags() const
{ {
InterfaceFlags result = announcedFlags(); InterfaceFlags result = announcedFlags();
result &= ~InterfaceFlags(OWN_IF_MASK); result &= ~InterfaceFlags(OWN_IF_MASK);
@ -83,7 +83,7 @@ SubTaskPrivate::InterfaceFlags SubTaskPrivate::interfaceFlags() const
} }
// return the interface flags that can be deduced from the interface // return the interface flags that can be deduced from the interface
inline SubTaskPrivate::InterfaceFlags SubTaskPrivate::deducedFlags() const inline StagePrivate::InterfaceFlags StagePrivate::deducedFlags() const
{ {
InterfaceFlags f; InterfaceFlags f;
if (starts_) f |= READS_START; if (starts_) f |= READS_START;
@ -95,11 +95,11 @@ inline SubTaskPrivate::InterfaceFlags SubTaskPrivate::deducedFlags() const
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name) PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name)
: SubTaskPrivate(me, name), dir(dir) : StagePrivate(me, name), dir(dir)
{ {
} }
SubTaskPrivate::InterfaceFlags PropagatingEitherWayPrivate::announcedFlags() const { StagePrivate::InterfaceFlags PropagatingEitherWayPrivate::announcedFlags() const {
InterfaceFlags f; InterfaceFlags f;
if (dir & PropagatingEitherWay::FORWARD) if (dir & PropagatingEitherWay::FORWARD)
f |= InterfaceFlags({READS_START, WRITES_NEXT_START}); f |= InterfaceFlags({READS_START, WRITES_NEXT_START});
@ -187,7 +187,7 @@ PropagatingEitherWay::PropagatingEitherWay(const std::string &name)
} }
PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate *impl) PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate *impl)
: SubTask(impl) : Stage(impl)
{ {
initInterface(); initInterface();
} }
@ -292,10 +292,10 @@ bool PropagatingBackward::computeForward(const InterfaceState &from)
GeneratorPrivate::GeneratorPrivate(Generator *me, const std::string &name) GeneratorPrivate::GeneratorPrivate(Generator *me, const std::string &name)
: SubTaskPrivate(me, name) : StagePrivate(me, name)
{} {}
SubTaskPrivate::InterfaceFlags GeneratorPrivate::announcedFlags() const { StagePrivate::InterfaceFlags GeneratorPrivate::announcedFlags() const {
return InterfaceFlags({WRITES_NEXT_START,WRITES_PREV_END}); return InterfaceFlags({WRITES_NEXT_START,WRITES_PREV_END});
} }
@ -309,7 +309,7 @@ bool GeneratorPrivate::compute() {
Generator::Generator(const std::string &name) Generator::Generator(const std::string &name)
: SubTask(new GeneratorPrivate(this, name)) : Stage(new GeneratorPrivate(this, name))
{} {}
PIMPL_FUNCTIONS(Generator) PIMPL_FUNCTIONS(Generator)
@ -326,14 +326,14 @@ void Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double co
ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name) ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
: SubTaskPrivate(me, name) : StagePrivate(me, name)
{ {
starts_.reset(new Interface([this](const Interface::iterator& it) { this->newStartState(it); })); starts_.reset(new Interface([this](const Interface::iterator& it) { this->newStartState(it); }));
ends_.reset(new Interface([this](const Interface::iterator& it) { this->newEndState(it); })); ends_.reset(new Interface([this](const Interface::iterator& it) { this->newEndState(it); }));
it_pairs_ = std::make_pair(starts_->begin(), ends_->begin()); it_pairs_ = std::make_pair(starts_->begin(), ends_->begin());
} }
SubTaskPrivate::InterfaceFlags ConnectingPrivate::announcedFlags() const { StagePrivate::InterfaceFlags ConnectingPrivate::announcedFlags() const {
return InterfaceFlags({READS_START, READS_END}); return InterfaceFlags({READS_START, READS_END});
} }
@ -366,7 +366,7 @@ bool ConnectingPrivate::compute() {
Connecting::Connecting(const std::string &name) Connecting::Connecting(const std::string &name)
: SubTask(new ConnectingPrivate(this, name)) : Stage(new ConnectingPrivate(this, name))
{ {
} }
PIMPL_FUNCTIONS(Connecting) PIMPL_FUNCTIONS(Connecting)

View File

@ -2,7 +2,7 @@
#pragma once #pragma once
#include <moveit_task_constructor/subtask.h> #include <moveit_task_constructor/stage.h>
#include <moveit_task_constructor/storage.h> #include <moveit_task_constructor/storage.h>
// define pimpl() functions accessing correctly casted pimpl_ pointer // define pimpl() functions accessing correctly casted pimpl_ pointer
@ -13,14 +13,14 @@
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
class ContainerBasePrivate; class ContainerBasePrivate;
class SubTaskPrivate { class StagePrivate {
friend class SubTask; friend class Stage;
friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage); friend std::ostream& operator<<(std::ostream &os, const StagePrivate& stage);
public: public:
typedef std::list<SubTask::pointer> container_type; typedef std::list<Stage::pointer> container_type;
SubTaskPrivate(SubTask* me, const std::string& name); StagePrivate(Stage* me, const std::string& name);
enum InterfaceFlag { enum InterfaceFlag {
READS_START = 0x01, READS_START = 0x01,
@ -62,7 +62,7 @@ public:
inline void setNextStarts(Interface * next_starts) { next_starts_ = next_starts; } inline void setNextStarts(Interface * next_starts) { next_starts_ = next_starts; }
protected: protected:
SubTask* const me_; // associated/owning SubTask instance Stage* const me_; // associated/owning Stage instance
const std::string name_; const std::string name_;
InterfacePtr starts_; InterfacePtr starts_;
@ -77,10 +77,10 @@ private:
Interface *prev_ends_; // interface to be used for sendBackward() Interface *prev_ends_; // interface to be used for sendBackward()
Interface *next_starts_; // interface to be used for sendForward() Interface *next_starts_; // interface to be used for sendForward()
}; };
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage); std::ostream& operator<<(std::ostream &os, const StagePrivate& stage);
class PropagatingEitherWayPrivate : public SubTaskPrivate { class PropagatingEitherWayPrivate : public StagePrivate {
friend class PropagatingEitherWay; friend class PropagatingEitherWay;
public: public:
@ -121,7 +121,7 @@ public:
}; };
class GeneratorPrivate : public SubTaskPrivate { class GeneratorPrivate : public StagePrivate {
public: public:
inline GeneratorPrivate(Generator *me, const std::string &name); inline GeneratorPrivate(Generator *me, const std::string &name);
InterfaceFlags announcedFlags() const override; InterfaceFlags announcedFlags() const override;
@ -131,7 +131,7 @@ public:
}; };
class ConnectingPrivate : public SubTaskPrivate { class ConnectingPrivate : public StagePrivate {
friend class Connecting; friend class Connecting;
public: public:
@ -152,6 +152,6 @@ private:
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_; std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
}; };
PIMPL_FUNCTIONS(SubTask) PIMPL_FUNCTIONS(Stage)
} } } }

14
src/stages/CMakeLists.txt Normal file
View File

@ -0,0 +1,14 @@
add_library(${PROJECT_NAME}_stages
move.cpp
current_state.cpp
gripper.cpp
generate_grasp_pose.cpp
cartesian_position_motion.cpp
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/stages/move.h
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/stages/current_state.h
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/stages/gripper.h
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/stages/generate_grasp_pose.h
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/stages/cartesian_position_motion.h
)
target_link_libraries(${PROJECT_NAME}_stages ${catkin_LIBRARIES})

View File

@ -1,4 +1,4 @@
#include <moveit_task_constructor/subtasks/cartesian_position_motion.h> #include <moveit_task_constructor/stages/cartesian_position_motion.h>
#include <moveit_task_constructor/storage.h> #include <moveit_task_constructor/storage.h>
#include <ros/ros.h> #include <ros/ros.h>
@ -13,7 +13,7 @@
#include <Eigen/Geometry> #include <Eigen/Geometry>
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace stages {
CartesianPositionMotion::CartesianPositionMotion(std::string name) CartesianPositionMotion::CartesianPositionMotion(std::string name)
: PropagatingEitherWay(name), : PropagatingEitherWay(name),

View File

@ -1,6 +1,6 @@
#include <moveit_task_constructor/subtasks/current_state.h> #include <moveit_task_constructor/stages/current_state.h>
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace stages {
CurrentState::CurrentState(std::string name) CurrentState::CurrentState(std::string name)
: Generator(name) : Generator(name)

View File

@ -1,4 +1,4 @@
#include <moveit_task_constructor/subtasks/generate_grasp_pose.h> #include <moveit_task_constructor/stages/generate_grasp_pose.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <moveit_msgs/DisplayRobotState.h> #include <moveit_msgs/DisplayRobotState.h>
@ -15,7 +15,7 @@
#include <chrono> #include <chrono>
#include <functional> #include <functional>
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace stages {
GenerateGraspPose::GenerateGraspPose(std::string name) GenerateGraspPose::GenerateGraspPose(std::string name)
: Generator(name), : Generator(name),

View File

@ -1,4 +1,4 @@
#include <moveit_task_constructor/subtasks/gripper.h> #include <moveit_task_constructor/stages/gripper.h>
#include <moveit_task_constructor/storage.h> #include <moveit_task_constructor/storage.h>
#include <moveit_task_constructor/task.h> #include <moveit_task_constructor/task.h>
@ -11,7 +11,7 @@
#include <moveit_msgs/MotionPlanRequest.h> #include <moveit_msgs/MotionPlanRequest.h>
#include <moveit_msgs/MotionPlanResponse.h> #include <moveit_msgs/MotionPlanResponse.h>
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace stages {
Gripper::Gripper(std::string name) Gripper::Gripper(std::string name)
: PropagatingEitherWay(name) : PropagatingEitherWay(name)

View File

@ -1,4 +1,4 @@
#include <moveit_task_constructor/subtasks/move.h> #include <moveit_task_constructor/stages/move.h>
#include <moveit_task_constructor/storage.h> #include <moveit_task_constructor/storage.h>
#include <moveit_task_constructor/task.h> #include <moveit_task_constructor/task.h>
@ -11,7 +11,7 @@
#include <moveit_msgs/MotionPlanRequest.h> #include <moveit_msgs/MotionPlanRequest.h>
#include <moveit_msgs/MotionPlanResponse.h> #include <moveit_msgs/MotionPlanResponse.h>
namespace moveit { namespace task_constructor { namespace subtasks { namespace moveit { namespace task_constructor { namespace stages {
Move::Move(std::string name) Move::Move(std::string name)
: Connecting(name), : Connecting(name),
@ -50,7 +50,7 @@ bool Move::compute(const InterfaceState &from, const InterfaceState &to) {
if(!planner_->generatePlan(from.state, req, res)) if(!planner_->generatePlan(from.state, req, res))
return false; return false;
// finish subtask // finish stage
connect(from, to, res.trajectory_); connect(from, to, res.trajectory_);
return true; return true;

View File

@ -1,14 +0,0 @@
add_library(${PROJECT_NAME}_subtasks
move.cpp
current_state.cpp
gripper.cpp
generate_grasp_pose.cpp
cartesian_position_motion.cpp
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/move.h
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/current_state.h
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/gripper.h
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/generate_grasp_pose.h
${CMAKE_SOURCE_DIR}/include/${PROJECT_NAME}/subtasks/cartesian_position_motion.h
)
target_link_libraries(${PROJECT_NAME}_subtasks ${catkin_LIBRARIES})

View File

@ -118,7 +118,7 @@ const robot_state::RobotState& Task::getCurrentRobotState() const {
} }
void Task::printState(){ void Task::printState(){
ContainerBase::StageCallback processor = [](const SubTask& stage, int depth) -> bool { ContainerBase::StageCallback processor = [](const Stage& stage, int depth) -> bool {
std::cout << std::string(2*depth, ' ') << stage << std::endl; std::cout << std::string(2*depth, ' ') << stage << std::endl;
}; };
traverseStages(processor); traverseStages(processor);

View File

@ -12,13 +12,13 @@ if (CATKIN_ENABLE_TESTING)
endif() 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}_stages ${PROJECT_NAME})
add_executable(test_plan_gripper test_plan_gripper.cpp) add_executable(test_plan_gripper test_plan_gripper.cpp)
target_link_libraries(test_plan_gripper ${PROJECT_NAME}_subtasks ${PROJECT_NAME}) target_link_libraries(test_plan_gripper ${PROJECT_NAME}_stages ${PROJECT_NAME})
add_executable(test_plan_generate_grasp_pose test_plan_generate_grasp_pose.cpp) add_executable(test_plan_generate_grasp_pose test_plan_generate_grasp_pose.cpp)
target_link_libraries(test_plan_generate_grasp_pose ${PROJECT_NAME}_subtasks ${PROJECT_NAME}) target_link_libraries(test_plan_generate_grasp_pose ${PROJECT_NAME}_stages ${PROJECT_NAME})
add_executable(test_plan_cartesian_forward test_plan_cartesian_forward.cpp) add_executable(test_plan_cartesian_forward test_plan_cartesian_forward.cpp)
target_link_libraries(test_plan_cartesian_forward ${PROJECT_NAME}_subtasks ${PROJECT_NAME}) target_link_libraries(test_plan_cartesian_forward ${PROJECT_NAME}_stages ${PROJECT_NAME})

View File

@ -1,5 +1,5 @@
#include <container_p.h> #include <container_p.h>
#include <subtask_p.h> #include <stage_p.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <initializer_list> #include <initializer_list>
@ -44,7 +44,7 @@ protected:
void SetUp() {} void SetUp() {}
void TearDown() {} void TearDown() {}
void validateOrder(const SerialContainerPrivate* container, const std::initializer_list<SubTaskPrivate*> &expected) { void validateOrder(const SerialContainerPrivate* container, const std::initializer_list<StagePrivate*> &expected) {
size_t num = container->children().size(); size_t num = container->children().size();
ASSERT_TRUE(num == expected.size()) << "different list lengths"; ASSERT_TRUE(num == expected.size()) << "different list lengths";
@ -61,7 +61,7 @@ protected:
size_t pos = 0; size_t pos = 0;
auto exp_it = expected.begin(); auto exp_it = expected.begin();
for (auto it = container->children().begin(), end = container->children().end(); it != end; ++it, ++exp_it, ++pos) { for (auto it = container->children().begin(), end = container->children().end(); it != end; ++it, ++exp_it, ++pos) {
SubTaskPrivate *child = (*it)->pimpl(); StagePrivate *child = (*it)->pimpl();
EXPECT_EQ(child, *exp_it) << "wrong order"; EXPECT_EQ(child, *exp_it) << "wrong order";
EXPECT_EQ(child->parent(), container) << "wrong parent"; EXPECT_EQ(child->parent(), container) << "wrong parent";
EXPECT_EQ(it, container->position(pos)) << "bad forward position resolution"; EXPECT_EQ(it, container->position(pos)) << "bad forward position resolution";
@ -73,8 +73,8 @@ protected:
TEST_F(BaseTest, interfaceFlags) { TEST_F(BaseTest, interfaceFlags) {
std::unique_ptr<Generator> g = std::make_unique<TestGenerator>(); std::unique_ptr<Generator> g = std::make_unique<TestGenerator>();
EXPECT_EQ(g->pimpl()->interfaceFlags(), EXPECT_EQ(g->pimpl()->interfaceFlags(),
SubTaskPrivate::InterfaceFlags({SubTaskPrivate::WRITES_NEXT_START, StagePrivate::InterfaceFlags({StagePrivate::WRITES_NEXT_START,
SubTaskPrivate::WRITES_PREV_END})); StagePrivate::WRITES_PREV_END}));
} }
#define VALIDATE(...) \ #define VALIDATE(...) \

View File

@ -2,8 +2,8 @@
#include <moveit_task_constructor/task.h> #include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/subtasks/current_state.h> #include <moveit_task_constructor/stages/current_state.h>
#include <moveit_task_constructor/subtasks/cartesian_position_motion.h> #include <moveit_task_constructor/stages/cartesian_position_motion.h>
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
@ -12,10 +12,10 @@ int main(int argc, char** argv){
Task t; Task t;
t.add( std::make_unique<subtasks::CurrentState>("current state") ); t.add( std::make_unique<stages::CurrentState>("current state") );
{ {
auto move= std::make_unique<subtasks::CartesianPositionMotion>("lift object"); auto move= std::make_unique<stages::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);
@ -29,7 +29,7 @@ int main(int argc, char** argv){
} }
{ {
auto move= std::make_unique<subtasks::CartesianPositionMotion>("lift object"); auto move= std::make_unique<stages::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);

View File

@ -2,7 +2,7 @@
#include <moveit_task_constructor/task.h> #include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/subtasks/current_state.h> #include <moveit_task_constructor/stages/current_state.h>
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
@ -11,7 +11,7 @@ int main(int argc, char** argv){
Task t; Task t;
t.add( std::make_unique<subtasks::CurrentState>("current state") ); t.add( std::make_unique<stages::CurrentState>("current state") );
t.plan(); t.plan();

View File

@ -1,5 +1,5 @@
#include <moveit_task_constructor/task.h> #include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/subtasks/generate_grasp_pose.h> #include <moveit_task_constructor/stages/generate_grasp_pose.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <moveit_msgs/CollisionObject.h> #include <moveit_msgs/CollisionObject.h>
@ -36,7 +36,7 @@ int main(int argc, char** argv){
Task t; Task t;
auto st= std::make_unique<subtasks::GenerateGraspPose>("generate grasp candidates"); auto st= std::make_unique<stages::GenerateGraspPose>("generate grasp candidates");
st->setEndEffector("gripper"); st->setEndEffector("gripper");
//st->setGroup("arm"); //st->setGroup("arm");

View File

@ -2,8 +2,8 @@
#include <moveit_task_constructor/task.h> #include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/subtasks/current_state.h> #include <moveit_task_constructor/stages/current_state.h>
#include <moveit_task_constructor/subtasks/gripper.h> #include <moveit_task_constructor/stages/gripper.h>
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
@ -14,9 +14,9 @@ int main(int argc, char** argv){
Task t; Task t;
t.add( std::make_unique<subtasks::CurrentState>("current state") ); t.add( std::make_unique<stages::CurrentState>("current state") );
{ {
auto gripper= std::make_unique<subtasks::Gripper>("close gripper"); auto gripper= std::make_unique<stages::Gripper>("close gripper");
gripper->setEndEffector("gripper"); gripper->setEndEffector("gripper");
gripper->setTo("closed"); gripper->setTo("closed");
t.add(std::move( gripper )); t.add(std::move( gripper ));
@ -26,12 +26,12 @@ int main(int argc, char** argv){
/*TODO currently not implemented in gripper*/ /*TODO currently not implemented in gripper*/
/* /*
{ {
auto gripper= std::make_unique<subtasks::Gripper>("close gripper"); auto gripper= std::make_unique<stages::Gripper>("close gripper");
gripper->setEndEffector("gripper"); gripper->setEndEffector("gripper");
gripper->setTo("closed"); gripper->setTo("closed");
t.add(std::move( gripper )); t.add(std::move( gripper ));
} }
t.add( std::make_unique<subtasks::CurrentState>("current state") ); t.add( std::make_unique<stages::CurrentState>("current state") );
t.plan(); t.plan();
*/ */
return 0; return 0;