Stage::init(PlanningScene) -> Stage::init(RobotModel)

This commit is contained in:
Robert Haschke 2018-02-21 22:26:08 +01:00
parent 2a2d406368
commit 9cd6efe46f
24 changed files with 115 additions and 136 deletions

View File

@ -3,7 +3,7 @@
#include <moveit/task_constructor/solvers/cartesian_path.h> #include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h> #include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit/task_constructor/stages/current_state.h> #include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/task_constructor/stages/move_to.h> #include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/move_relative.h> #include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/connect.h> #include <moveit/task_constructor/stages/connect.h>
@ -18,7 +18,7 @@
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
void spawnObject(planning_scene::PlanningScenePtr scene) { void spawnObject(const planning_scene::PlanningScenePtr& scene) {
moveit_msgs::CollisionObject o; moveit_msgs::CollisionObject o;
o.id= "object"; o.id= "object";
o.header.frame_id= "world"; o.header.frame_id= "world";
@ -52,7 +52,21 @@ int main(int argc, char** argv){
pipeline->setPlannerId("RRTConnectkConfigDefault"); pipeline->setPlannerId("RRTConnectkConfigDefault");
auto cartesian = std::make_shared<solvers::CartesianPath>(); auto cartesian = std::make_shared<solvers::CartesianPath>();
t.add(std::make_unique<stages::CurrentState>()); Stage* initial_stage = nullptr;
// create a fixed initial scene
{
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
auto& state = scene->getCurrentStateNonConst();
state.setToDefaultValues(state.getJointModelGroup("left_arm"), "home");
state.setToDefaultValues(state.getJointModelGroup("right_arm"), "home");
state.update();
spawnObject(scene);
auto initial = std::make_unique<stages::FixedState>();
initial_stage = initial.get();
initial->setState(scene);
t.add(std::move(initial));
}
{ {
auto stage = std::make_unique<stages::FixCollisionObjects>(); auto stage = std::make_unique<stages::FixCollisionObjects>();
stage->restrictDirection(stages::MoveTo::FORWARD); stage->restrictDirection(stages::MoveTo::FORWARD);
@ -97,6 +111,7 @@ int main(int argc, char** argv){
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()), Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
"lh_tool_frame"); "lh_tool_frame");
gengrasp->setAngleDelta(M_PI / 10.); gengrasp->setAngleDelta(M_PI / 10.);
gengrasp->setMonitoredStage(initial_stage);
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp)); auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
ik->properties().configureInitFrom(Stage::PARENT, {"group", "eef", "default_pose"}); ik->properties().configureInitFrom(Stage::PARENT, {"group", "eef", "default_pose"});
@ -157,12 +172,6 @@ int main(int argc, char** argv){
} }
try { try {
auto scene = t.initScene(ros::Duration(0));
auto& state = scene->getCurrentStateNonConst();
state.setToDefaultValues(state.getJointModelGroup("left_arm"), "home");
state.setToDefaultValues(state.getJointModelGroup("right_arm"), "home");
state.update();
spawnObject(scene);
t.plan(); t.plan();
std::cout << "waiting for <enter>\n"; std::cout << "waiting for <enter>\n";
char ch; char ch;

View File

@ -43,7 +43,13 @@ int main(int argc, char** argv){
Task t; Task t;
t.add( std::make_unique<stages::CurrentState>("current state") ); Stage* initial_stage = nullptr;
{
auto initial = std::make_unique<stages::CurrentState>("current state");
initial_stage = initial.get();
t.add(std::move(initial));
}
{ {
auto move= std::make_unique<stages::Gripper>("open gripper"); auto move= std::make_unique<stages::Gripper>("open gripper");
@ -81,6 +87,7 @@ int main(int argc, char** argv){
gengrasp->setObject("object"); gengrasp->setObject("object");
gengrasp->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); gengrasp->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
gengrasp->setAngleDelta(.2); gengrasp->setAngleDelta(.2);
gengrasp->setMonitoredStage(initial_stage);
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp)); auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
ik->setEndEffector("left_gripper"); ik->setEndEffector("left_gripper");

View File

@ -46,7 +46,13 @@ int main(int argc, char** argv){
t.setProperty("planner", std::string("RRTConnectkConfigDefault")); t.setProperty("planner", std::string("RRTConnectkConfigDefault"));
t.setProperty("link", std::string("s_model_tool0")); t.setProperty("link", std::string("s_model_tool0"));
t.add(std::make_unique<stages::CurrentState>("current state")); Stage* initial_stage = nullptr;
{
auto initial = std::make_unique<stages::CurrentState>("current state");
initial_stage = initial.get();
t.add(std::move(initial));
}
{ {
auto move = std::make_unique<stages::Gripper>("open gripper"); auto move = std::make_unique<stages::Gripper>("open gripper");
@ -82,6 +88,7 @@ int main(int argc, char** argv){
gengrasp->setObject("object"); gengrasp->setObject("object");
gengrasp->setToolToGraspTF(Eigen::Translation3d(.03,0,0), "s_model_tool0"); gengrasp->setToolToGraspTF(Eigen::Translation3d(.03,0,0), "s_model_tool0");
gengrasp->setAngleDelta(-.2); gengrasp->setAngleDelta(-.2);
gengrasp->setMonitoredStage(initial_stage);
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp)); auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
ik->properties().configureInitFrom(Stage::PARENT, {"eef"}); ik->properties().configureInitFrom(Stage::PARENT, {"eef"});

View File

@ -62,7 +62,7 @@ public:
virtual void clear(); virtual void clear();
void reset() override; void reset() override;
void init(const planning_scene::PlanningSceneConstPtr& scene) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
/// validate connectivity of children (after init() was done) /// validate connectivity of children (after init() was done)
virtual void validateConnectivity() const; virtual void validateConnectivity() const;
@ -90,7 +90,7 @@ public:
SerialContainer(const std::string& name = "serial container"); SerialContainer(const std::string& name = "serial container");
void reset() override; void reset() override;
void init(const planning_scene::PlanningSceneConstPtr &scene) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
/// validate connectivity of children (after init() was done) /// validate connectivity of children (after init() was done)
void validateConnectivity() const override; void validateConnectivity() const override;
@ -140,7 +140,7 @@ public:
ParallelContainerBase(const std::string &name); ParallelContainerBase(const std::string &name);
void reset() override; void reset() override;
void init(const planning_scene::PlanningSceneConstPtr &scene) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
/// validate connectivity of children (after init() was done) /// validate connectivity of children (after init() was done)
void validateConnectivity() const override; void validateConnectivity() const override;
@ -202,7 +202,7 @@ public:
Fallbacks(const std::string &name) : ParallelContainerBase(name) {} Fallbacks(const std::string &name) : ParallelContainerBase(name) {}
void reset() override; void reset() override;
void init(const planning_scene::PlanningSceneConstPtr &scene) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override; bool canCompute() const override;
bool compute() override; bool compute() override;
}; };

View File

@ -50,6 +50,10 @@
inline const Class##Private* pimpl() const; \ inline const Class##Private* pimpl() const; \
inline Class##Private* pimpl(); inline Class##Private* pimpl();
namespace moveit { namespace core {
MOVEIT_CLASS_FORWARD(RobotModel)
} }
namespace planning_scene { namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene) MOVEIT_CLASS_FORWARD(PlanningScene)
} }
@ -148,7 +152,7 @@ public:
* The planning scene is the initial planning scene of the task. Use it as is or * The planning scene is the initial planning scene of the task. Use it as is or
* to learn about the robot model. * to learn about the robot model.
*/ */
virtual void init(const planning_scene::PlanningSceneConstPtr& scene); virtual void init(const moveit::core::RobotModelConstPtr& robot_model);
const ContainerBase* parent() const; const ContainerBase* parent() const;
const std::string& name() const; const std::string& name() const;
@ -220,7 +224,7 @@ public:
void restrictDirection(Direction dir); void restrictDirection(Direction dir);
void reset() override; void reset() override;
void init(const planning_scene::PlanningSceneConstPtr &scene) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
virtual bool computeForward(const InterfaceState& from) = 0; virtual bool computeForward(const InterfaceState& from) = 0;
virtual bool computeBackward(const InterfaceState& to) = 0; virtual bool computeBackward(const InterfaceState& to) = 0;
@ -314,7 +318,7 @@ public:
MonitoringGenerator(const std::string &name = "monitoring generator", Stage* monitored = nullptr); MonitoringGenerator(const std::string &name = "monitoring generator", Stage* monitored = nullptr);
void setMonitoredStage(Stage* monitored); void setMonitoredStage(Stage* monitored);
void init(const planning_scene::PlanningSceneConstPtr &scene) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
protected: protected:
MonitoringGenerator(MonitoringGeneratorPrivate* impl); MonitoringGenerator(MonitoringGeneratorPrivate* impl);

View File

@ -47,7 +47,7 @@ class Connect : public Connecting {
public: public:
Connect(std::string name, const solvers::PlannerInterfacePtr &planner); Connect(std::string name, const solvers::PlannerInterfacePtr &planner);
void init(const planning_scene::PlanningSceneConstPtr &scene); void init(const moveit::core::RobotModelConstPtr& robot_model);
bool compute(const InterfaceState &from, const InterfaceState &to); bool compute(const InterfaceState &from, const InterfaceState &to);
protected: protected:

View File

@ -49,7 +49,7 @@ class CurrentState : public Generator {
public: public:
CurrentState(const std::string& name = "current state"); CurrentState(const std::string& name = "current state");
void init(const planning_scene::PlanningSceneConstPtr& scene) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override; bool canCompute() const override;
bool compute() override; bool compute() override;
@ -58,8 +58,8 @@ public:
} }
protected: protected:
moveit::core::RobotModelConstPtr robot_model_;
planning_scene::PlanningScenePtr scene_; planning_scene::PlanningScenePtr scene_;
bool ran_ = false;
}; };
} } } } } }

