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/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_relative.h>
#include <moveit/task_constructor/stages/connect.h>
@ -18,7 +18,7 @@
using namespace moveit::task_constructor;
void spawnObject(planning_scene::PlanningScenePtr scene) {
void spawnObject(const planning_scene::PlanningScenePtr& scene) {
moveit_msgs::CollisionObject o;
o.id= "object";
o.header.frame_id= "world";
@ -52,7 +52,21 @@ int main(int argc, char** argv){
pipeline->setPlannerId("RRTConnectkConfigDefault");
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>();
stage->restrictDirection(stages::MoveTo::FORWARD);
@ -97,6 +111,7 @@ int main(int argc, char** argv){
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
"lh_tool_frame");
gengrasp->setAngleDelta(M_PI / 10.);
gengrasp->setMonitoredStage(initial_stage);
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
ik->properties().configureInitFrom(Stage::PARENT, {"group", "eef", "default_pose"});
@ -157,12 +172,6 @@ int main(int argc, char** argv){
}
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();
std::cout << "waiting for <enter>\n";
char ch;

View File

@ -43,7 +43,13 @@ int main(int argc, char** argv){
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");
@ -81,6 +87,7 @@ int main(int argc, char** argv){
gengrasp->setObject("object");
gengrasp->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
gengrasp->setAngleDelta(.2);
gengrasp->setMonitoredStage(initial_stage);
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
ik->setEndEffector("left_gripper");

View File

@ -46,7 +46,13 @@ int main(int argc, char** argv){
t.setProperty("planner", std::string("RRTConnectkConfigDefault"));
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");
@ -82,6 +88,7 @@ int main(int argc, char** argv){
gengrasp->setObject("object");
gengrasp->setToolToGraspTF(Eigen::Translation3d(.03,0,0), "s_model_tool0");
gengrasp->setAngleDelta(-.2);
gengrasp->setMonitoredStage(initial_stage);
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
ik->properties().configureInitFrom(Stage::PARENT, {"eef"});

View File

@ -62,7 +62,7 @@ public:
virtual void clear();
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)
virtual void validateConnectivity() const;
@ -90,7 +90,7 @@ public:
SerialContainer(const std::string& name = "serial container");
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)
void validateConnectivity() const override;
@ -140,7 +140,7 @@ public:
ParallelContainerBase(const std::string &name);
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)
void validateConnectivity() const override;
@ -202,7 +202,7 @@ public:
Fallbacks(const std::string &name) : ParallelContainerBase(name) {}
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 compute() override;
};

View File

@ -50,6 +50,10 @@
inline const Class##Private* pimpl() const; \
inline Class##Private* pimpl();
namespace moveit { namespace core {
MOVEIT_CLASS_FORWARD(RobotModel)
} }
namespace planning_scene {
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
* 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 std::string& name() const;
@ -220,7 +224,7 @@ public:
void restrictDirection(Direction dir);
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 computeBackward(const InterfaceState& to) = 0;
@ -314,7 +318,7 @@ public:
MonitoringGenerator(const std::string &name = "monitoring generator", Stage* monitored = nullptr);
void setMonitoredStage(Stage* monitored);
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
protected:
MonitoringGenerator(MonitoringGeneratorPrivate* impl);

View File

@ -47,7 +47,7 @@ class Connect : public Connecting {
public:
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);
protected:

View File

@ -49,7 +49,7 @@ class CurrentState : public Generator {
public:
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 compute() override;
@ -58,8 +58,8 @@ public:
}
protected:
moveit::core::RobotModelConstPtr robot_model_;
planning_scene::PlanningScenePtr scene_;
bool ran_ = false;
};
} } }

View File

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

View File

