From 0e5d39a94d820254d50ea6e98a5ee723a44c8e0a Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 28 Feb 2017 15:27:05 +0100 Subject: [PATCH] introduce a number of interfaces as loose ends --- CMakeLists.txt | 12 +++-- include/moveit_task_constructor/storage.h | 13 +++-- include/moveit_task_constructor/subtask.h | 31 ++++++++---- include/moveit_task_constructor/task.h | 14 ++++++ src/subtask.cpp | 58 ++++++++++++++++++----- src/task.cpp | 18 +++++-- src/test/test_plan_current_state.cpp | 5 +- 7 files changed, 121 insertions(+), 30 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 595c0ad7..5e6c2fa4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,9 +2,12 @@ cmake_minimum_required(VERSION 2.6.12) project(moveit_task_constructor) -find_package(catkin REQUIRED) - -find_package(moveit_core REQUIRED) +find_package(catkin REQUIRED COMPONENTS + roscpp + moveit_core + moveit_ros_planning + moveit_msgs +) catkin_package( INCLUDE_DIRS include @@ -12,7 +15,7 @@ catkin_package( include_directories( include - ${moveit_core_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} ) add_compile_options(-std=c++14) @@ -21,6 +24,7 @@ add_library(${PROJECT_NAME}_subtasks src/subtasks/move.cpp src/subtasks/current_state.cpp ) +target_link_libraries(${PROJECT_NAME}_subtasks ${catkin_LIBRARIES}) add_library(${PROJECT_NAME} src/subtask.cpp diff --git a/include/moveit_task_constructor/storage.h b/include/moveit_task_constructor/storage.h index 1aa4532f..246db11b 100644 --- a/include/moveit_task_constructor/storage.h +++ b/include/moveit_task_constructor/storage.h @@ -11,12 +11,19 @@ MOVEIT_CLASS_FORWARD(InterfaceState); struct SubTrajectory { robot_trajectory::RobotTrajectoryPtr trajectory; - std::vector next; - std::vector prev; + std::vector begin; + std::vector end; }; struct InterfaceState { - SubTrajectory* connection; + InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next) + : state(ps), + previous_trajectory(previous), + next_trajectory(next) + {} + + SubTrajectory* previous_trajectory; + SubTrajectory* next_trajectory; planning_scene::PlanningSceneConstPtr state; }; diff --git a/include/moveit_task_constructor/subtask.h b/include/moveit_task_constructor/subtask.h index cb671fc4..f71152b1 100644 --- a/include/moveit_task_constructor/subtask.h +++ b/include/moveit_task_constructor/subtask.h @@ -22,24 +22,39 @@ public: virtual bool canCompute() = 0; virtual bool compute() = 0; - const std::vector& getOut(); - const std::vector& getIn(); + const std::string& getName(); + const std::list& getEnd(); + const std::list& getBegin(); const std::list& getTrajectories(); - void addPredecessor(SubTaskConstPtr); - void addSuccessor(SubTaskConstPtr); + void setRobotModel(robot_model::RobotModelConstPtr); + + void addPredecessor(SubTaskPtr); + void addSuccessor(SubTaskPtr); protected: + void sendForward(); + void sendBackward(); + void sendBothWays(robot_trajectory::RobotTrajectoryPtr&, planning_scene::PlanningScenePtr&); + + void connectBegin(InterfaceState&, SubTrajectory*); + void connectEnd(InterfaceState&, SubTrajectory*); + + InterfaceState* newBegin(planning_scene::PlanningScenePtr, SubTrajectory*); + InterfaceState* newEnd(planning_scene::PlanningScenePtr, SubTrajectory*); + const std::string name_; // maintain raw pointers to predecessors to avoid pointer circles - std::vector predecessors_; - std::vector successors_; + std::vector predecessors_; + std::vector successors_; std::list trajectories_; - std::shared_ptr< std::vector > ins_; - std::shared_ptr< std::vector > outs_; + std::list beginnings_; + std::list endings_; + + robot_model::RobotModelConstPtr robot_model_; }; } diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index ce1bdbdf..395df8bb 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -6,11 +6,21 @@ #include +namespace moveit::core { + MOVEIT_CLASS_FORWARD(RobotModel); +} + +namespace robot_model_loader { + MOVEIT_CLASS_FORWARD(RobotModelLoader); +} + namespace moveit::task_constructor { MOVEIT_CLASS_FORWARD(SubTask); + MOVEIT_CLASS_FORWARD(Task); + class Task { public: Task(); @@ -21,7 +31,11 @@ public: bool plan(); protected: + void addSubTask( SubTaskPtr ); + std::vector subtasks_; + + robot_model_loader::RobotModelLoaderPtr rml_; }; } diff --git a/src/subtask.cpp b/src/subtask.cpp index 392325d2..fcc75774 100644 --- a/src/subtask.cpp +++ b/src/subtask.cpp @@ -1,30 +1,66 @@ #include moveit::task_constructor::SubTask::SubTask(std::string name) - : name_(name), - ins_(new std::vector), - outs_(new std::vector) + : name_(name) {}; -void moveit::task_constructor::SubTask::addPredecessor(SubTaskConstPtr prev_task){ +void moveit::task_constructor::SubTask::addPredecessor(SubTaskPtr prev_task){ predecessors_.push_back( prev_task.get() ); } -void moveit::task_constructor::SubTask::addSuccessor(SubTaskConstPtr next_task){ +void moveit::task_constructor::SubTask::addSuccessor(SubTaskPtr next_task){ successors_.push_back( next_task ); } -const std::vector& -moveit::task_constructor::SubTask::getIn(){ - return *ins_; +const std::string& +moveit::task_constructor::SubTask::getName(){ + return name_; } -const std::vector& -moveit::task_constructor::SubTask::getOut(){ - return *outs_; +const std::list& +moveit::task_constructor::SubTask::getBegin(){ + return beginnings_; +} + +const std::list& +moveit::task_constructor::SubTask::getEnd(){ + return endings_; } const std::list& moveit::task_constructor::SubTask::getTrajectories(){ return trajectories_; } + +void +moveit::task_constructor::SubTask::setRobotModel(robot_model::RobotModelConstPtr model){ + robot_model_= model; +} + + +void +moveit::task_constructor::SubTask::sendBothWays(robot_trajectory::RobotTrajectoryPtr& traj, planning_scene::PlanningScenePtr& ps){ + trajectories_.emplace_back(); + SubTrajectory& subtraj= trajectories_.back(); + subtraj.trajectory= traj; + + subtraj.begin.reserve(predecessors_.size()); + for( SubTask* pred : predecessors_ ) + subtraj.begin.push_back( pred->newEnd(ps, &subtraj) ); + + subtraj.end.resize(successors_.size()); + for( SubTaskPtr succ : successors_ ) + subtraj.end.push_back( succ->newBegin(ps, &subtraj) ); +} + +moveit::task_constructor::InterfaceState* +moveit::task_constructor::SubTask::newBegin(planning_scene::PlanningScenePtr ps, SubTrajectory* old_end){ + beginnings_.push_back( InterfaceState(ps, old_end, NULL)); + return &beginnings_.back(); +} + +moveit::task_constructor::InterfaceState* +moveit::task_constructor::SubTask::newEnd(planning_scene::PlanningScenePtr ps, SubTrajectory* old_beginning){ + endings_.push_back( InterfaceState(ps, NULL, old_beginning)); + return &endings_.back(); +} diff --git a/src/task.cpp b/src/task.cpp index 6ba84a51..56fb4327 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -1,20 +1,32 @@ #include #include +#include + moveit::task_constructor::Task::Task() -{} +{ + rml_.reset(new robot_model_loader::RobotModelLoader); + if( !rml_->getModel() ) + throw Exception("Task failed to construct RobotModel"); +} void moveit::task_constructor::Task::addStart( SubTaskPtr subtask ){ subtasks_.clear(); - subtasks_.push_back( subtask ); + addSubTask( subtask ); } void moveit::task_constructor::Task::addAfter( SubTaskPtr subtask ){ subtask->addPredecessor( subtasks_.back() ); subtasks_.back()->addSuccessor( subtask ); - subtasks_.push_back( subtask ); + addSubTask( subtask ); } bool moveit::task_constructor::Task::plan(){ return false; } + + +void moveit::task_constructor::Task::addSubTask( SubTaskPtr subtask ){ + subtask->setRobotModel( rml_->getModel() ); + subtasks_.push_back( subtask ); +} diff --git a/src/test/test_plan_current_state.cpp b/src/test/test_plan_current_state.cpp index 962f0a54..fff098c9 100644 --- a/src/test/test_plan_current_state.cpp +++ b/src/test/test_plan_current_state.cpp @@ -1,3 +1,5 @@ +#include + #include #include @@ -5,7 +7,8 @@ using namespace moveit::task_constructor; -int main(){ +int main(int argc, char** argv){ + ros::init(argc, argv, "test_plan_current_state"); Task t;