View File

@ -43,11 +43,10 @@
namespace moveit { namespace task_constructor { namespace stages { namespace moveit { namespace task_constructor { namespace stages {
class GenerateGraspPose : public Generator { class GenerateGraspPose : public MonitoringGenerator {
public: public:
GenerateGraspPose(std::string name); GenerateGraspPose(const std::string& name);
void init(const planning_scene::PlanningSceneConstPtr& scene) override;
bool canCompute() const override; bool canCompute() const override;
bool compute() override; bool compute() override;
@ -64,6 +63,9 @@ public:
} }
void setAngleDelta(double delta); void setAngleDelta(double delta);
protected:
void onNewSolution(const SolutionBase& s) override;
protected: protected:
planning_scene::PlanningScenePtr scene_; planning_scene::PlanningScenePtr scene_;
double current_angle_ = 0.0; double current_angle_ = 0.0;

View File

@ -50,7 +50,7 @@ class Gripper : public PropagatingEitherWay {
public: public:
Gripper(std::string name); Gripper(std::string name);
void init(const planning_scene::PlanningSceneConstPtr &scene); void init(const moveit::core::RobotModelConstPtr& robot_model);
bool computeForward(const InterfaceState& from) override; bool computeForward(const InterfaceState& from) override;
bool computeBackward(const InterfaceState& to) override; bool computeBackward(const InterfaceState& to) override;

View File

@ -46,7 +46,7 @@ class Move : public Connecting {
public: public:
Move(std::string name); Move(std::string name);
void init(const planning_scene::PlanningSceneConstPtr &scene); void init(const moveit::core::RobotModelConstPtr& robot_model);
bool compute(const InterfaceState &from, const InterfaceState &to); bool compute(const InterfaceState &from, const InterfaceState &to);
void setGroup(const std::string &group); void setGroup(const std::string &group);

View File

@ -50,7 +50,7 @@ class MoveRelative : public PropagatingEitherWay {
public: public:
MoveRelative(std::string name, const solvers::PlannerInterfacePtr& planner); MoveRelative(std::string name, const solvers::PlannerInterfacePtr& planner);
void init(const planning_scene::PlanningSceneConstPtr &scene) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool computeForward(const InterfaceState& from) override; bool computeForward(const InterfaceState& from) override;
bool computeBackward(const InterfaceState& to) override; bool computeBackward(const InterfaceState& to) override;

View File

@ -49,7 +49,7 @@ class MoveTo : public PropagatingEitherWay {
public: public:
MoveTo(std::string name, const solvers::PlannerInterfacePtr& planner); MoveTo(std::string name, const solvers::PlannerInterfacePtr& planner);
void init(const planning_scene::PlanningSceneConstPtr &scene) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool computeForward(const InterfaceState& from) override; bool computeForward(const InterfaceState& from) override;
bool computeBackward(const InterfaceState& to) override; bool computeBackward(const InterfaceState& to) override;

View File

@ -46,9 +46,6 @@
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
namespace robot_model_loader {
MOVEIT_CLASS_FORWARD(RobotModelLoader)
}
namespace moveit { namespace core { namespace moveit { namespace core {
MOVEIT_CLASS_FORWARD(RobotModel) MOVEIT_CLASS_FORWARD(RobotModel)
MOVEIT_CLASS_FORWARD(RobotState) MOVEIT_CLASS_FORWARD(RobotState)
@ -76,7 +73,7 @@ public:
~Task(); ~Task();
std::string id() const; std::string id() const;
const moveit::core::RobotModelPtr getRobotModel() const; const moveit::core::RobotModelConstPtr getRobotModel() const { return robot_model_; }
void add(Stage::pointer &&stage); void add(Stage::pointer &&stage);
void clear() override; void clear() override;
@ -92,14 +89,10 @@ public:
/// remove function callback /// remove function callback
void erase(TaskCallbackList::const_iterator which); void erase(TaskCallbackList::const_iterator which);
/// initialize planning scene from get_planning_scene service (waiting given timeout for it)
/// if service is not available or timeout is zero, use an empty planning scene
planning_scene::PlanningScenePtr initScene(ros::Duration timeout = ros::Duration(-1));
/// reset all stages /// reset all stages
void reset() override; void reset() override;
/// initialize all stages with given scene /// initialize all stages with given scene
void init(const planning_scene::PlanningSceneConstPtr &scene) override; void init(const moveit::core::RobotModelConstPtr& model) override;
/// reset, init scene (if not yet done), and init all stages, then start planning /// reset, init scene (if not yet done), and init all stages, then start planning
bool plan(); bool plan();
@ -124,15 +117,13 @@ public:
void setProperty(const std::string& name, const boost::any& value); void setProperty(const std::string& name, const boost::any& value);
protected: protected:
void initModel();
bool canCompute() const override; bool canCompute() const override;
bool compute() override; bool compute() override;
void onNewSolution(const SolutionBase &s) override; void onNewSolution(const SolutionBase &s) override;
private: private:
std::string id_; std::string id_;
robot_model_loader::RobotModelLoaderPtr rml_; moveit::core::RobotModelConstPtr robot_model_;
planning_scene::PlanningScenePtr scene_; // initial scene
// introspection and monitoring // introspection and monitoring
std::unique_ptr<Introspection> introspection_; std::unique_ptr<Introspection> introspection_;

View File

@ -198,12 +198,12 @@ void ContainerBase::reset()
Stage::reset(); Stage::reset();
} }
void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
auto impl = pimpl(); auto impl = pimpl();
auto& children = impl->children(); auto& children = impl->children();
Stage::init(scene); Stage::init(robot_model);
// we need to have some children to do the actual work // we need to have some children to do the actual work
if (children.empty()) if (children.empty())
@ -212,7 +212,7 @@ void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
// recursively init all children and accumulate errors // recursively init all children and accumulate errors
InitStageException errors; InitStageException errors;
for (auto& child : children) { for (auto& child : children) {
try { child->init(scene); } catch (InitStageException &e) { errors.append(e); } try { child->init(robot_model); } catch (InitStageException &e) { errors.append(e); }
} }
if (errors) throw errors; if (errors) throw errors;
@ -408,14 +408,14 @@ bool SerialContainerPrivate::connect(container_type::const_iterator cur)
* This context is provided by two stages pushing from both ends * This context is provided by two stages pushing from both ends
* into a (potentially long) sequence of propagating stages (tbd). * into a (potentially long) sequence of propagating stages (tbd).
*/ */
void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
// reset pull interfaces // reset pull interfaces
auto impl = pimpl(); auto impl = pimpl();
impl->starts_.reset(); impl->starts_.reset();
impl->ends_.reset(); impl->ends_.reset();
ContainerBase::init(scene); // throws if there are no children ContainerBase::init(robot_model); // throws if there are no children
auto start = impl->children().begin(); auto start = impl->children().begin();
auto last = --impl->children().end(); auto last = --impl->children().end();
@ -703,10 +703,10 @@ void ParallelContainerBase::reset()
/* States received by the container need to be copied to all children's pull interfaces. /* States received by the container need to be copied to all children's pull interfaces.
* States generated by children can be directly forwarded into the container's push interfaces. * States generated by children can be directly forwarded into the container's push interfaces.
*/ */
void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) void ParallelContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
// recursively init children // recursively init children
ContainerBase::init(scene); ContainerBase::init(robot_model);
auto impl = pimpl(); auto impl = pimpl();
// determine the union of interfaces required by children // determine the union of interfaces required by children
@ -886,9 +886,9 @@ void Fallbacks::reset()
ParallelContainerBase::reset(); ParallelContainerBase::reset();
} }
void Fallbacks::init(const planning_scene::PlanningSceneConstPtr &scene) void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
ParallelContainerBase::init(scene); ParallelContainerBase::init(robot_model);
active_child_ = pimpl()->children().front().get(); active_child_ = pimpl()->children().front().get();
} }

View File

@ -128,7 +128,7 @@ void Stage::reset()
impl->properties_.reset(); impl->properties_.reset();
} }
void Stage::init(const planning_scene::PlanningSceneConstPtr &scene) void Stage::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
// init properties once from parent // init properties once from parent
auto impl = pimpl(); auto impl = pimpl();
@ -390,9 +390,9 @@ void PropagatingEitherWay::reset()
ComputeBase::reset(); ComputeBase::reset();
} }
void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr& scene) void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
Stage::init(scene); Stage::init(robot_model);
auto impl = pimpl(); auto impl = pimpl();
// In AUTO-mode, i.e. when auto-detecting direction of propagation from context, // In AUTO-mode, i.e. when auto-detecting direction of propagation from context,
@ -524,7 +524,7 @@ void MonitoringGenerator::setMonitoredStage(Stage* monitored)
impl->monitored_ = monitored; impl->monitored_ = monitored;
} }
void MonitoringGenerator::init(const planning_scene::PlanningSceneConstPtr& scene) void MonitoringGenerator::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
Generator::init(robot_model); Generator::init(robot_model);

