From a32007613eb78c8b97131c28bb1e8b6f75167bb5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 16 Oct 2017 08:51:01 +0200 Subject: [PATCH] renamed subtask -> stage --- CMakeLists.txt | 8 +-- include/moveit_task_constructor/container.h | 8 +-- .../{subtask.h => stage.h} | 30 +++++------ .../cartesian_position_motion.h | 4 +- .../{subtasks => stages}/current_state.h | 4 +- .../generate_grasp_pose.h | 4 +- .../{subtasks => stages}/gripper.h | 4 +- .../{subtasks => stages}/move.h | 4 +- include/moveit_task_constructor/storage.h | 2 +- include/moveit_task_constructor/task.h | 4 +- src/container.cpp | 38 +++++++------- src/container_p.h | 14 ++--- src/demo/CMakeLists.txt | 4 +- src/demo/plan_pick_trixi.cpp | 24 ++++----- src/demo/plan_pick_ur5.cpp | 24 ++++----- src/{subtask.cpp => stage.cpp} | 52 +++++++++---------- src/{subtask_p.h => stage_p.h} | 24 ++++----- src/stages/CMakeLists.txt | 14 +++++ .../cartesian_position_motion.cpp | 4 +- src/{subtasks => stages}/current_state.cpp | 4 +- .../generate_grasp_pose.cpp | 4 +- src/{subtasks => stages}/gripper.cpp | 4 +- src/{subtasks => stages}/move.cpp | 6 +-- src/subtasks/CMakeLists.txt | 14 ----- src/task.cpp | 2 +- src/test/CMakeLists.txt | 8 +-- src/test/container.cpp | 10 ++-- src/test/test_plan_cartesian_forward.cpp | 10 ++-- src/test/test_plan_current_state.cpp | 4 +- src/test/test_plan_generate_grasp_pose.cpp | 4 +- src/test/test_plan_gripper.cpp | 12 ++--- 31 files changed, 176 insertions(+), 176 deletions(-) rename include/moveit_task_constructor/{subtask.h => stage.h} (83%) rename include/moveit_task_constructor/{subtasks => stages}/cartesian_position_motion.h (92%) rename include/moveit_task_constructor/{subtasks => stages}/current_state.h (75%) rename include/moveit_task_constructor/{subtasks => stages}/generate_grasp_pose.h (92%) rename include/moveit_task_constructor/{subtasks => stages}/gripper.h (90%) rename include/moveit_task_constructor/{subtasks => stages}/move.h (85%) rename src/{subtask.cpp => stage.cpp} (86%) rename src/{subtask_p.h => stage_p.h} (88%) create mode 100644 src/stages/CMakeLists.txt rename src/{subtasks => stages}/cartesian_position_motion.cpp (98%) rename src/{subtasks => stages}/current_state.cpp (72%) rename src/{subtasks => stages}/generate_grasp_pose.cpp (97%) rename src/{subtasks => stages}/gripper.cpp (96%) rename src/{subtasks => stages}/move.cpp (90%) delete mode 100644 src/subtasks/CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index e9839001..88bb1635 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/include/moveit_task_constructor/container.h b/include/moveit_task_constructor/container.h index 3034d299..c6e90735 100644 --- a/include/moveit_task_constructor/container.h +++ b/include/moveit_task_constructor/container.h @@ -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 StageCallback; + typedef std::function StageCallback; typedef std::function&)> SolutionCallback; virtual bool canInsert(const value_type& stage, int before = -1) const = 0; diff --git a/include/moveit_task_constructor/subtask.h b/include/moveit_task_constructor/stage.h similarity index 83% rename from include/moveit_task_constructor/subtask.h rename to include/moveit_task_constructor/stage.h index 18cbbda8..07c1348f 100644 --- a/include/moveit_task_constructor/subtask.h +++ b/include/moveit_task_constructor/stage.h @@ -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 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 pointer; + PRIVATE_CLASS(Stage) + typedef std::unique_ptr 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 and get rid of destructor - SubTaskPrivate* const pimpl_; // constness guarantees one initial write + // TODO: use unique_ptr 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); diff --git a/include/moveit_task_constructor/subtasks/cartesian_position_motion.h b/include/moveit_task_constructor/stages/cartesian_position_motion.h similarity index 92% rename from include/moveit_task_constructor/subtasks/cartesian_position_motion.h rename to include/moveit_task_constructor/stages/cartesian_position_motion.h index 42c096af..44959781 100644 --- a/include/moveit_task_constructor/subtasks/cartesian_position_motion.h +++ b/include/moveit_task_constructor/stages/cartesian_position_motion.h @@ -2,7 +2,7 @@ #pragma once -#include +#include #include @@ -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: diff --git a/include/moveit_task_constructor/subtasks/current_state.h b/include/moveit_task_constructor/stages/current_state.h similarity index 75% rename from include/moveit_task_constructor/subtasks/current_state.h rename to include/moveit_task_constructor/stages/current_state.h index 703f1a93..266caf67 100644 --- a/include/moveit_task_constructor/subtasks/current_state.h +++ b/include/moveit_task_constructor/stages/current_state.h @@ -2,9 +2,9 @@ #pragma once -#include +#include -namespace moveit { namespace task_constructor { namespace subtasks { +namespace moveit { namespace task_constructor { namespace stages { class CurrentState : public Generator { public: diff --git a/include/moveit_task_constructor/subtasks/generate_grasp_pose.h b/include/moveit_task_constructor/stages/generate_grasp_pose.h similarity index 92% rename from include/moveit_task_constructor/subtasks/generate_grasp_pose.h rename to include/moveit_task_constructor/stages/generate_grasp_pose.h index 5a7d6283..41b922d7 100644 --- a/include/moveit_task_constructor/subtasks/generate_grasp_pose.h +++ b/include/moveit_task_constructor/stages/generate_grasp_pose.h @@ -2,7 +2,7 @@ #pragma once -#include +#include #include @@ -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: diff --git a/include/moveit_task_constructor/subtasks/gripper.h b/include/moveit_task_constructor/stages/gripper.h similarity index 90% rename from include/moveit_task_constructor/subtasks/gripper.h rename to include/moveit_task_constructor/stages/gripper.h index af290590..8dd68321 100644 --- a/include/moveit_task_constructor/subtasks/gripper.h +++ b/include/moveit_task_constructor/stages/gripper.h @@ -2,13 +2,13 @@ #pragma once -#include +#include 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: diff --git a/include/moveit_task_constructor/subtasks/move.h b/include/moveit_task_constructor/stages/move.h similarity index 85% rename from include/moveit_task_constructor/subtasks/move.h rename to include/moveit_task_constructor/stages/move.h index c5c9e5f3..3cdbeab0 100644 --- a/include/moveit_task_constructor/subtasks/move.h +++ b/include/moveit_task_constructor/stages/move.h @@ -2,13 +2,13 @@ #pragma once -#include +#include 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: diff --git a/include/moveit_task_constructor/storage.h b/include/moveit_task_constructor/storage.h index 981a7912..ea086fe3 100644 --- a/include/moveit_task_constructor/storage.h +++ b/include/moveit_task_constructor/storage.h @@ -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. diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index aabc2b1f..4b7268f3 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -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(); diff --git a/src/container.cpp b/src/container.cpp index 3251df1f..030e7071 100644 --- a/src/container.cpp +++ b/src/container.cpp @@ -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; diff --git a/src/container_p.h b/src/container_p.h index 9840398e..0dae5720 100644 --- a/src/container_p.h +++ b/src/container_p.h @@ -1,31 +1,31 @@ #pragma once #include -#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; diff --git a/src/demo/CMakeLists.txt b/src/demo/CMakeLists.txt index 142c0dc4..f3b870c8 100644 --- a/src/demo/CMakeLists.txt +++ b/src/demo/CMakeLists.txt @@ -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}) diff --git a/src/demo/plan_pick_trixi.cpp b/src/demo/plan_pick_trixi.cpp index ac42c3f3..bcb6ffdd 100644 --- a/src/demo/plan_pick_trixi.cpp +++ b/src/demo/plan_pick_trixi.cpp @@ -1,11 +1,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -43,17 +43,17 @@ int main(int argc, char** argv){ Task t; - t.add( std::make_unique("current state") ); + t.add( std::make_unique("current state") ); { - auto move= std::make_unique("open gripper"); + auto move= std::make_unique("open gripper"); move->setEndEffector("left_gripper"); move->setTo("open"); t.add(std::move(move)); } { - auto move= std::make_unique("move to pre-grasp"); + auto move= std::make_unique("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("proceed to grasp pose"); + auto move= std::make_unique("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("generate grasp pose"); + auto gengrasp= std::make_unique("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("grasp"); + auto move= std::make_unique("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("lift object"); + auto move= std::make_unique("lift object"); move->setGroup("left_arm"); move->setLink("l_gripper_tool_frame"); move->setMinMaxDistance(0.03, 0.05); diff --git a/src/demo/plan_pick_ur5.cpp b/src/demo/plan_pick_ur5.cpp index 064a8b85..4dd48ef2 100644 --- a/src/demo/plan_pick_ur5.cpp +++ b/src/demo/plan_pick_ur5.cpp @@ -1,11 +1,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include @@ -41,17 +41,17 @@ int main(int argc, char** argv){ Task t; - t.add(std::make_unique("current state")); + t.add(std::make_unique("current state")); { - auto move = std::make_unique("open gripper"); + auto move = std::make_unique("open gripper"); move->setEndEffector("gripper"); move->setTo("open"); t.add(std::move(std::move(move))); } { - auto move = std::make_unique("move to pre-grasp"); + auto move = std::make_unique("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("proceed to grasp pose"); + auto move = std::make_unique("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("generate grasp pose"); + auto gengrasp = std::make_unique("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("grasp"); + auto move = std::make_unique("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("lift object"); + auto move = std::make_unique("lift object"); move->setGroup("arm"); move->setLink("s_model_tool0"); move->setMinMaxDistance(0.03, 0.05); diff --git a/src/subtask.cpp b/src/stage.cpp similarity index 86% rename from src/subtask.cpp rename to src/stage.cpp index 45b408c3..7aa1608f 100644 --- a/src/subtask.cpp +++ b/src/stage.cpp @@ -1,4 +1,4 @@ -#include "subtask_p.h" +#include "stage_p.h" #include "container_p.h" #include #include @@ -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 -const char* direction(const SubTaskPrivate& stage) { - SubTaskPrivate::InterfaceFlags f = stage.deducedFlags(); +template +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(stage) + os << std::setw(5) << direction(stage) << std::setw(3) << stage.trajectories_.size() - << std::setw(5) << direction(stage); + << std::setw(5) << direction(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) diff --git a/src/subtask_p.h b/src/stage_p.h similarity index 88% rename from src/subtask_p.h rename to src/stage_p.h index 1219f093..bc38a199 100644 --- a/src/subtask_p.h +++ b/src/stage_p.h @@ -2,7 +2,7 @@ #pragma once -#include +#include #include // 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 container_type; + typedef std::list 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 it_pairs_; }; -PIMPL_FUNCTIONS(SubTask) +PIMPL_FUNCTIONS(Stage) } } diff --git a/src/stages/CMakeLists.txt b/src/stages/CMakeLists.txt new file mode 100644 index 00000000..398f11e7 --- /dev/null +++ b/src/stages/CMakeLists.txt @@ -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}) diff --git a/src/subtasks/cartesian_position_motion.cpp b/src/stages/cartesian_position_motion.cpp similarity index 98% rename from src/subtasks/cartesian_position_motion.cpp rename to src/stages/cartesian_position_motion.cpp index 53cf3c61..2e29553d 100644 --- a/src/subtasks/cartesian_position_motion.cpp +++ b/src/stages/cartesian_position_motion.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -13,7 +13,7 @@ #include -namespace moveit { namespace task_constructor { namespace subtasks { +namespace moveit { namespace task_constructor { namespace stages { CartesianPositionMotion::CartesianPositionMotion(std::string name) : PropagatingEitherWay(name), diff --git a/src/subtasks/current_state.cpp b/src/stages/current_state.cpp similarity index 72% rename from src/subtasks/current_state.cpp rename to src/stages/current_state.cpp index b56aa569..25ece875 100644 --- a/src/subtasks/current_state.cpp +++ b/src/stages/current_state.cpp @@ -1,6 +1,6 @@ -#include +#include -namespace moveit { namespace task_constructor { namespace subtasks { +namespace moveit { namespace task_constructor { namespace stages { CurrentState::CurrentState(std::string name) : Generator(name) diff --git a/src/subtasks/generate_grasp_pose.cpp b/src/stages/generate_grasp_pose.cpp similarity index 97% rename from src/subtasks/generate_grasp_pose.cpp rename to src/stages/generate_grasp_pose.cpp index f3ace218..8a3bb10b 100644 --- a/src/subtasks/generate_grasp_pose.cpp +++ b/src/stages/generate_grasp_pose.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -15,7 +15,7 @@ #include #include -namespace moveit { namespace task_constructor { namespace subtasks { +namespace moveit { namespace task_constructor { namespace stages { GenerateGraspPose::GenerateGraspPose(std::string name) : Generator(name), diff --git a/src/subtasks/gripper.cpp b/src/stages/gripper.cpp similarity index 96% rename from src/subtasks/gripper.cpp rename to src/stages/gripper.cpp index 3bfc3e00..d0cde1c9 100644 --- a/src/subtasks/gripper.cpp +++ b/src/stages/gripper.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -11,7 +11,7 @@ #include #include -namespace moveit { namespace task_constructor { namespace subtasks { +namespace moveit { namespace task_constructor { namespace stages { Gripper::Gripper(std::string name) : PropagatingEitherWay(name) diff --git a/src/subtasks/move.cpp b/src/stages/move.cpp similarity index 90% rename from src/subtasks/move.cpp rename to src/stages/move.cpp index e8eb033e..94d1cf25 100644 --- a/src/subtasks/move.cpp +++ b/src/stages/move.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -11,7 +11,7 @@ #include #include -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; diff --git a/src/subtasks/CMakeLists.txt b/src/subtasks/CMakeLists.txt deleted file mode 100644 index d32f43ac..00000000 --- a/src/subtasks/CMakeLists.txt +++ /dev/null @@ -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}) diff --git a/src/task.cpp b/src/task.cpp index 4524141f..57348c92 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -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); diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 341062e3..4a8f6566 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -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}) diff --git a/src/test/container.cpp b/src/test/container.cpp index d05bac84..9d05a903 100644 --- a/src/test/container.cpp +++ b/src/test/container.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include #include @@ -44,7 +44,7 @@ protected: void SetUp() {} void TearDown() {} - void validateOrder(const SerialContainerPrivate* container, const std::initializer_list &expected) { + void validateOrder(const SerialContainerPrivate* container, const std::initializer_list &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 g = std::make_unique(); 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(...) \ diff --git a/src/test/test_plan_cartesian_forward.cpp b/src/test/test_plan_cartesian_forward.cpp index 3822b6ce..09e514c3 100644 --- a/src/test/test_plan_cartesian_forward.cpp +++ b/src/test/test_plan_cartesian_forward.cpp @@ -2,8 +2,8 @@ #include -#include -#include +#include +#include using namespace moveit::task_constructor; @@ -12,10 +12,10 @@ int main(int argc, char** argv){ Task t; - t.add( std::make_unique("current state") ); + t.add( std::make_unique("current state") ); { - auto move= std::make_unique("lift object"); + auto move= std::make_unique("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("lift object"); + auto move= std::make_unique("lift object"); move->setGroup("arm"); move->setLink("s_model_tool0"); move->setMinDistance(0.01); diff --git a/src/test/test_plan_current_state.cpp b/src/test/test_plan_current_state.cpp index bf267448..d7682386 100644 --- a/src/test/test_plan_current_state.cpp +++ b/src/test/test_plan_current_state.cpp @@ -2,7 +2,7 @@ #include -#include +#include using namespace moveit::task_constructor; @@ -11,7 +11,7 @@ int main(int argc, char** argv){ Task t; - t.add( std::make_unique("current state") ); + t.add( std::make_unique("current state") ); t.plan(); diff --git a/src/test/test_plan_generate_grasp_pose.cpp b/src/test/test_plan_generate_grasp_pose.cpp index 8af2c3bb..e1c756af 100644 --- a/src/test/test_plan_generate_grasp_pose.cpp +++ b/src/test/test_plan_generate_grasp_pose.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include #include @@ -36,7 +36,7 @@ int main(int argc, char** argv){ Task t; - auto st= std::make_unique("generate grasp candidates"); + auto st= std::make_unique("generate grasp candidates"); st->setEndEffector("gripper"); //st->setGroup("arm"); diff --git a/src/test/test_plan_gripper.cpp b/src/test/test_plan_gripper.cpp index dae255ff..e6f231d7 100644 --- a/src/test/test_plan_gripper.cpp +++ b/src/test/test_plan_gripper.cpp @@ -2,8 +2,8 @@ #include -#include -#include +#include +#include using namespace moveit::task_constructor; @@ -14,9 +14,9 @@ int main(int argc, char** argv){ Task t; - t.add( std::make_unique("current state") ); + t.add( std::make_unique("current state") ); { - auto gripper= std::make_unique("close gripper"); + auto gripper= std::make_unique("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("close gripper"); + auto gripper= std::make_unique("close gripper"); gripper->setEndEffector("gripper"); gripper->setTo("closed"); t.add(std::move( gripper )); } - t.add( std::make_unique("current state") ); + t.add( std::make_unique("current state") ); t.plan(); */ return 0;