mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
introduce a number of interfaces as loose ends
This commit is contained in:
parent
bd67649fd2
commit
0e5d39a94d
@ -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
|
||||||
|
|||||||
@ -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;
|
||||||
};
|
};
|
||||||
|
|||||||
@ -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_;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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_;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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();
|
||||||
|
}
|
||||||
|
|||||||
18
src/task.cpp
18
src/task.cpp
@ -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 );
|
||||||
|
}
|
||||||
|
|||||||
@ -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;
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user