View File

@ -50,10 +50,10 @@ Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner)
p.declare<std::string>("group", "name of planning group"); p.declare<std::string>("group", "name of planning group");
} }
void Connect::init(const planning_scene::PlanningSceneConstPtr &scene) void Connect::init(const core::RobotModelConstPtr& robot_model)
{ {
Connecting::init(scene); Connecting::init(robot_model);
planner_->init(scene->getRobotModel()); planner_->init(robot_model);
} }
bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {

View File

@ -53,19 +53,19 @@ CurrentState::CurrentState(const std::string &name)
p.declare<ros::Duration>("timeout", ros::Duration(-1), "max time to wait for get_planning_scene service"); p.declare<ros::Duration>("timeout", ros::Duration(-1), "max time to wait for get_planning_scene service");
} }
void CurrentState::init(const planning_scene::PlanningSceneConstPtr &scene) void CurrentState::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
Generator::init(scene); Generator::init(robot_model);
scene_ = scene->diff(); robot_model_ = robot_model;
scene_.reset();
} }
bool CurrentState::canCompute() const{ bool CurrentState::canCompute() const{
return !ran_; return !scene_;
} }
bool CurrentState::compute(){ bool CurrentState::compute() {
auto rml = std::make_shared<robot_model_loader::RobotModelLoader>(); scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
scene_ = std::make_shared<planning_scene::PlanningScene>(rml->getModel());
ros::NodeHandle h; ros::NodeHandle h;
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene"); ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
@ -90,7 +90,6 @@ bool CurrentState::compute(){
if (client.call(req, res)) { if (client.call(req, res)) {
scene_->setPlanningSceneMsg(res.scene); scene_->setPlanningSceneMsg(res.scene);
spawn(InterfaceState(scene_), 0.0); spawn(InterfaceState(scene_), 0.0);
ran_= true;
return true; return true;
} }
} }

View File

@ -46,8 +46,8 @@
namespace moveit { namespace task_constructor { namespace stages { namespace moveit { namespace task_constructor { namespace stages {
GenerateGraspPose::GenerateGraspPose(std::string name) GenerateGraspPose::GenerateGraspPose(const std::string& name)
: Generator(name) : MonitoringGenerator(name)
{ {
auto& p = properties(); auto& p = properties();
p.declare<std::string>("eef", "name of end-effector group"); p.declare<std::string>("eef", "name of end-effector group");
@ -57,12 +57,6 @@ GenerateGraspPose::GenerateGraspPose(std::string name)
p.declare<double>("angle_delta", 0.1, "angular steps (rad)"); p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
} }
void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
{
Generator::init(scene);
scene_ = scene->diff();
}
void GenerateGraspPose::setEndEffector(const std::string &eef){ void GenerateGraspPose::setEndEffector(const std::string &eef){
setProperty("eef", eef); setProperty("eef", eef);
} }
@ -91,8 +85,15 @@ void GenerateGraspPose::setAngleDelta(double delta){
setProperty("angle_delta", delta); setProperty("angle_delta", delta);
} }
void GenerateGraspPose::onNewSolution(const SolutionBase& s)
{
if (scene_)
ROS_WARN_NAMED("GenerateGraspPose", "got additional solution from monitored stage");
scene_ = s.end()->scene()->diff();
}
bool GenerateGraspPose::canCompute() const{ bool GenerateGraspPose::canCompute() const{
return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI; return scene_ && current_angle_ < 2*M_PI && current_angle_ > -2*M_PI;
} }
bool GenerateGraspPose::compute(){ bool GenerateGraspPose::compute(){

View File

@ -59,10 +59,10 @@ Gripper::Gripper(std::string name)
p.declare<std::string>("grasp_object", "name of grasp object"); p.declare<std::string>("grasp_object", "name of grasp object");
} }
void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene) void Gripper::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
PropagatingEitherWay::init(scene); PropagatingEitherWay::init(robot_model);
planner_ = Task::createPlanner(scene->getRobotModel()); planner_ = Task::createPlanner(robot_model);
} }
void Gripper::setEndEffector(std::string eef){ void Gripper::setEndEffector(std::string eef){

View File

@ -58,10 +58,10 @@ Move::Move(std::string name)
p.declare<std::string>("planner", "", "planner id"); p.declare<std::string>("planner", "", "planner id");
} }
void Move::init(const planning_scene::PlanningSceneConstPtr &scene) void Move::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
Connecting::init(scene); Connecting::init(robot_model);
planner_ = Task::createPlanner(scene->getRobotModel()); planner_ = Task::createPlanner(robot_model);
} }
void Move::setGroup(const std::string& group){ void Move::setGroup(const std::string& group){

View File

@ -90,10 +90,10 @@ void MoveRelative::along(const geometry_msgs::Vector3Stamped &direction)
setProperty("direction", direction); setProperty("direction", direction);
} }
void MoveRelative::init(const planning_scene::PlanningSceneConstPtr &scene) void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
PropagatingEitherWay::init(scene); PropagatingEitherWay::init(robot_model);
planner_->init(scene->getRobotModel()); planner_->init(robot_model);
} }
bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene, bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,

