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}
src/storage.cpp
src/subtask.cpp
src/stage.cpp
src/container.cpp
src/task.cpp
src/debug.cpp
src/subtask_p.h
src/stage_p.h
src/container_p.h
include/${PROJECT_NAME}/utils.h
include/${PROJECT_NAME}/storage.h
include/${PROJECT_NAME}/subtask.h
include/${PROJECT_NAME}/stage.h
include/${PROJECT_NAME}/container.h
include/${PROJECT_NAME}/task.h
include/${PROJECT_NAME}/debug.h
)
add_subdirectory(src/subtasks)
add_subdirectory(src/stages)
add_subdirectory(src/demo)
add_subdirectory(src/test)

View File

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

View File

@ -2,7 +2,7 @@
#pragma once
#include <moveit_task_constructor/subtask.h>
#include <moveit_task_constructor/stage.h>
#include <ros/ros.h>
@ -14,7 +14,7 @@ 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 stages {
class CartesianPositionMotion : public PropagatingEitherWay {
public:

View File

@ -2,9 +2,9 @@
#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 {
public:

View File

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

View File

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

View File

@ -2,13 +2,13 @@
#pragma once
#include <moveit_task_constructor/subtask.h>
#include <moveit_task_constructor/stage.h>
namespace moveit {
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 {
public:

View File

@ -22,7 +22,7 @@ namespace moveit { namespace task_constructor {
MOVEIT_CLASS_FORWARD(SubTrajectory)
MOVEIT_CLASS_FORWARD(InterfaceState)
MOVEIT_CLASS_FORWARD(Interface)
MOVEIT_CLASS_FORWARD(SubTask)
MOVEIT_CLASS_FORWARD(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 {
MOVEIT_CLASS_FORWARD(SubTask)
MOVEIT_CLASS_FORWARD(Stage)
MOVEIT_CLASS_FORWARD(ContainerBase)
MOVEIT_CLASS_FORWARD(Task)
@ -28,7 +28,7 @@ public:
const std::string &planning_plugin_param_name = "planning_plugin",
const std::string &adapter_plugins_param_name = "request_adapters");
void add(SubTask::pointer &&stage);
void add(Stage::pointer &&stage);
using SerialContainer::clear;
bool plan();

View File

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

View File

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

View File

@ -2,7 +2,7 @@ add_executable(foo test.cpp)
target_link_libraries(foo)
add_executable(plan_pick_ur5 plan_pick_ur5.cpp)
target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_subtasks ${PROJECT_NAME})
target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_stages ${PROJECT_NAME})
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/debug.h>
#include <moveit_task_constructor/subtasks/current_state.h>
#include <moveit_task_constructor/subtasks/gripper.h>
#include <moveit_task_constructor/subtasks/move.h>
#include <moveit_task_constructor/subtasks/generate_grasp_pose.h>
#include <moveit_task_constructor/subtasks/cartesian_position_motion.h>
#include <moveit_task_constructor/stages/current_state.h>
#include <moveit_task_constructor/stages/gripper.h>
#include <moveit_task_constructor/stages/move.h>
#include <moveit_task_constructor/stages/generate_grasp_pose.h>
#include <moveit_task_constructor/stages/cartesian_position_motion.h>
#include <ros/ros.h>
#include <moveit_msgs/CollisionObject.h>
@ -43,17 +43,17 @@ int main(int argc, char** argv){
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->setTo("open");
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->setPlannerId("RRTConnectkConfigDefault");
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->setLink("l_gripper_tool_frame");
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->setGroup("arm");
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->setTo("closed");
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->setLink("l_gripper_tool_frame");
move->setMinMaxDistance(0.03, 0.05);

View File

@ -1,11 +1,11 @@
#include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/debug.h>
#include <moveit_task_constructor/subtasks/current_state.h>
#include <moveit_task_constructor/subtasks/gripper.h>
#include <moveit_task_constructor/subtasks/move.h>
#include <moveit_task_constructor/subtasks/generate_grasp_pose.h>
#include <moveit_task_constructor/subtasks/cartesian_position_motion.h>
#include <moveit_task_constructor/stages/current_state.h>
#include <moveit_task_constructor/stages/gripper.h>
#include <moveit_task_constructor/stages/move.h>
#include <moveit_task_constructor/stages/generate_grasp_pose.h>
#include <moveit_task_constructor/stages/cartesian_position_motion.h>
#include <ros/ros.h>
@ -41,17 +41,17 @@ int main(int argc, char** argv){
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->setTo("open");
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->setPlannerId("RRTConnectkConfigDefault");
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->setLink("s_model_tool0");
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->setGroup("arm");
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->setTo("closed");
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->setLink("s_model_tool0");
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 <iostream>
#include <iomanip>
@ -6,42 +6,42 @@
namespace moveit { namespace task_constructor {
SubTask::SubTask(SubTaskPrivate *impl)
Stage::Stage(StagePrivate *impl)
: pimpl_(impl)
{
}
SubTask::~SubTask()
Stage::~Stage()
{
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_;
}
std::ostream& operator<<(std::ostream &os, const SubTask& stage) {
std::ostream& operator<<(std::ostream &os, const Stage& stage) {
os << *stage.pimpl();
return os;
}
template<SubTaskPrivate::InterfaceFlag own, SubTaskPrivate::InterfaceFlag other>
const char* direction(const SubTaskPrivate& stage) {
SubTaskPrivate::InterfaceFlags f = stage.deducedFlags();
template<StagePrivate::InterfaceFlag own, StagePrivate::InterfaceFlag other>
const char* direction(const StagePrivate& stage) {
StagePrivate::InterfaceFlags f = stage.deducedFlags();
bool own_if = f & own;
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 (other_if ^ reverse) return "->";
return "<-";
};
std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
std::ostream& operator<<(std::ostream &os, const StagePrivate& stage) {
// starts
for (const Interface* i : {stage.prev_ends_, stage.starts_.get()}) {
os << std::setw(3);
@ -49,9 +49,9 @@ std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage) {
else os << "-";
}
// 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(5) << direction<SubTaskPrivate::READS_END, SubTaskPrivate::WRITES_NEXT_START>(stage);
<< std::setw(5) << direction<StagePrivate::READS_END, StagePrivate::WRITES_NEXT_START>(stage);
// ends
for (const Interface* i : {stage.ends_.get(), stage.next_starts_}) {
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)
{}
SubTrajectory& SubTaskPrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
SubTrajectory& StagePrivate::addTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory, double cost){
trajectories_.emplace_back(trajectory);
SubTrajectory& back = trajectories_.back();
return back;
}
SubTaskPrivate::InterfaceFlags SubTaskPrivate::interfaceFlags() const
StagePrivate::InterfaceFlags StagePrivate::interfaceFlags() const
{
InterfaceFlags result = announcedFlags();
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
inline SubTaskPrivate::InterfaceFlags SubTaskPrivate::deducedFlags() const
inline StagePrivate::InterfaceFlags StagePrivate::deducedFlags() const
{
InterfaceFlags f;
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)
: SubTaskPrivate(me, name), dir(dir)
: StagePrivate(me, name), dir(dir)
{
}
SubTaskPrivate::InterfaceFlags PropagatingEitherWayPrivate::announcedFlags() const {
StagePrivate::InterfaceFlags PropagatingEitherWayPrivate::announcedFlags() const {
InterfaceFlags f;
if (dir & PropagatingEitherWay::FORWARD)
f |= InterfaceFlags({READS_START, WRITES_NEXT_START});
@ -187,7 +187,7 @@ PropagatingEitherWay::PropagatingEitherWay(const std::string &name)
}
PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate *impl)
: SubTask(impl)
: Stage(impl)
{
initInterface();
}
@ -292,10 +292,10 @@ bool PropagatingBackward::computeForward(const InterfaceState &from)
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});
}
@ -309,7 +309,7 @@ bool GeneratorPrivate::compute() {
Generator::Generator(const std::string &name)
: SubTask(new GeneratorPrivate(this, name))
: Stage(new GeneratorPrivate(this, name))
{}
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)
: SubTaskPrivate(me, name)
: StagePrivate(me, name)
{
starts_.reset(new Interface([this](const Interface::iterator& it) { this->newStartState(it); }));
ends_.reset(new Interface([this](const Interface::iterator& it) { this->newEndState(it); }));
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});
}
@ -366,7 +366,7 @@ bool ConnectingPrivate::compute() {
Connecting::Connecting(const std::string &name)
: SubTask(new ConnectingPrivate(this, name))
: Stage(new ConnectingPrivate(this, name))
{
}
PIMPL_FUNCTIONS(Connecting)

View File

@ -2,7 +2,7 @@
#pragma once
#include <moveit_task_constructor/subtask.h>
#include <moveit_task_constructor/stage.h>
#include <moveit_task_constructor/storage.h>
// define pimpl() functions accessing correctly casted pimpl_ pointer
@ -13,14 +13,14 @@
namespace moveit { namespace task_constructor {
class ContainerBasePrivate;
class SubTaskPrivate {
friend class SubTask;
friend std::ostream& operator<<(std::ostream &os, const SubTaskPrivate& stage);
class StagePrivate {
friend class Stage;
friend std::ostream& operator<<(std::ostream &os, const StagePrivate& stage);
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 {
READS_START = 0x01,
@ -62,7 +62,7 @@ public:
inline void setNextStarts(Interface * next_starts) { next_starts_ = next_starts; }
protected:
SubTask* const me_; // associated/owning SubTask instance
Stage* const me_; // associated/owning Stage instance
const std::string name_;
InterfacePtr starts_;
@ -77,10 +77,10 @@ private:
Interface *prev_ends_; // interface to be used for sendBackward()
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;
public:
@ -121,7 +121,7 @@ public:
};
class GeneratorPrivate : public SubTaskPrivate {
class GeneratorPrivate : public StagePrivate {
public:
inline GeneratorPrivate(Generator *me, const std::string &name);
InterfaceFlags announcedFlags() const override;
@ -131,7 +131,7 @@ public:
};
class ConnectingPrivate : public SubTaskPrivate {
class ConnectingPrivate : public StagePrivate {
friend class Connecting;
public:
@ -152,6 +152,6 @@ private:
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 <ros/ros.h>
@ -13,7 +13,7 @@
#include <Eigen/Geometry>
namespace moveit { namespace task_constructor { namespace subtasks {
namespace moveit { namespace task_constructor { namespace stages {
CartesianPositionMotion::CartesianPositionMotion(std::string 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)
: 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 <moveit_msgs/DisplayRobotState.h>
@ -15,7 +15,7 @@
#include <chrono>
#include <functional>
namespace moveit { namespace task_constructor { namespace subtasks {
namespace moveit { namespace task_constructor { namespace stages {
GenerateGraspPose::GenerateGraspPose(std::string 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/task.h>
@ -11,7 +11,7 @@
#include <moveit_msgs/MotionPlanRequest.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)
: 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/task.h>
@ -11,7 +11,7 @@
#include <moveit_msgs/MotionPlanRequest.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)
: Connecting(name),
@ -50,7 +50,7 @@ bool Move::compute(const InterfaceState &from, const InterfaceState &to) {
if(!planner_->generatePlan(from.state, req, res))
return false;
// finish subtask
// finish stage
connect(from, to, res.trajectory_);
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(){
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;
};
traverseStages(processor);

View File

@ -12,13 +12,13 @@ if (CATKIN_ENABLE_TESTING)
endif()
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)
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)
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)
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 <subtask_p.h>
#include <stage_p.h>
#include <gtest/gtest.h>
#include <initializer_list>
@ -44,7 +44,7 @@ protected:
void SetUp() {}
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();
ASSERT_TRUE(num == expected.size()) << "different list lengths";
@ -61,7 +61,7 @@ protected:
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();
StagePrivate *child = (*it)->pimpl();
EXPECT_EQ(child, *exp_it) << "wrong order";
EXPECT_EQ(child->parent(), container) << "wrong parent";
EXPECT_EQ(it, container->position(pos)) << "bad forward position resolution";
@ -73,8 +73,8 @@ protected:
TEST_F(BaseTest, interfaceFlags) {
std::unique_ptr<Generator> g = std::make_unique<TestGenerator>();
EXPECT_EQ(g->pimpl()->interfaceFlags(),
SubTaskPrivate::InterfaceFlags({SubTaskPrivate::WRITES_NEXT_START,
SubTaskPrivate::WRITES_PREV_END}));
StagePrivate::InterfaceFlags({StagePrivate::WRITES_NEXT_START,
StagePrivate::WRITES_PREV_END}));
}
#define VALIDATE(...) \

View File

@ -2,8 +2,8 @@
#include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/subtasks/current_state.h>
#include <moveit_task_constructor/subtasks/cartesian_position_motion.h>
#include <moveit_task_constructor/stages/current_state.h>
#include <moveit_task_constructor/stages/cartesian_position_motion.h>
using namespace moveit::task_constructor;
@ -12,10 +12,10 @@ int main(int argc, char** argv){
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->setLink("s_model_tool0");
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->setLink("s_model_tool0");
move->setMinDistance(0.01);

View File

@ -2,7 +2,7 @@
#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;
@ -11,7 +11,7 @@ int main(int argc, char** argv){
Task t;
t.add( std::make_unique<subtasks::CurrentState>("current state") );
t.add( std::make_unique<stages::CurrentState>("current state") );
t.plan();

View File

@ -1,5 +1,5 @@
#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 <moveit_msgs/CollisionObject.h>
@ -36,7 +36,7 @@ int main(int argc, char** argv){
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->setGroup("arm");

View File

@ -2,8 +2,8 @@
#include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/subtasks/current_state.h>
#include <moveit_task_constructor/subtasks/gripper.h>
#include <moveit_task_constructor/stages/current_state.h>
#include <moveit_task_constructor/stages/gripper.h>
using namespace moveit::task_constructor;
@ -14,9 +14,9 @@ int main(int argc, char** argv){
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->setTo("closed");
t.add(std::move( gripper ));
@ -26,12 +26,12 @@ int main(int argc, char** argv){
/*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->setTo("closed");
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();
*/
return 0;