@ -50,7 +50,7 @@ class Gripper : public PropagatingEitherWay {
public:
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 computeBackward(const InterfaceState& to) override;

View File

@ -46,7 +46,7 @@ class Move : public Connecting {
public:
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);
void setGroup(const std::string &group);

View File

@ -50,7 +50,7 @@ class MoveRelative : public PropagatingEitherWay {
public:
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 computeBackward(const InterfaceState& to) override;

View File

@ -49,7 +49,7 @@ class MoveTo : public PropagatingEitherWay {
public:
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 computeBackward(const InterfaceState& to) override;

View File

@ -46,9 +46,6 @@
#include <moveit/macros/class_forward.h>
namespace robot_model_loader {
MOVEIT_CLASS_FORWARD(RobotModelLoader)
}
namespace moveit { namespace core {
MOVEIT_CLASS_FORWARD(RobotModel)
MOVEIT_CLASS_FORWARD(RobotState)
@ -76,7 +73,7 @@ public:
~Task();
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 clear() override;
@ -92,14 +89,10 @@ public:
/// remove function callback
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
void reset() override;
/// 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
bool plan();
@ -124,15 +117,13 @@ public:
void setProperty(const std::string& name, const boost::any& value);
protected:
void initModel();
bool canCompute() const override;
bool compute() override;
void onNewSolution(const SolutionBase &s) override;
private:
std::string id_;
robot_model_loader::RobotModelLoaderPtr rml_;
planning_scene::PlanningScenePtr scene_; // initial scene
moveit::core::RobotModelConstPtr robot_model_;
// introspection and monitoring
std::unique_ptr<Introspection> introspection_;

View File

@ -198,12 +198,12 @@ void ContainerBase::reset()
Stage::reset();
}
void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model)
{
auto impl = pimpl();
auto& children = impl->children();
Stage::init(scene);
Stage::init(robot_model);
// we need to have some children to do the actual work
if (children.empty())
@ -212,7 +212,7 @@ void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
// recursively init all children and accumulate errors
InitStageException errors;
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;
@ -408,14 +408,14 @@ bool SerialContainerPrivate::connect(container_type::const_iterator cur)
* This context is provided by two stages pushing from both ends
* 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
auto impl = pimpl();
impl->starts_.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 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 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
ContainerBase::init(scene);
ContainerBase::init(robot_model);
auto impl = pimpl();
// determine the union of interfaces required by children
@ -886,9 +886,9 @@ void Fallbacks::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();
}

View File

@ -128,7 +128,7 @@ void Stage::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
auto impl = pimpl();
@ -390,9 +390,9 @@ void PropagatingEitherWay::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();
// 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;
}
void MonitoringGenerator::init(const planning_scene::PlanningSceneConstPtr& scene)
void MonitoringGenerator::init(const moveit::core::RobotModelConstPtr& 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");
}
void Connect::init(const planning_scene::PlanningSceneConstPtr &scene)
void Connect::init(const core::RobotModelConstPtr& robot_model)
{
Connecting::init(scene);
planner_->init(scene->getRobotModel());
Connecting::init(robot_model);
planner_->init(robot_model);
}
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");
}
void CurrentState::init(const planning_scene::PlanningSceneConstPtr &scene)
void CurrentState::init(const moveit::core::RobotModelConstPtr& robot_model)
{
Generator::init(scene);
scene_ = scene->diff();
Generator::init(robot_model);
robot_model_ = robot_model;
scene_.reset();
}
bool CurrentState::canCompute() const{
return !ran_;
return !scene_;
}
bool CurrentState::compute() {
auto rml = std::make_shared<robot_model_loader::RobotModelLoader>();
scene_ = std::make_shared<planning_scene::PlanningScene>(rml->getModel());
scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
ros::NodeHandle h;
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
@ -90,7 +90,6 @@ bool CurrentState::compute(){
if (client.call(req, res)) {
scene_->setPlanningSceneMsg(res.scene);
spawn(InterfaceState(scene_), 0.0);
ran_= true;
return true;
}
}

View File

@ -46,8 +46,8 @@
namespace moveit { namespace task_constructor { namespace stages {
GenerateGraspPose::GenerateGraspPose(std::string name)
: Generator(name)
GenerateGraspPose::GenerateGraspPose(const std::string& name)
: MonitoringGenerator(name)
{
auto& p = properties();
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)");
}
void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
{
Generator::init(scene);
scene_ = scene->diff();
}
void GenerateGraspPose::setEndEffector(const std::string &eef){
setProperty("eef", eef);
}
@ -91,8 +85,15 @@ void GenerateGraspPose::setAngleDelta(double 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{
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(){

View File

@ -59,10 +59,10 @@ Gripper::Gripper(std::string name)
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);
planner_ = Task::createPlanner(scene->getRobotModel());
PropagatingEitherWay::init(robot_model);
planner_ = Task::createPlanner(robot_model);
}
void Gripper::setEndEffector(std::string eef){

View File

@ -58,10 +58,10 @@ Move::Move(std::string name)
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);
planner_ = Task::createPlanner(scene->getRobotModel());
Connecting::init(robot_model);
planner_ = Task::createPlanner(robot_model);
}
void Move::setGroup(const std::string& group){

View File

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

View File

@ -43,8 +43,6 @@
#include <ros/ros.h>
#include <moveit_msgs/GetPlanningScene.h>
#include <moveit/robot_model_loader/robot_model_loader.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)
: 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
//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);
}
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
Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns,
const std::string& planning_plugin_param_name,
@ -184,7 +144,7 @@ void Task::reset()
WrapperBase::reset();
}
void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
void Task::init(const moveit::core::RobotModelConstPtr& model)
{
auto impl = pimpl();
// initialize push connections of wrapped child
@ -193,7 +153,7 @@ void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
child->setNextStarts(impl->pendingForward());
// 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
stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE}));
// and *finally* validate connectivity
@ -223,8 +183,7 @@ bool Task::compute()
bool Task::plan()
{
reset();
if (!scene_) initScene();
init(scene_);
init(robot_model_);
while(ros::ok() && canCompute()) {
if (compute()) {

View File

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