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:
v4hn 2017-02-28 16:47:09 +01:00
parent 8af3645af9
commit 039f1e6896
7 changed files with 68 additions and 49 deletions

View File

@ -2,7 +2,17 @@
#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 {

View File

@ -10,8 +10,11 @@
#include <vector>
#include <list>
namespace moveit::task_constructor {
namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene);
}
namespace moveit::task_constructor {
MOVEIT_CLASS_FORWARD(SubTask);
@ -27,7 +30,7 @@ public:
const std::list<InterfaceState>& getBegin();
const std::list<SubTrajectory>& getTrajectories();
void setRobotModel(robot_model::RobotModelConstPtr);
void setPlanningScene(planning_scene::PlanningSceneConstPtr);
void addPredecessor(SubTaskPtr);
void addSuccessor(SubTaskPtr);
@ -35,13 +38,13 @@ public:
protected:
void sendForward();
void sendBackward();
void sendBothWays(robot_trajectory::RobotTrajectoryPtr&, planning_scene::PlanningScenePtr&);
void sendBothWays(robot_trajectory::RobotTrajectoryPtr&, planning_scene::PlanningSceneConstPtr&);
void connectBegin(InterfaceState&, SubTrajectory*);
void connectEnd(InterfaceState&, SubTrajectory*);
InterfaceState* newBegin(planning_scene::PlanningScenePtr, SubTrajectory*);
InterfaceState* newEnd(planning_scene::PlanningScenePtr, SubTrajectory*);
InterfaceState* newBegin(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
InterfaceState* newEnd(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
const std::string name_;
@ -54,7 +57,7 @@ protected:
std::list<InterfaceState> beginnings_;
std::list<InterfaceState> endings_;
robot_model::RobotModelConstPtr robot_model_;
planning_scene::PlanningSceneConstPtr scene_;
};
}

View File

@ -8,14 +8,14 @@ namespace moveit::task_constructor::subtasks {
class CurrentState : public SubTask {
public:
using SubTask::SubTask;
CurrentState(std::string name);
virtual bool canCompute();
virtual bool compute();
protected:
bool succeeded_;
bool ran_;
};
}

View File

@ -6,8 +6,8 @@
#include <vector>
namespace moveit::core {
MOVEIT_CLASS_FORWARD(RobotModel);
namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene);
}
namespace robot_model_loader {
@ -35,6 +35,7 @@ protected:
std::vector<SubTaskPtr> subtasks_;
planning_scene::PlanningScenePtr scene_;
robot_model_loader::RobotModelLoaderPtr rml_;
};

View File

@ -33,13 +33,12 @@ moveit::task_constructor::SubTask::getTrajectories(){
}
void
moveit::task_constructor::SubTask::setRobotModel(robot_model::RobotModelConstPtr model){
robot_model_= model;
moveit::task_constructor::SubTask::setPlanningScene(planning_scene::PlanningSceneConstPtr scene){
scene_= scene;
}
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();
SubTrajectory& subtraj= trajectories_.back();
subtraj.trajectory= traj;
@ -54,13 +53,13 @@ moveit::task_constructor::SubTask::sendBothWays(robot_trajectory::RobotTrajector
}
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));
return &beginnings_.back();
}
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));
return &endings_.back();
}

View File

@ -1,47 +1,24 @@
#include <moveit_task_constructor/subtasks/current_state.h>
#include <ros/ros.h>
#include <moveit_msgs/GetPlanningScene.h>
moveit::task_constructor::subtasks::CurrentState::CurrentState(std::string name)
: moveit::task_constructor::SubTask::SubTask(name)
{
ran_= false;
}
bool
moveit::task_constructor::subtasks::CurrentState::canCompute(){
return !succeeded_;
return !ran_;
}
bool
moveit::task_constructor::subtasks::CurrentState::compute(){
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;
succeeded_= client.call(req, res);
if(!succeeded_)
return false;
ran_= true;
// empty trajectory ref -> this node only produces states
robot_trajectory::RobotTrajectoryPtr traj;
planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(robot_model_));
ps->setPlanningSceneMsg(res.scene);
sendBothWays(traj, ps);
sendBothWays(traj, scene_);
return true;
}

View File

@ -1,6 +1,9 @@
#include <moveit_task_constructor/task.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>
moveit::task_constructor::Task::Task()
@ -8,6 +11,32 @@ moveit::task_constructor::Task::Task()
rml_.reset(new robot_model_loader::RobotModelLoader);
if( !rml_->getModel() )
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 ){
@ -32,6 +61,6 @@ bool moveit::task_constructor::Task::plan(){
void moveit::task_constructor::Task::addSubTask( SubTaskPtr subtask ){
subtask->setRobotModel( rml_->getModel() );
subtask->setPlanningScene( scene_ );
subtasks_.push_back( subtask );
}