fix Task::operator=(Task&&)

To move a task instance to another one, it's not sufficient to swap all task members,
but we also need to adapt all back pointers, i.e. me_ and parent_ pointers of children,
to point to the (swapped) task instances.
This commit is contained in:
Robert Haschke 2019-06-03 16:36:58 +02:00
parent 26f679bd8e
commit 95455c2a26
12 changed files with 239 additions and 81 deletions

View File

@ -69,6 +69,7 @@ namespace moveit { namespace task_constructor {
class ContainerBasePrivate : public StagePrivate
{
friend class ContainerBase;
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
public:
typedef StagePrivate::container_type container_type;

View File

@ -52,9 +52,9 @@
namespace moveit { namespace task_constructor {
MOVEIT_CLASS_FORWARD(Stage)
MOVEIT_CLASS_FORWARD(Task)
MOVEIT_CLASS_FORWARD(SolutionBase)
class TaskPrivate;
class IntrospectionPrivate;
/** The Introspection class provides publishing of task state and solutions.
@ -65,7 +65,7 @@ class Introspection {
IntrospectionPrivate *impl;
public:
Introspection(const Task &task);
Introspection(const TaskPrivate *task);
Introspection(const Introspection &other) = delete;
~Introspection();

View File

@ -54,6 +54,7 @@ class ContainerBase;
class StagePrivate {
friend class Stage;
friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
public:
/// container type used to store children
@ -124,7 +125,7 @@ public:
bool storeFailures() const { return introspection_ != nullptr; }
protected:
Stage* const me_; // associated/owning Stage instance
Stage* me_; // associated/owning Stage instance
std::string name_;
PropertyMap properties_;

View File

@ -51,16 +51,13 @@ namespace moveit { namespace core {
MOVEIT_CLASS_FORWARD(RobotState)
}}
namespace robot_model_loader {
MOVEIT_CLASS_FORWARD(RobotModelLoader)
}
namespace moveit { namespace task_constructor {
MOVEIT_CLASS_FORWARD(Stage)
MOVEIT_CLASS_FORWARD(ContainerBase)
MOVEIT_CLASS_FORWARD(Task)
class TaskPrivate;
/** A Task is the root of a tree of stages.
*
* Actually a tasks wraps a single container (by default a SerialContainer),
@ -68,6 +65,8 @@ MOVEIT_CLASS_FORWARD(Task)
*/
class Task : protected WrapperBase {
public:
PRIVATE_CLASS(Task)
// +1 TODO: move into MoveIt! core
static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model,
const std::string &ns = "move_group",
@ -81,7 +80,7 @@ public:
std::string id() const;
const moveit::core::RobotModelConstPtr& getRobotModel() const { return robot_model_; }
const moveit::core::RobotModelConstPtr& getRobotModel() const;
/// setting the robot model also resets the task
void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model);
/// load robot model from given parameter
@ -146,14 +145,6 @@ protected:
void onNewSolution(const SolutionBase &s) override;
private:
std::string id_;
robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
moveit::core::RobotModelConstPtr robot_model_;
bool preempt_requested_;
// introspection and monitoring
std::unique_ptr<Introspection> introspection_;
std::list<Task::TaskCallback> task_cbs_; // functions to monitor task's planning progress
};
inline std::ostream& operator<<(std::ostream& os, const Task& task) {

View File

@ -0,0 +1,72 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Robert Haschke
Desc: Private API of class Task
*/
#pragma once
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/task.h>
namespace robot_model_loader {
MOVEIT_CLASS_FORWARD(RobotModelLoader)
}
namespace moveit { namespace task_constructor {
class TaskPrivate : public WrapperBasePrivate {
friend class Task;
public:
TaskPrivate(Task* me, const std::string& id);
const std::string& id() const { return id_; }
const ContainerBase* stages() const;
protected:
static void swap(StagePrivate*& lhs, StagePrivate*& rhs);
private:
std::string id_;
robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
moveit::core::RobotModelConstPtr robot_model_;
bool preempt_requested_;
// introspection and monitoring
std::unique_ptr<Introspection> introspection_;
std::list<Task::TaskCallback> task_cbs_; // functions to monitor task's planning progress
};
PIMPL_FUNCTIONS(Task)
} }

View File

@ -10,6 +10,7 @@ add_library(${PROJECT_NAME}
${PROJECT_INCLUDE}/stage_p.h
${PROJECT_INCLUDE}/storage.h
${PROJECT_INCLUDE}/task.h
${PROJECT_INCLUDE}/task_p.h
${PROJECT_INCLUDE}/utils.h
${PROJECT_INCLUDE}/solvers/planner_interface.h

View File

@ -35,6 +35,7 @@
/* Authors: Robert Haschke */
#include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/task_p.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/storage.h>
@ -59,8 +60,8 @@ std::string getProcessId() {
class IntrospectionPrivate {
public:
IntrospectionPrivate(const Task &task)
: nh_(std::string("~/") + task.id()) // topics + services are advertised in private namespace
IntrospectionPrivate(const TaskPrivate *task)
: nh_(std::string("~/") + task->id()) // topics + services are advertised in private namespace
, task_(task)
, process_id_(getProcessId())
{
@ -72,14 +73,14 @@ public:
void resetMaps () {
// reset maps
stage_to_id_map_.clear();
stage_to_id_map_[&task_] = 0; // root is task having ID = 0
stage_to_id_map_[task_] = 0; // root is task having ID = 0
id_solution_bimap_.clear();
}
ros::NodeHandle nh_;
/// associated task
const Task &task_;
const TaskPrivate *task_;
const std::string process_id_;
/// publish task detailed description and current state
@ -91,11 +92,11 @@ public:
ros::ServiceServer get_solution_service_;
/// mapping from stages to their id
std::map<const void*, moveit_task_constructor_msgs::StageStatistics::_id_type> stage_to_id_map_;
std::map<const StagePrivate*, moveit_task_constructor_msgs::StageStatistics::_id_type> stage_to_id_map_;
boost::bimap<uint32_t, const SolutionBase*> id_solution_bimap_;
};
Introspection::Introspection(const Task &task)
Introspection::Introspection(const TaskPrivate* task)
: impl(new IntrospectionPrivate(task))
{
impl->get_solution_service_ = impl->nh_.advertiseService(GET_SOLUTION_SERVICE, &Introspection::getSolution, this);
@ -123,7 +124,7 @@ void Introspection::reset()
// send empty task description message to indicate reset
::moveit_task_constructor_msgs::TaskDescription msg;
msg.process_id = impl->process_id_;
msg.id = impl->task_.id();
msg.id = impl->task_->id();
impl->task_description_publisher_.publish(msg);
impl->resetMaps();
@ -139,7 +140,7 @@ void Introspection::fillSolution(moveit_task_constructor_msgs::Solution &msg,
{
s.fillMessage(msg, this);
msg.process_id = impl->process_id_;
msg.task_id = impl->task_.id();
msg.task_id = impl->task_->id();
s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
}
@ -152,7 +153,7 @@ void Introspection::publishSolution(const SolutionBase &s)
void Introspection::publishAllSolutions(bool wait)
{
for (const auto& solution : impl->task_.solutions()) {
for (const auto& solution : impl->task_->stages()->solutions()) {
publishSolution(*solution);
if (wait) {
@ -183,11 +184,11 @@ bool Introspection::getSolution(moveit_task_constructor_msgs::GetSolution::Reque
uint32_t Introspection::stageId(const Stage* const s)
{
return impl->stage_to_id_map_.insert(std::make_pair(s, impl->stage_to_id_map_.size())).first->second;
return impl->stage_to_id_map_.insert(std::make_pair(s->pimpl(), impl->stage_to_id_map_.size())).first->second;
}
uint32_t Introspection::stageId(const Stage* const s) const
{
auto it = impl->stage_to_id_map_.find(s);
auto it = impl->stage_to_id_map_.find(s->pimpl());
if (it == impl->stage_to_id_map_.end())
throw std::runtime_error("unknown stage");
return it->second;
@ -232,7 +233,7 @@ moveit_task_constructor_msgs::TaskDescription& Introspection::fillTaskDescriptio
desc.properties.push_back(p);
}
auto it = impl->stage_to_id_map_.find(stage.pimpl()->parent());
auto it = impl->stage_to_id_map_.find(stage.pimpl()->parent()->pimpl());
assert (it != impl->stage_to_id_map_.cend());
desc.parent_id = it->second;
@ -242,9 +243,9 @@ moveit_task_constructor_msgs::TaskDescription& Introspection::fillTaskDescriptio
};
msg.stages.clear();
impl->task_.stages()->traverseRecursively(stageProcessor);
impl->task_->stages()->traverseRecursively(stageProcessor);
msg.id = impl->task_.id();
msg.id = impl->task_->id();
msg.process_id = impl->process_id_;
return msg;
}
@ -264,9 +265,9 @@ moveit_task_constructor_msgs::TaskStatistics& Introspection::fillTaskStatistics(
};
msg.stages.clear();
impl->task_.stages()->traverseRecursively(stageProcessor);
impl->task_->stages()->traverseRecursively(stageProcessor);
msg.id = impl->task_.id();
msg.id = impl->task_->id();
msg.process_id = impl->process_id_;
return msg;
}

View File

@ -43,6 +43,7 @@
#include <iomanip>
#include <algorithm>
#include <ros/console.h>
#include <utility>
namespace moveit { namespace task_constructor {

View File

@ -36,9 +36,7 @@
/* Authors: Michael Goerner, Robert Haschke */
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/task_p.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
@ -72,11 +70,42 @@ std::string rosNormalizeName(const std::string &name) {
namespace moveit { namespace task_constructor {
TaskPrivate::TaskPrivate(Task* me, const std::string& id)
: WrapperBasePrivate(me, std::string())
, id_(rosNormalizeName(id)), preempt_requested_(false)
{
}
void swap(StagePrivate*& lhs, StagePrivate*& rhs)
{
// It only makes sense to swap pimpl instances of a Task!
// However, due to member protection rules, we can only implement it here
assert(typeid(lhs) == typeid(rhs));
// swap instances
::std::swap(lhs, rhs);
// as well as their me_ pointers
::std::swap(lhs->me_, rhs->me_);
// and redirect the parent pointers of children to new parents
auto& lhs_children = static_cast<ContainerBasePrivate*>(lhs)->children_;
for (auto it = lhs_children.begin(), end = lhs_children.end(); it != end; ++it)
(*it)->pimpl()->setHierarchy(static_cast<ContainerBase*>(lhs->me_), it);
auto& rhs_children = static_cast<ContainerBasePrivate*>(rhs)->children_;
for (auto it = rhs_children.begin(), end = rhs_children.end(); it != end; ++it)
(*it)->pimpl()->setHierarchy(static_cast<ContainerBase*>(rhs->me_), it);
}
const ContainerBase* TaskPrivate::stages() const
{
return children().empty() ? nullptr : static_cast<ContainerBase*>(children().front().get());
}
Task::Task(const std::string& id, ContainerBase::pointer &&container)
: WrapperBase(std::string(), std::move(container)), id_(id), preempt_requested_(false)
: WrapperBase(new TaskPrivate(this, id), std::move(container))
{
if (!id.empty()) stages()->setName(id);
id_ = rosNormalizeName(id);
// monitor state on commandline
//addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout)));
@ -86,18 +115,15 @@ Task::Task(const std::string& id, ContainerBase::pointer &&container)
}
Task::Task(Task&& other)
: WrapperBase(std::string(), std::make_unique<SerialContainer>())
: WrapperBase(new TaskPrivate(this, std::string()), std::make_unique<SerialContainer>())
{
*this = std::move(other);
}
Task& Task::operator=(Task&& other)
{
id_ = std::move(other.id_);
robot_model_ = std::move(other.robot_model_);
introspection_ = std::move(other.introspection_);
task_cbs_ = std::move(other.task_cbs_);
std::swap(pimpl_, other.pimpl_);
clear(); // remove all stages of current task
swap(this->pimpl_, other.pimpl_);
return *this;
}
@ -147,22 +173,25 @@ Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::str
Task::~Task()
{
auto impl = pimpl();
clear(); // remove all stages
robot_model_.reset();
impl->robot_model_.reset();
// only destroy loader after all references to the model are gone!
robot_model_loader_.reset();
impl->robot_model_loader_.reset();
}
void Task::setRobotModel(const core::RobotModelConstPtr& robot_model)
{
auto impl = pimpl();
reset(); // solutions, scenes, etc become invalid
robot_model_ = robot_model;
impl->robot_model_ = robot_model;
}
void Task::loadRobotModel(const std::string& robot_description) {
robot_model_loader_ = std::make_shared<robot_model_loader::RobotModelLoader>(robot_description);
setRobotModel(robot_model_loader_->getModel());
if (!robot_model_)
auto impl = pimpl();
impl->robot_model_loader_ = std::make_shared<robot_model_loader::RobotModelLoader>(robot_description);
setRobotModel(impl->robot_model_loader_->getModel());
if (!impl->robot_model_)
throw Exception("Task failed to construct RobotModel");
}
@ -182,73 +211,77 @@ void Task::clear()
void Task::enableIntrospection(bool enable)
{
if (enable && !introspection_)
introspection_.reset(new Introspection(*this));
else if (!enable && introspection_) {
auto impl = pimpl();
if (enable && !impl->introspection_)
impl->introspection_.reset(new Introspection(impl));
else if (!enable && impl->introspection_) {
// reset introspection instance of all stages
pimpl()->setIntrospection(nullptr);
pimpl()->traverseStages([](Stage& stage, int) {
stage.pimpl()->setIntrospection(nullptr);
return true;
}, 1, UINT_MAX);
introspection_.reset();
impl->introspection_.reset();
}
}
Introspection &Task::introspection()
{
auto impl = pimpl();
enableIntrospection(true);
return *introspection_;
return *impl->introspection_;
}
Task::TaskCallbackList::const_iterator Task::addTaskCallback(TaskCallback &&cb)
{
task_cbs_.emplace_back(std::move(cb));
return --task_cbs_.cend();
auto impl = pimpl();
impl->task_cbs_.emplace_back(std::move(cb));
return --(impl->task_cbs_.cend());
}
void Task::erase(TaskCallbackList::const_iterator which)
{
task_cbs_.erase(which);
pimpl()->task_cbs_.erase(which);
}
void Task::reset()
{
auto impl = pimpl();
// signal introspection, that this task was reset
if (introspection_)
introspection_->reset();
if (impl->introspection_)
impl->introspection_->reset();
WrapperBase::reset();
}
void Task::init()
{
if (!robot_model_)
auto impl = pimpl();
if (!impl->robot_model_)
loadRobotModel();
auto impl = pimpl();
// initialize push connections of wrapped child
StagePrivate *child = wrapped()->pimpl();
child->setPrevEnds(impl->pendingBackward());
child->setNextStarts(impl->pendingForward());
// and *afterwards* initialize all children recursively
stages()->init(robot_model_);
stages()->init(impl->robot_model_);
// task expects its wrapped child to push to both ends, this triggers interface resolution
stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE}));
// and *finally* validate connectivity
stages()->pimpl()->validateConnectivity();
// provide introspection instance to all stages
impl->setIntrospection(introspection_.get());
impl->traverseStages([this](Stage& stage, int) {
stage.pimpl()->setIntrospection(introspection_.get());
impl->setIntrospection(impl->introspection_.get());
impl->traverseStages([impl](Stage& stage, int) {
stage.pimpl()->setIntrospection(impl->introspection_.get());
return true;
}, 1, UINT_MAX);
// first time publish task
if (introspection_)
introspection_->publishTaskDescription();
if (impl->introspection_)
impl->introspection_->publishTaskDescription();
}
bool Task::canCompute() const
@ -263,17 +296,18 @@ void Task::compute()
bool Task::plan(size_t max_solutions)
{
auto impl = pimpl();
reset();
init();
preempt_requested_ = false;
while(ros::ok() && !preempt_requested_ && canCompute() &&
impl->preempt_requested_ = false;
while(ros::ok() && !impl->preempt_requested_ && canCompute() &&
(max_solutions == 0 || numSolutions() < max_solutions)) {
compute();
for (const auto& cb : task_cbs_)
for (const auto& cb : impl->task_cbs_)
cb(*this);
if (introspection_)
introspection_->publishTaskState();
if (impl->introspection_)
impl->introspection_->publishTaskState();
}
printState();
return numSolutions() > 0;
@ -281,7 +315,7 @@ bool Task::plan(size_t max_solutions)
void Task::preempt()
{
preempt_requested_ = true;
pimpl()->preempt_requested_ = true;
}
void Task::execute(const SolutionBase &s)
@ -290,7 +324,7 @@ void Task::execute(const SolutionBase &s)
ac.waitForServer();
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal goal;
s.fillMessage(goal.solution, introspection_.get());
s.fillMessage(goal.solution, pimpl()->introspection_.get());
ac.sendGoal(goal);
ac.waitForResult();
}
@ -298,14 +332,15 @@ void Task::execute(const SolutionBase &s)
void Task::publishAllSolutions(bool wait)
{
enableIntrospection(true);
introspection_->publishAllSolutions(wait);
pimpl()->introspection_->publishAllSolutions(wait);
}
void Task::onNewSolution(const SolutionBase &s)
{
auto impl = pimpl();
// no need to call WrapperBase::onNewSolution!
if (introspection_)
introspection_->publishSolution(s);
if (impl->introspection_)
impl->introspection_->publishSolution(s);
}
ContainerBase* Task::stages()
@ -332,7 +367,13 @@ void Task::setProperty(const std::string &name, const boost::any &value)
std::string Task::id() const
{
return id_;
return pimpl()->id();
}
const core::RobotModelConstPtr& Task::getRobotModel() const
{
auto impl = pimpl();
return impl->robot_model_;
}
void Task::printState(std::ostream& os) const {

View File

@ -13,7 +13,7 @@ if (CATKIN_ENABLE_TESTING)
target_link_libraries(${PROJECT_NAME}-test-stage ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp)
target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} gtest_utils gtest_main)
target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
catkin_add_gtest(${PROJECT_NAME}-test-properties test_properties.cpp)
target_link_libraries(${PROJECT_NAME}-test-properties ${PROJECT_NAME} gtest_main)

View File

@ -1,6 +1,9 @@
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/task_p.h>
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/utils/robot_model_test_utils.h>
#include "gtest_value_printers.h"
#include <gtest/gtest.h>
@ -16,6 +19,15 @@ public:
void compute() override { if (runs > 0) --runs; }
};
class MonitoringGeneratorMockup : public MonitoringGenerator {
public:
MonitoringGeneratorMockup(Stage* monitored)
: MonitoringGenerator("monitoring generator", monitored) {}
bool canCompute() const override { return false; }
void compute() override {}
void onNewSolution(const SolutionBase &s) override {}
};
class PropagatorMockup : public PropagatingEitherWay {
int fw_runs = 0;
int bw_runs = 0;
@ -536,3 +548,40 @@ TEST(Task, move) {
EXPECT_EQ(t1.stages()->numChildren(), 2u);
EXPECT_EQ(t2.stages()->numChildren(), 0u);
}
TEST(Task, reuse) {
// create dummy robot model
moveit::core::RobotModelBuilder builder("robot", "base");
builder.addChain("base->a->b->c", "continuous");
builder.addGroupChain("base", "c", "group");
moveit::core::RobotModelConstPtr robot_model = builder.build();
Task t("first");
t.setRobotModel(robot_model);
auto configure = [] (Task& t) {
auto ref = new stages::FixedState("fixed");
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
ref->setState(scene);
t.add(Stage::pointer(ref));
t.add(std::make_unique<ConnectMockup>());
t.add(std::make_unique<MonitoringGeneratorMockup>(ref));
};
try {
configure(t);
t.plan(1);
t = Task("second");
t.setRobotModel(robot_model);
EXPECT_EQ(static_cast<void*>(t.pimpl()->me()), static_cast<void*>(&t));
EXPECT_EQ(t.pimpl()->children().size(), 1u);
EXPECT_EQ(static_cast<void*>(t.stages()->pimpl()->parent()), static_cast<void*>(&t));
configure(t);
t.plan(1);
} catch (const InitStageException &e) {
ADD_FAILURE() << "InitStageException:" << std::endl << e << t;
}
}

View File

@ -37,7 +37,7 @@
#include "local_task_model.h"
#include "factory_model.h"
#include "properties/property_factory.h"
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/task_p.h>
#include <rviz/properties/property_tree_model.h>
#include <ros/console.h>