introduce a number of interfaces as loose ends

This commit is contained in:
v4hn 2017-02-28 15:27:05 +01:00
parent bd67649fd2
commit 0e5d39a94d
7 changed files with 121 additions and 30 deletions

View File

@ -2,9 +2,12 @@ cmake_minimum_required(VERSION 2.6.12)
project(moveit_task_constructor) project(moveit_task_constructor)
find_package(catkin REQUIRED) find_package(catkin REQUIRED COMPONENTS
roscpp
find_package(moveit_core REQUIRED) moveit_core
moveit_ros_planning
moveit_msgs
)
catkin_package( catkin_package(
INCLUDE_DIRS include INCLUDE_DIRS include
@ -12,7 +15,7 @@ catkin_package(
include_directories( include_directories(
include include
${moveit_core_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
) )
add_compile_options(-std=c++14) add_compile_options(-std=c++14)
@ -21,6 +24,7 @@ add_library(${PROJECT_NAME}_subtasks
src/subtasks/move.cpp src/subtasks/move.cpp
src/subtasks/current_state.cpp src/subtasks/current_state.cpp
) )
target_link_libraries(${PROJECT_NAME}_subtasks ${catkin_LIBRARIES})
add_library(${PROJECT_NAME} add_library(${PROJECT_NAME}
src/subtask.cpp src/subtask.cpp

View File

@ -11,12 +11,19 @@ MOVEIT_CLASS_FORWARD(InterfaceState);
struct SubTrajectory { struct SubTrajectory {
robot_trajectory::RobotTrajectoryPtr trajectory; robot_trajectory::RobotTrajectoryPtr trajectory;
std::vector<InterfaceState*> next; std::vector<InterfaceState*> begin;
std::vector<InterfaceState*> prev; std::vector<InterfaceState*> end;
}; };
struct InterfaceState { 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; planning_scene::PlanningSceneConstPtr state;
}; };

View File

@ -22,24 +22,39 @@ public:
virtual bool canCompute() = 0; virtual bool canCompute() = 0;
virtual bool compute() = 0; virtual bool compute() = 0;
const std::vector<InterfaceState>& getOut(); const std::string& getName();
const std::vector<InterfaceState>& getIn(); const std::list<InterfaceState>& getEnd();
const std::list<InterfaceState>& getBegin();
const std::list<SubTrajectory>& getTrajectories(); const std::list<SubTrajectory>& getTrajectories();
void addPredecessor(SubTaskConstPtr); void setRobotModel(robot_model::RobotModelConstPtr);
void addSuccessor(SubTaskConstPtr);
void addPredecessor(SubTaskPtr);
void addSuccessor(SubTaskPtr);
protected: 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_; const std::string name_;
// maintain raw pointers to predecessors to avoid pointer circles // maintain raw pointers to predecessors to avoid pointer circles
std::vector<const SubTask*> predecessors_; std::vector<SubTask*> predecessors_;
std::vector<SubTaskConstPtr> successors_; std::vector<SubTaskPtr> successors_;
std::list<SubTrajectory> trajectories_; std::list<SubTrajectory> trajectories_;
std::shared_ptr< std::vector<InterfaceState> > ins_; std::list<InterfaceState> beginnings_;
std::shared_ptr< std::vector<InterfaceState> > outs_; std::list<InterfaceState> endings_;
robot_model::RobotModelConstPtr robot_model_;
}; };
} }

View File

@ -6,11 +6,21 @@
#include <vector> #include <vector>
namespace moveit::core {
MOVEIT_CLASS_FORWARD(RobotModel);
}
namespace robot_model_loader {
MOVEIT_CLASS_FORWARD(RobotModelLoader);
}
namespace moveit::task_constructor { namespace moveit::task_constructor {
MOVEIT_CLASS_FORWARD(SubTask); MOVEIT_CLASS_FORWARD(SubTask);
MOVEIT_CLASS_FORWARD(Task); MOVEIT_CLASS_FORWARD(Task);
class Task { class Task {
public: public:
Task(); Task();
@ -21,7 +31,11 @@ public:
bool plan(); bool plan();
protected: protected:
void addSubTask( SubTaskPtr );
std::vector<SubTaskPtr> subtasks_; std::vector<SubTaskPtr> subtasks_;
robot_model_loader::RobotModelLoaderPtr rml_;
}; };
} }

View File

@ -1,30 +1,66 @@
#include <moveit_task_constructor/subtask.h> #include <moveit_task_constructor/subtask.h>
moveit::task_constructor::SubTask::SubTask(std::string name) moveit::task_constructor::SubTask::SubTask(std::string name)
: name_(name), : name_(name)
ins_(new std::vector<InterfaceState>),
outs_(new std::vector<InterfaceState>)
{}; {};
void moveit::task_constructor::SubTask::addPredecessor(SubTaskConstPtr prev_task){ void moveit::task_constructor::SubTask::addPredecessor(SubTaskPtr prev_task){
predecessors_.push_back( prev_task.get() ); 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 ); successors_.push_back( next_task );
} }
const std::vector<moveit::task_constructor::InterfaceState>& const std::string&
moveit::task_constructor::SubTask::getIn(){ moveit::task_constructor::SubTask::getName(){
return *ins_; return name_;
} }
const std::vector<moveit::task_constructor::InterfaceState>& const std::list<moveit::task_constructor::InterfaceState>&
moveit::task_constructor::SubTask::getOut(){ moveit::task_constructor::SubTask::getBegin(){
return *outs_; return beginnings_;
}
const std::list<moveit::task_constructor::InterfaceState>&
moveit::task_constructor::SubTask::getEnd(){
return endings_;
} }
const std::list<moveit::task_constructor::SubTrajectory>& const std::list<moveit::task_constructor::SubTrajectory>&
moveit::task_constructor::SubTask::getTrajectories(){ moveit::task_constructor::SubTask::getTrajectories(){
return trajectories_; 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();
}

View File

@ -1,20 +1,32 @@
#include <moveit_task_constructor/task.h> #include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/subtask.h> #include <moveit_task_constructor/subtask.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
moveit::task_constructor::Task::Task() 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 ){ void moveit::task_constructor::Task::addStart( SubTaskPtr subtask ){
subtasks_.clear(); subtasks_.clear();
subtasks_.push_back( subtask ); addSubTask( subtask );
} }
void moveit::task_constructor::Task::addAfter( SubTaskPtr subtask ){ void moveit::task_constructor::Task::addAfter( SubTaskPtr subtask ){
subtask->addPredecessor( subtasks_.back() ); subtask->addPredecessor( subtasks_.back() );
subtasks_.back()->addSuccessor( subtask ); subtasks_.back()->addSuccessor( subtask );
subtasks_.push_back( subtask ); addSubTask( subtask );
} }
bool moveit::task_constructor::Task::plan(){ bool moveit::task_constructor::Task::plan(){
return false; return false;
} }
void moveit::task_constructor::Task::addSubTask( SubTaskPtr subtask ){
subtask->setRobotModel( rml_->getModel() );
subtasks_.push_back( subtask );
}

View File

@ -1,3 +1,5 @@
#include <ros/ros.h>
#include <moveit_task_constructor/task.h> #include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/subtask.h> #include <moveit_task_constructor/subtask.h>
@ -5,7 +7,8 @@
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
int main(){ int main(int argc, char** argv){
ros::init(argc, argv, "test_plan_current_state");
Task t; Task t;