View File

@ -79,10 +79,10 @@ void MoveTo::setGoal(const std::string &joint_pose)
setProperty("joint_pose", joint_pose); setProperty("joint_pose", joint_pose);
} }
void MoveTo::init(const planning_scene::PlanningSceneConstPtr &scene) void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model)
{ {
PropagatingEitherWay::init(scene); PropagatingEitherWay::init(robot_model);
planner_->init(scene->getRobotModel()); planner_->init(robot_model);
} }
bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene, bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,

View File

@ -43,8 +43,6 @@
#include <ros/ros.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>
#include <moveit/planning_pipeline/planning_pipeline.h> #include <moveit/planning_pipeline/planning_pipeline.h>
@ -55,7 +53,10 @@ namespace moveit { namespace task_constructor {
Task::Task(const std::string& id, ContainerBase::pointer &&container) Task::Task(const std::string& id, ContainerBase::pointer &&container)
: WrapperBase(std::string(), std::move(container)), id_(id) : WrapperBase(std::string(), std::move(container)), id_(id)
{ {
initModel(); robot_model_loader::RobotModelLoader rml;
robot_model_ = rml.getModel();
if (!robot_model_)
throw Exception("Task failed to construct RobotModel");
// monitor state on commandline // monitor state on commandline
//addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout))); //addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout)));
@ -63,47 +64,6 @@ Task::Task(const std::string& id, ContainerBase::pointer &&container)
enableIntrospection(true); enableIntrospection(true);
} }
void Task::initModel () {
rml_.reset(new robot_model_loader::RobotModelLoader);
if( !rml_->getModel() )
throw Exception("Task failed to construct RobotModel");
}
const moveit::core::RobotModelPtr Task::getRobotModel() const {
return rml_ ? rml_->getModel() : moveit::core::RobotModelPtr();
}
planning_scene::PlanningScenePtr Task::initScene(ros::Duration timeout) {
assert(rml_);
scene_ = std::make_shared<planning_scene::PlanningScene>(rml_->getModel());
ros::NodeHandle h;
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
if (timeout != ros::Duration() && client.waitForExistence(timeout)) {
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))
std::const_pointer_cast<planning_scene::PlanningScene>(scene_)->setPlanningSceneMsg(res.scene);
return scene_;
}
ROS_WARN("failed to acquire current PlanningScene, using empty one");
return scene_;
}
planning_pipeline::PlanningPipelinePtr planning_pipeline::PlanningPipelinePtr
Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns, Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns,
const std::string& planning_plugin_param_name, const std::string& planning_plugin_param_name,
@ -184,7 +144,7 @@ void Task::reset()
WrapperBase::reset(); WrapperBase::reset();
} }
void Task::init(const planning_scene::PlanningSceneConstPtr &scene) void Task::init(const moveit::core::RobotModelConstPtr& model)
{ {
auto impl = pimpl(); auto impl = pimpl();
// initialize push connections of wrapped child // initialize push connections of wrapped child
@ -193,7 +153,7 @@ void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
child->setNextStarts(impl->pendingForward()); child->setNextStarts(impl->pendingForward());
// and *afterwards* initialize all children recursively // and *afterwards* initialize all children recursively
stages()->init(scene); stages()->init(model);
// task expects its wrapped child to push to both ends, this triggers interface resolution // task expects its wrapped child to push to both ends, this triggers interface resolution
stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE})); stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE}));
// and *finally* validate connectivity // and *finally* validate connectivity
@ -223,8 +183,7 @@ bool Task::compute()
bool Task::plan() bool Task::plan()
{ {
reset(); reset();
if (!scene_) initScene(); init(robot_model_);
init(scene_);
while(ros::ok() && canCompute()) { while(ros::ok() && canCompute()) {
if (compute()) { if (compute()) {

View File

@ -68,7 +68,7 @@ void append(ContainerBase& c, const std::initializer_list<StageType>& types, int
class SerialTest : public ::testing::Test { class SerialTest : public ::testing::Test {
protected: protected:
planning_scene::PlanningScenePtr scene; moveit::core::RobotModelConstPtr robot_model;
SerialContainer serial; SerialContainer serial;
InterfacePtr dummy; InterfacePtr dummy;
@ -112,7 +112,7 @@ protected:
reset(start, end); reset(start, end);
append(serial, types); append(serial, types);
try { try {
serial.init(scene); serial.init(robot_model);
// prune pull interfaces based on provided external interface (start, end) // prune pull interfaces based on provided external interface (start, end)
InterfaceFlags accepted; InterfaceFlags accepted;
if (start) accepted |= WRITES_PREV_END; if (start) accepted |= WRITES_PREV_END;
@ -141,7 +141,7 @@ TEST_F(SerialTest, insertion_order) {
SerialContainerPrivate *impl = serial.pimpl(); SerialContainerPrivate *impl = serial.pimpl();
EXPECT_EQ(impl->parent(), nullptr); EXPECT_EQ(impl->parent(), nullptr);
EXPECT_THROW(serial.init(scene), InitStageException); EXPECT_THROW(serial.init(robot_model), InitStageException);
VALIDATE(); VALIDATE();
/***** inserting first stage *****/ /***** inserting first stage *****/