mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
26f679bd8e
commit
95455c2a26
@ -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;
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -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_;
|
||||
|
||||
|
||||
@ -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) {
|
||||
|
||||
72
core/include/moveit/task_constructor/task_p.h
Normal file
72
core/include/moveit/task_constructor/task_p.h
Normal 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)
|
||||
|
||||
} }
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -43,6 +43,7 @@
|
||||
#include <iomanip>
|
||||
#include <algorithm>
|
||||
#include <ros/console.h>
|
||||
#include <utility>
|
||||
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
|
||||
@ -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 {
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -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>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user