mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Task now holds a PlanningScene
instead of just a RobotModel. All generating subtasks need the scene, so avoid every single one of them asking for it.
This commit is contained in:
parent
8af3645af9
commit
039f1e6896
@ -2,7 +2,17 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/macros/class_forward.h>
|
||||||
|
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
namespace planning_scene {
|
||||||
|
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||||
|
}
|
||||||
|
|
||||||
|
namespace robot_trajectory {
|
||||||
|
MOVEIT_CLASS_FORWARD(RobotTrajectory);
|
||||||
|
}
|
||||||
|
|
||||||
namespace moveit::task_constructor {
|
namespace moveit::task_constructor {
|
||||||
|
|
||||||
|
|||||||
@ -10,8 +10,11 @@
|
|||||||
#include <vector>
|
#include <vector>
|
||||||
#include <list>
|
#include <list>
|
||||||
|
|
||||||
namespace moveit::task_constructor {
|
namespace planning_scene {
|
||||||
|
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||||
|
}
|
||||||
|
|
||||||
|
namespace moveit::task_constructor {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(SubTask);
|
MOVEIT_CLASS_FORWARD(SubTask);
|
||||||
|
|
||||||
@ -27,7 +30,7 @@ public:
|
|||||||
const std::list<InterfaceState>& getBegin();
|
const std::list<InterfaceState>& getBegin();
|
||||||
const std::list<SubTrajectory>& getTrajectories();
|
const std::list<SubTrajectory>& getTrajectories();
|
||||||
|
|
||||||
void setRobotModel(robot_model::RobotModelConstPtr);
|
void setPlanningScene(planning_scene::PlanningSceneConstPtr);
|
||||||
|
|
||||||
void addPredecessor(SubTaskPtr);
|
void addPredecessor(SubTaskPtr);
|
||||||
void addSuccessor(SubTaskPtr);
|
void addSuccessor(SubTaskPtr);
|
||||||
@ -35,13 +38,13 @@ public:
|
|||||||
protected:
|
protected:
|
||||||
void sendForward();
|
void sendForward();
|
||||||
void sendBackward();
|
void sendBackward();
|
||||||
void sendBothWays(robot_trajectory::RobotTrajectoryPtr&, planning_scene::PlanningScenePtr&);
|
void sendBothWays(robot_trajectory::RobotTrajectoryPtr&, planning_scene::PlanningSceneConstPtr&);
|
||||||
|
|
||||||
void connectBegin(InterfaceState&, SubTrajectory*);
|
void connectBegin(InterfaceState&, SubTrajectory*);
|
||||||
void connectEnd(InterfaceState&, SubTrajectory*);
|
void connectEnd(InterfaceState&, SubTrajectory*);
|
||||||
|
|
||||||
InterfaceState* newBegin(planning_scene::PlanningScenePtr, SubTrajectory*);
|
InterfaceState* newBegin(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
|
||||||
InterfaceState* newEnd(planning_scene::PlanningScenePtr, SubTrajectory*);
|
InterfaceState* newEnd(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
|
||||||
|
|
||||||
const std::string name_;
|
const std::string name_;
|
||||||
|
|
||||||
@ -54,7 +57,7 @@ protected:
|
|||||||
std::list<InterfaceState> beginnings_;
|
std::list<InterfaceState> beginnings_;
|
||||||
std::list<InterfaceState> endings_;
|
std::list<InterfaceState> endings_;
|
||||||
|
|
||||||
robot_model::RobotModelConstPtr robot_model_;
|
planning_scene::PlanningSceneConstPtr scene_;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -8,14 +8,14 @@ namespace moveit::task_constructor::subtasks {
|
|||||||
|
|
||||||
class CurrentState : public SubTask {
|
class CurrentState : public SubTask {
|
||||||
public:
|
public:
|
||||||
using SubTask::SubTask;
|
CurrentState(std::string name);
|
||||||
|
|
||||||
virtual bool canCompute();
|
virtual bool canCompute();
|
||||||
|
|
||||||
virtual bool compute();
|
virtual bool compute();
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
bool succeeded_;
|
bool ran_;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -6,8 +6,8 @@
|
|||||||
|
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
namespace moveit::core {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(RobotModel);
|
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace robot_model_loader {
|
namespace robot_model_loader {
|
||||||
@ -35,6 +35,7 @@ protected:
|
|||||||
|
|
||||||
std::vector<SubTaskPtr> subtasks_;
|
std::vector<SubTaskPtr> subtasks_;
|
||||||
|
|
||||||
|
planning_scene::PlanningScenePtr scene_;
|
||||||
robot_model_loader::RobotModelLoaderPtr rml_;
|
robot_model_loader::RobotModelLoaderPtr rml_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -33,13 +33,12 @@ moveit::task_constructor::SubTask::getTrajectories(){
|
|||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
moveit::task_constructor::SubTask::setRobotModel(robot_model::RobotModelConstPtr model){
|
moveit::task_constructor::SubTask::setPlanningScene(planning_scene::PlanningSceneConstPtr scene){
|
||||||
robot_model_= model;
|
scene_= scene;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void
|
void
|
||||||
moveit::task_constructor::SubTask::sendBothWays(robot_trajectory::RobotTrajectoryPtr& traj, planning_scene::PlanningScenePtr& ps){
|
moveit::task_constructor::SubTask::sendBothWays(robot_trajectory::RobotTrajectoryPtr& traj, planning_scene::PlanningSceneConstPtr& ps){
|
||||||
trajectories_.emplace_back();
|
trajectories_.emplace_back();
|
||||||
SubTrajectory& subtraj= trajectories_.back();
|
SubTrajectory& subtraj= trajectories_.back();
|
||||||
subtraj.trajectory= traj;
|
subtraj.trajectory= traj;
|
||||||
@ -54,13 +53,13 @@ moveit::task_constructor::SubTask::sendBothWays(robot_trajectory::RobotTrajector
|
|||||||
}
|
}
|
||||||
|
|
||||||
moveit::task_constructor::InterfaceState*
|
moveit::task_constructor::InterfaceState*
|
||||||
moveit::task_constructor::SubTask::newBegin(planning_scene::PlanningScenePtr ps, SubTrajectory* old_end){
|
moveit::task_constructor::SubTask::newBegin(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_end){
|
||||||
beginnings_.push_back( InterfaceState(ps, old_end, NULL));
|
beginnings_.push_back( InterfaceState(ps, old_end, NULL));
|
||||||
return &beginnings_.back();
|
return &beginnings_.back();
|
||||||
}
|
}
|
||||||
|
|
||||||
moveit::task_constructor::InterfaceState*
|
moveit::task_constructor::InterfaceState*
|
||||||
moveit::task_constructor::SubTask::newEnd(planning_scene::PlanningScenePtr ps, SubTrajectory* old_beginning){
|
moveit::task_constructor::SubTask::newEnd(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_beginning){
|
||||||
endings_.push_back( InterfaceState(ps, NULL, old_beginning));
|
endings_.push_back( InterfaceState(ps, NULL, old_beginning));
|
||||||
return &endings_.back();
|
return &endings_.back();
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,47 +1,24 @@
|
|||||||
|
|
||||||
#include <moveit_task_constructor/subtasks/current_state.h>
|
#include <moveit_task_constructor/subtasks/current_state.h>
|
||||||
|
|
||||||
#include <ros/ros.h>
|
moveit::task_constructor::subtasks::CurrentState::CurrentState(std::string name)
|
||||||
#include <moveit_msgs/GetPlanningScene.h>
|
: moveit::task_constructor::SubTask::SubTask(name)
|
||||||
|
{
|
||||||
|
ran_= false;
|
||||||
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
moveit::task_constructor::subtasks::CurrentState::canCompute(){
|
moveit::task_constructor::subtasks::CurrentState::canCompute(){
|
||||||
return !succeeded_;
|
return !ran_;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
moveit::task_constructor::subtasks::CurrentState::compute(){
|
moveit::task_constructor::subtasks::CurrentState::compute(){
|
||||||
ros::NodeHandle h;
|
ran_= true;
|
||||||
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
|
|
||||||
client.waitForExistence();
|
|
||||||
|
|
||||||
moveit_msgs::GetPlanningScene::Request req;
|
|
||||||
moveit_msgs::GetPlanningScene::Response res;
|
|
||||||
|
|
||||||
req.components.components =
|
|
||||||
moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS
|
|
||||||
| moveit_msgs::PlanningSceneComponents::ROBOT_STATE
|
|
||||||
| moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS
|
|
||||||
| moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES
|
|
||||||
| moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY
|
|
||||||
| moveit_msgs::PlanningSceneComponents::OCTOMAP
|
|
||||||
| moveit_msgs::PlanningSceneComponents::TRANSFORMS
|
|
||||||
| moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX
|
|
||||||
| moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING
|
|
||||||
| moveit_msgs::PlanningSceneComponents::OBJECT_COLORS;
|
|
||||||
|
|
||||||
succeeded_= client.call(req, res);
|
|
||||||
|
|
||||||
if(!succeeded_)
|
|
||||||
return false;
|
|
||||||
|
|
||||||
// empty trajectory ref -> this node only produces states
|
// empty trajectory ref -> this node only produces states
|
||||||
robot_trajectory::RobotTrajectoryPtr traj;
|
robot_trajectory::RobotTrajectoryPtr traj;
|
||||||
|
|
||||||
planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(robot_model_));
|
sendBothWays(traj, scene_);
|
||||||
ps->setPlanningSceneMsg(res.scene);
|
|
||||||
|
|
||||||
sendBothWays(traj, ps);
|
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|||||||
31
src/task.cpp
31
src/task.cpp
@ -1,6 +1,9 @@
|
|||||||
#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 <ros/ros.h>
|
||||||
|
|
||||||
|
#include <moveit_msgs/GetPlanningScene.h>
|
||||||
#include <moveit/robot_model_loader/robot_model_loader.h>
|
#include <moveit/robot_model_loader/robot_model_loader.h>
|
||||||
|
|
||||||
moveit::task_constructor::Task::Task()
|
moveit::task_constructor::Task::Task()
|
||||||
@ -8,6 +11,32 @@ moveit::task_constructor::Task::Task()
|
|||||||
rml_.reset(new robot_model_loader::RobotModelLoader);
|
rml_.reset(new robot_model_loader::RobotModelLoader);
|
||||||
if( !rml_->getModel() )
|
if( !rml_->getModel() )
|
||||||
throw Exception("Task failed to construct RobotModel");
|
throw Exception("Task failed to construct RobotModel");
|
||||||
|
|
||||||
|
ros::NodeHandle h;
|
||||||
|
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
|
||||||
|
client.waitForExistence();
|
||||||
|
|
||||||
|
moveit_msgs::GetPlanningScene::Request req;
|
||||||
|
moveit_msgs::GetPlanningScene::Response res;
|
||||||
|
|
||||||
|
req.components.components =
|
||||||
|
moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS
|
||||||
|
| moveit_msgs::PlanningSceneComponents::ROBOT_STATE
|
||||||
|
| moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS
|
||||||
|
| moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES
|
||||||
|
| moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY
|
||||||
|
| moveit_msgs::PlanningSceneComponents::OCTOMAP
|
||||||
|
| moveit_msgs::PlanningSceneComponents::TRANSFORMS
|
||||||
|
| moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX
|
||||||
|
| moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING
|
||||||
|
| moveit_msgs::PlanningSceneComponents::OBJECT_COLORS;
|
||||||
|
|
||||||
|
if(!client.call(req, res)){
|
||||||
|
throw Exception("Task failed to aquire current PlanningScene");
|
||||||
|
}
|
||||||
|
|
||||||
|
planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(rml_->getModel()));
|
||||||
|
ps->setPlanningSceneMsg(res.scene);
|
||||||
}
|
}
|
||||||
|
|
||||||
void moveit::task_constructor::Task::addStart( SubTaskPtr subtask ){
|
void moveit::task_constructor::Task::addStart( SubTaskPtr subtask ){
|
||||||
@ -32,6 +61,6 @@ bool moveit::task_constructor::Task::plan(){
|
|||||||
|
|
||||||
|
|
||||||
void moveit::task_constructor::Task::addSubTask( SubTaskPtr subtask ){
|
void moveit::task_constructor::Task::addSubTask( SubTaskPtr subtask ){
|
||||||
subtask->setRobotModel( rml_->getModel() );
|
subtask->setPlanningScene( scene_ );
|
||||||
subtasks_.push_back( subtask );
|
subtasks_.push_back( subtask );
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user