mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
publish task info in private namespace: ~/taskid
- default task id is empty - introspection becomes member of task, created with Task::enableIntrospection(true)
This commit is contained in:
parent
af2009c19b
commit
cb85e1b864
@ -17,46 +17,47 @@ namespace moveit { namespace task_constructor {
|
|||||||
MOVEIT_CLASS_FORWARD(Task)
|
MOVEIT_CLASS_FORWARD(Task)
|
||||||
MOVEIT_CLASS_FORWARD(SolutionBase)
|
MOVEIT_CLASS_FORWARD(SolutionBase)
|
||||||
|
|
||||||
|
/** The Introspection class provides publishing of task state and solutions.
|
||||||
|
*
|
||||||
|
* It is interlinked to its task.
|
||||||
|
*/
|
||||||
class Introspection {
|
class Introspection {
|
||||||
// publish task detailed description and current state
|
ros::NodeHandle nh_;
|
||||||
|
/// associated task
|
||||||
|
const Task &task_;
|
||||||
|
|
||||||
|
/// publish task detailed description and current state
|
||||||
ros::Publisher task_description_publisher_;
|
ros::Publisher task_description_publisher_;
|
||||||
ros::Publisher task_statistics_publisher_;
|
ros::Publisher task_statistics_publisher_;
|
||||||
// publish new solutions
|
/// publish new solutions
|
||||||
ros::Publisher solution_publisher_;
|
ros::Publisher solution_publisher_;
|
||||||
// services to provide an individual Solution
|
/// services to provide an individual Solution
|
||||||
ros::ServiceServer get_solution_service_;
|
ros::ServiceServer get_solution_service_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
Introspection();
|
Introspection(const Task &task);
|
||||||
Introspection(const Introspection &other) = delete;
|
Introspection(const Introspection &other) = delete;
|
||||||
static Introspection &instance();
|
|
||||||
|
|
||||||
// publish detailed task description
|
/// publish detailed task description
|
||||||
void publishTaskDescription(const Task &t);
|
void publishTaskDescription();
|
||||||
void publishTaskDescription(const ::moveit_task_constructor::TaskDescription &msg) {
|
|
||||||
task_description_publisher_.publish(msg);
|
|
||||||
}
|
|
||||||
|
|
||||||
// publish the current state of given task
|
/// publish the current state of task
|
||||||
void publishTaskState(const Task &t);
|
void publishTaskState();
|
||||||
void publishTaskState(const ::moveit_task_constructor::TaskStatistics &msg) {
|
|
||||||
task_statistics_publisher_.publish(msg);
|
|
||||||
}
|
|
||||||
|
|
||||||
void operator()(const Task &t) { publishTaskState(t); }
|
/// indicate that this task was reset
|
||||||
|
void reset();
|
||||||
|
|
||||||
// publish the given solution
|
/// publish the given solution
|
||||||
void publishSolution(const SolutionBase &s);
|
void publishSolution(const SolutionBase &s);
|
||||||
void publishSolution(const ::moveit_task_constructor::Solution &msg) {
|
/// operator version for use in stage callbacks
|
||||||
solution_publisher_.publish(msg);
|
|
||||||
}
|
|
||||||
void operator()(const SolutionBase &s) { publishSolution(s); }
|
void operator()(const SolutionBase &s) { publishSolution(s); }
|
||||||
|
|
||||||
// get solution
|
/// publish all top-level solutions of task
|
||||||
|
void publishAllSolutions(bool wait = true);
|
||||||
|
|
||||||
|
/// get solution
|
||||||
bool getSolution(moveit_task_constructor::GetSolution::Request &req,
|
bool getSolution(moveit_task_constructor::GetSolution::Request &req,
|
||||||
moveit_task_constructor::GetSolution::Response &res);
|
moveit_task_constructor::GetSolution::Response &res);
|
||||||
};
|
};
|
||||||
|
|
||||||
void publishAllPlans(const Task &task, bool wait = true);
|
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -5,11 +5,14 @@
|
|||||||
|
|
||||||
#include "container.h"
|
#include "container.h"
|
||||||
|
|
||||||
#include <moveit/macros/class_forward.h>
|
#include <moveit_task_constructor/introspection.h>
|
||||||
|
|
||||||
#include <moveit_task_constructor/TaskDescription.h>
|
#include <moveit_task_constructor/TaskDescription.h>
|
||||||
#include <moveit_task_constructor/TaskStatistics.h>
|
#include <moveit_task_constructor/TaskStatistics.h>
|
||||||
#include <moveit_task_constructor/Solution.h>
|
#include <moveit_task_constructor/Solution.h>
|
||||||
|
|
||||||
|
#include <moveit/macros/class_forward.h>
|
||||||
|
|
||||||
namespace robot_model_loader {
|
namespace robot_model_loader {
|
||||||
MOVEIT_CLASS_FORWARD(RobotModelLoader)
|
MOVEIT_CLASS_FORWARD(RobotModelLoader)
|
||||||
}
|
}
|
||||||
@ -31,16 +34,22 @@ MOVEIT_CLASS_FORWARD(Task)
|
|||||||
* which are provided by the wrappers end_ and start_ and interfaces respectively. */
|
* which are provided by the wrappers end_ and start_ and interfaces respectively. */
|
||||||
class Task : protected WrapperBase {
|
class Task : protected WrapperBase {
|
||||||
public:
|
public:
|
||||||
Task(Stage::pointer &&container = std::make_unique<SerialContainer>("task pipeline"));
|
Task(const std::string& id = "",
|
||||||
|
Stage::pointer &&container = std::make_unique<SerialContainer>("task pipeline"));
|
||||||
static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model,
|
static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model,
|
||||||
const std::string &ns = "move_group",
|
const std::string &ns = "move_group",
|
||||||
const std::string &planning_plugin_param_name = "planning_plugin",
|
const std::string &planning_plugin_param_name = "planning_plugin",
|
||||||
const std::string &adapter_plugins_param_name = "request_adapters");
|
const std::string &adapter_plugins_param_name = "request_adapters");
|
||||||
|
~Task();
|
||||||
|
|
||||||
std::string id() const;
|
std::string id() const;
|
||||||
|
|
||||||
void add(Stage::pointer &&stage);
|
void add(Stage::pointer &&stage);
|
||||||
void clear() override;
|
void clear() override;
|
||||||
|
|
||||||
|
/// enable introspection publishing for use with rviz
|
||||||
|
void enableIntrospection(bool enable = true);
|
||||||
|
|
||||||
typedef std::function<void(const SolutionBase &s)> SolutionCallback;
|
typedef std::function<void(const SolutionBase &s)> SolutionCallback;
|
||||||
typedef std::list<SolutionCallback> SolutionCallbackList;
|
typedef std::list<SolutionCallback> SolutionCallbackList;
|
||||||
/// add function to be called for every newly found solution
|
/// add function to be called for every newly found solution
|
||||||
@ -55,6 +64,9 @@ public:
|
|||||||
/// remove function callback
|
/// remove function callback
|
||||||
void erase(TaskCallbackList::const_iterator which);
|
void erase(TaskCallbackList::const_iterator which);
|
||||||
|
|
||||||
|
void reset() override;
|
||||||
|
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
||||||
|
|
||||||
bool plan();
|
bool plan();
|
||||||
/// print current state std::cout
|
/// print current state std::cout
|
||||||
static void printState(const Task &t);
|
static void printState(const Task &t);
|
||||||
@ -74,12 +86,14 @@ public:
|
|||||||
/// process all solutions
|
/// process all solutions
|
||||||
void processSolutions(const Task::SolutionProcessor &processor) const;
|
void processSolutions(const Task::SolutionProcessor &processor) const;
|
||||||
|
|
||||||
|
/// publish all top-level solutions
|
||||||
|
void publishAllSolutions(bool wait = true);
|
||||||
|
|
||||||
/// access stage tree
|
/// access stage tree
|
||||||
ContainerBase *stages();
|
ContainerBase *stages();
|
||||||
const ContainerBase *stages() const;
|
const ContainerBase *stages() const;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
void reset() override;
|
|
||||||
void initModel();
|
void initModel();
|
||||||
void initScene();
|
void initScene();
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
@ -88,15 +102,18 @@ protected:
|
|||||||
void onNewSolution(SolutionBase &s) override;
|
void onNewSolution(SolutionBase &s) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
size_t id_; // unique task ID
|
std::string id_;
|
||||||
robot_model_loader::RobotModelLoaderPtr rml_;
|
robot_model_loader::RobotModelLoaderPtr rml_;
|
||||||
planning_scene::PlanningSceneConstPtr scene_; // initial scene
|
planning_scene::PlanningSceneConstPtr scene_; // initial scene
|
||||||
std::list<Task::SolutionCallback> solution_cbs_; // functions called for each new solution
|
|
||||||
std::list<Task::TaskCallback> task_cbs_; // functions to monitor task's planning progress
|
|
||||||
|
|
||||||
// use separate interfaces as targets for wrapper's prevEnds() / nextStarts()
|
// use separate interfaces as targets for wrapper's prevEnds() / nextStarts()
|
||||||
InterfacePtr task_starts_;
|
InterfacePtr task_starts_;
|
||||||
InterfacePtr task_ends_;
|
InterfacePtr task_ends_;
|
||||||
|
|
||||||
|
// introspection and monitoring
|
||||||
|
std::unique_ptr<Introspection> introspection_;
|
||||||
|
std::list<Task::SolutionCallback> solution_cbs_; // functions called for each new solution
|
||||||
|
std::list<Task::TaskCallback> task_cbs_; // functions to monitor task's planning progress
|
||||||
};
|
};
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -1,5 +1,4 @@
|
|||||||
#include <moveit_task_constructor/task.h>
|
#include <moveit_task_constructor/task.h>
|
||||||
#include <moveit_task_constructor/introspection.h>
|
|
||||||
|
|
||||||
#include <moveit_task_constructor/stages/current_state.h>
|
#include <moveit_task_constructor/stages/current_state.h>
|
||||||
#include <moveit_task_constructor/stages/gripper.h>
|
#include <moveit_task_constructor/stages/gripper.h>
|
||||||
@ -108,8 +107,7 @@ int main(int argc, char** argv){
|
|||||||
}
|
}
|
||||||
|
|
||||||
t.plan();
|
t.plan();
|
||||||
|
t.publishAllSolutions();
|
||||||
publishAllPlans(t);
|
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,5 +1,4 @@
|
|||||||
#include <moveit_task_constructor/task.h>
|
#include <moveit_task_constructor/task.h>
|
||||||
#include <moveit_task_constructor/introspection.h>
|
|
||||||
|
|
||||||
#include <moveit_task_constructor/stages/current_state.h>
|
#include <moveit_task_constructor/stages/current_state.h>
|
||||||
#include <moveit_task_constructor/stages/gripper.h>
|
#include <moveit_task_constructor/stages/gripper.h>
|
||||||
@ -106,8 +105,7 @@ int main(int argc, char** argv){
|
|||||||
}
|
}
|
||||||
|
|
||||||
t.plan();
|
t.plan();
|
||||||
|
t.publishAllSolutions();
|
||||||
publishAllPlans(t);
|
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -7,40 +7,58 @@
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
Introspection::Introspection()
|
Introspection::Introspection(const Task &task)
|
||||||
|
: nh_(std::string("~/") + task.id()) // topics + services are advertised in private namespace
|
||||||
|
, task_(task)
|
||||||
{
|
{
|
||||||
ros::NodeHandle n;
|
task_description_publisher_ = nh_.advertise<moveit_task_constructor::TaskDescription>(DESCRIPTION_TOPIC, 1, true);
|
||||||
task_description_publisher_ = n.advertise<moveit_task_constructor::TaskDescription>(DESCRIPTION_TOPIC, 1);
|
task_statistics_publisher_ = nh_.advertise<moveit_task_constructor::TaskStatistics>(STATISTICS_TOPIC, 1);
|
||||||
task_statistics_publisher_ = n.advertise<moveit_task_constructor::TaskStatistics>(STATISTICS_TOPIC, 1);
|
solution_publisher_ = nh_.advertise<::moveit_task_constructor::Solution>(SOLUTION_TOPIC, 1, true);
|
||||||
solution_publisher_ = n.advertise<::moveit_task_constructor::Solution>(SOLUTION_TOPIC, 1, true);
|
|
||||||
|
|
||||||
n = ros::NodeHandle("~"); // services are advertised in private namespace
|
get_solution_service_ = nh_.advertiseService("get_solution", &Introspection::getSolution, this);
|
||||||
get_solution_service_ = n.advertiseService("get_solution", &Introspection::getSolution, this);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Introspection &Introspection::instance()
|
void Introspection::publishTaskDescription()
|
||||||
{
|
|
||||||
static Introspection instance_;
|
|
||||||
return instance_;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Introspection::publishTaskDescription(const Task &t)
|
|
||||||
{
|
{
|
||||||
::moveit_task_constructor::TaskDescription msg;
|
::moveit_task_constructor::TaskDescription msg;
|
||||||
task_description_publisher_.publish(t.fillTaskDescription(msg));
|
task_description_publisher_.publish(task_.fillTaskDescription(msg));
|
||||||
}
|
}
|
||||||
|
|
||||||
void Introspection::publishTaskState(const Task &t)
|
void Introspection::publishTaskState()
|
||||||
{
|
{
|
||||||
::moveit_task_constructor::TaskStatistics msg;
|
::moveit_task_constructor::TaskStatistics msg;
|
||||||
task_statistics_publisher_.publish(t.fillTaskStatistics(msg));
|
task_statistics_publisher_.publish(task_.fillTaskStatistics(msg));
|
||||||
|
}
|
||||||
|
|
||||||
|
void Introspection::reset()
|
||||||
|
{
|
||||||
|
::moveit_task_constructor::TaskDescription msg;
|
||||||
|
task_description_publisher_.publish(msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Introspection::publishSolution(const SolutionBase &s)
|
void Introspection::publishSolution(const SolutionBase &s)
|
||||||
{
|
{
|
||||||
::moveit_task_constructor::Solution msg;
|
::moveit_task_constructor::Solution msg;
|
||||||
s.fillMessage(msg);
|
s.fillMessage(msg);
|
||||||
publishSolution(msg);
|
solution_publisher_.publish(msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Introspection::publishAllSolutions(bool wait)
|
||||||
|
{
|
||||||
|
Task::SolutionProcessor processor
|
||||||
|
= [this, wait](const ::moveit_task_constructor::Solution& msg, double cost) {
|
||||||
|
std::cout << "publishing solution with cost: " << cost << std::endl;
|
||||||
|
solution_publisher_.publish(msg);
|
||||||
|
if (wait) {
|
||||||
|
std::cout << "Press <Enter> to continue ..." << std::endl;
|
||||||
|
int ch = getchar();
|
||||||
|
if (ch == 'q' || ch == 'Q')
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
};
|
||||||
|
|
||||||
|
task_.processSolutions(processor);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Introspection::getSolution(moveit_task_constructor::GetSolution::Request &req,
|
bool Introspection::getSolution(moveit_task_constructor::GetSolution::Request &req,
|
||||||
@ -53,21 +71,4 @@ bool Introspection::getSolution(moveit_task_constructor::GetSolution::Request &
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void publishAllPlans(const Task &task, bool wait) {
|
|
||||||
Task::SolutionProcessor processor
|
|
||||||
= [wait](const ::moveit_task_constructor::Solution& msg, double cost) {
|
|
||||||
std::cout << "publishing solution with cost: " << cost << std::endl;
|
|
||||||
Introspection::instance().publishSolution(msg);
|
|
||||||
if (wait) {
|
|
||||||
std::cout << "Press <Enter> to continue ..." << std::endl;
|
|
||||||
int ch = getchar();
|
|
||||||
if (ch == 'q' || ch == 'Q')
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
return true;
|
|
||||||
};
|
|
||||||
|
|
||||||
task.processSolutions(processor);
|
|
||||||
}
|
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
53
src/task.cpp
53
src/task.cpp
@ -15,10 +15,8 @@
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
static size_t g_task_id = 0;
|
Task::Task(const std::string& id, ContainerBase::pointer &&container)
|
||||||
|
: WrapperBase(std::string()), id_(id)
|
||||||
Task::Task(ContainerBase::pointer &&container)
|
|
||||||
: WrapperBase(std::string()), id_(++g_task_id)
|
|
||||||
{
|
{
|
||||||
task_starts_.reset(new Interface(Interface::NotifyFunction()));
|
task_starts_.reset(new Interface(Interface::NotifyFunction()));
|
||||||
task_ends_.reset(new Interface(Interface::NotifyFunction()));
|
task_ends_.reset(new Interface(Interface::NotifyFunction()));
|
||||||
@ -28,10 +26,8 @@ Task::Task(ContainerBase::pointer &&container)
|
|||||||
|
|
||||||
// monitor state on commandline
|
// monitor state on commandline
|
||||||
addTaskCallback(&printState);
|
addTaskCallback(&printState);
|
||||||
// publish state
|
// enable introspection by default
|
||||||
addTaskCallback(std::ref(Introspection::instance()));
|
enableIntrospection(true);
|
||||||
// publish new solutions
|
|
||||||
addSolutionCallback(std::ref(Introspection::instance()));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Task::initModel () {
|
void Task::initModel () {
|
||||||
@ -90,6 +86,11 @@ Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::str
|
|||||||
return planner;
|
return planner;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Task::~Task()
|
||||||
|
{
|
||||||
|
reset();
|
||||||
|
}
|
||||||
|
|
||||||
void Task::add(pointer &&stage) {
|
void Task::add(pointer &&stage) {
|
||||||
if (!stage)
|
if (!stage)
|
||||||
throw std::runtime_error("stage insertion failed: invalid stage pointer");
|
throw std::runtime_error("stage insertion failed: invalid stage pointer");
|
||||||
@ -101,7 +102,14 @@ void Task::add(pointer &&stage) {
|
|||||||
void Task::clear()
|
void Task::clear()
|
||||||
{
|
{
|
||||||
stages()->clear();
|
stages()->clear();
|
||||||
id_ = ++g_task_id;
|
}
|
||||||
|
|
||||||
|
void Task::enableIntrospection(bool enable)
|
||||||
|
{
|
||||||
|
if (enable && !introspection_)
|
||||||
|
introspection_.reset(new Introspection(*this));
|
||||||
|
else if (!enable && introspection_)
|
||||||
|
introspection_.reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
Task::SolutionCallbackList::const_iterator Task::addSolutionCallback(SolutionCallback &&cb)
|
Task::SolutionCallbackList::const_iterator Task::addSolutionCallback(SolutionCallback &&cb)
|
||||||
@ -128,6 +136,10 @@ void Task::erase(TaskCallbackList::const_iterator which)
|
|||||||
|
|
||||||
void Task::reset()
|
void Task::reset()
|
||||||
{
|
{
|
||||||
|
// signal introspection, that this task was reset
|
||||||
|
if (introspection_)
|
||||||
|
introspection_->reset();
|
||||||
|
|
||||||
task_starts_->clear();
|
task_starts_->clear();
|
||||||
task_ends_->clear();
|
task_ends_->clear();
|
||||||
WrapperBase::reset();
|
WrapperBase::reset();
|
||||||
@ -138,6 +150,13 @@ void Task::reset()
|
|||||||
impl->setNextStarts(task_starts_);
|
impl->setNextStarts(task_starts_);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
|
{
|
||||||
|
WrapperBase::init(scene);
|
||||||
|
if (introspection_)
|
||||||
|
introspection_->publishTaskDescription();
|
||||||
|
}
|
||||||
|
|
||||||
bool Task::canCompute() const
|
bool Task::canCompute() const
|
||||||
{
|
{
|
||||||
return stages()->canCompute();
|
return stages()->canCompute();
|
||||||
@ -148,7 +167,8 @@ bool Task::compute()
|
|||||||
return stages()->compute();
|
return stages()->compute();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Task::plan(){
|
bool Task::plan()
|
||||||
|
{
|
||||||
reset();
|
reset();
|
||||||
initScene();
|
initScene();
|
||||||
init(scene_);
|
init(scene_);
|
||||||
@ -157,6 +177,8 @@ bool Task::plan(){
|
|||||||
if (compute()) {
|
if (compute()) {
|
||||||
for (const auto& cb : task_cbs_)
|
for (const auto& cb : task_cbs_)
|
||||||
cb(*this);
|
cb(*this);
|
||||||
|
if (introspection_)
|
||||||
|
introspection_->publishTaskState();
|
||||||
} else
|
} else
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@ -186,10 +208,18 @@ void Task::processSolutions(const Task::SolutionProcessor& processor) const {
|
|||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Task::publishAllSolutions(bool wait)
|
||||||
|
{
|
||||||
|
enableIntrospection(true);
|
||||||
|
introspection_->publishAllSolutions(wait);
|
||||||
|
}
|
||||||
|
|
||||||
void Task::onNewSolution(SolutionBase &s)
|
void Task::onNewSolution(SolutionBase &s)
|
||||||
{
|
{
|
||||||
for (const auto& cb : solution_cbs_)
|
for (const auto& cb : solution_cbs_)
|
||||||
cb(s);
|
cb(s);
|
||||||
|
if (introspection_)
|
||||||
|
introspection_->publishSolution(s);
|
||||||
}
|
}
|
||||||
|
|
||||||
inline ContainerBase* Task::stages()
|
inline ContainerBase* Task::stages()
|
||||||
@ -204,8 +234,7 @@ inline const ContainerBase* Task::stages() const
|
|||||||
|
|
||||||
std::string Task::id() const
|
std::string Task::id() const
|
||||||
{
|
{
|
||||||
ros::NodeHandle n("~");
|
return id_;
|
||||||
return std::to_string(id_) + '@' + n.getNamespace();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Task::printState(const Task &t){
|
void Task::printState(const Task &t){
|
||||||
|
|||||||
@ -125,7 +125,7 @@ protected:
|
|||||||
SCOPED_TRACE("first i=" + std::to_string(i));
|
SCOPED_TRACE("first i=" + std::to_string(i));
|
||||||
num_inserts = 0;
|
num_inserts = 0;
|
||||||
num_updates = 0;
|
num_updates = 0;
|
||||||
model.processTaskDescriptionMessage(genMsg("first"));
|
model.processTaskDescriptionMessage("1", genMsg("first"));
|
||||||
|
|
||||||
if (i == 0) EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task
|
if (i == 0) EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task
|
||||||
else EXPECT_EQ(num_inserts, 0);
|
else EXPECT_EQ(num_inserts, 0);
|
||||||
@ -137,7 +137,7 @@ protected:
|
|||||||
SCOPED_TRACE("second i=" + std::to_string(i));
|
SCOPED_TRACE("second i=" + std::to_string(i));
|
||||||
num_inserts = 0;
|
num_inserts = 0;
|
||||||
num_updates = 0;
|
num_updates = 0;
|
||||||
model.processTaskDescriptionMessage(genMsg("second")); // 1 notify for inserted task
|
model.processTaskDescriptionMessage("2", genMsg("second")); // 1 notify for inserted task
|
||||||
|
|
||||||
if (i == 0) EXPECT_EQ(num_inserts, 1);
|
if (i == 0) EXPECT_EQ(num_inserts, 1);
|
||||||
else EXPECT_EQ(num_inserts, 0);
|
else EXPECT_EQ(num_inserts, 0);
|
||||||
@ -190,13 +190,13 @@ TEST_F(TaskListModelTest, threeChildren) {
|
|||||||
TEST_F(TaskListModelTest, visitedPopulate) {
|
TEST_F(TaskListModelTest, visitedPopulate) {
|
||||||
// first population without children
|
// first population without children
|
||||||
children = 0;
|
children = 0;
|
||||||
model.processTaskDescriptionMessage(genMsg("first"));
|
model.processTaskDescriptionMessage("1", genMsg("first"));
|
||||||
validate(model, {"first"}); // validation visits root node
|
validate(model, {"first"}); // validation visits root node
|
||||||
EXPECT_EQ(num_inserts, 1);
|
EXPECT_EQ(num_inserts, 1);
|
||||||
|
|
||||||
children = 3;
|
children = 3;
|
||||||
num_inserts = 0;
|
num_inserts = 0;
|
||||||
model.processTaskDescriptionMessage(genMsg("first"));
|
model.processTaskDescriptionMessage("1", genMsg("first"));
|
||||||
validate(model, {"first"});
|
validate(model, {"first"});
|
||||||
// second population with children should emit insert notifies for them
|
// second population with children should emit insert notifies for them
|
||||||
EXPECT_EQ(num_inserts, 3);
|
EXPECT_EQ(num_inserts, 3);
|
||||||
|
|||||||
@ -141,43 +141,58 @@ void TaskDisplay::changedRobotDescription()
|
|||||||
loadRobotModel();
|
loadRobotModel();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void TaskDisplay::taskDescriptionCB(const ros::MessageEvent<const moveit_task_constructor::TaskDescription> &event)
|
||||||
|
{
|
||||||
|
const moveit_task_constructor::TaskDescriptionConstPtr& msg = event.getMessage();
|
||||||
|
const std::string id = event.getPublisherName() + "/" + msg->id;
|
||||||
|
mainloop_jobs_.addJob([this, id, msg]() {
|
||||||
|
task_model_->processTaskDescriptionMessage(id, *msg);
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
void TaskDisplay::taskStatisticsCB(const ros::MessageEvent<const moveit_task_constructor::TaskStatistics> &event)
|
||||||
|
{
|
||||||
|
const moveit_task_constructor::TaskStatisticsConstPtr& msg = event.getMessage();
|
||||||
|
const std::string id = event.getPublisherName() + "/" + msg->id;
|
||||||
|
mainloop_jobs_.addJob([this, id, msg]() {
|
||||||
|
task_model_->processTaskStatisticsMessage(id, *msg);
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
void TaskDisplay::taskSolutionCB(const ros::MessageEvent<const moveit_task_constructor::Solution> &event)
|
||||||
|
{
|
||||||
|
const moveit_task_constructor::SolutionConstPtr& msg = event.getMessage();
|
||||||
|
const std::string id = event.getPublisherName() + "/" + msg->task_id;
|
||||||
|
mainloop_jobs_.addJob([this, id, msg]() {
|
||||||
|
if (task_model_) task_model_->processSolutionMessage(id, *msg);
|
||||||
|
// TODO: use already processed trajectory (e.g. by ID)
|
||||||
|
trajectory_visual_->showTrajectory(*msg);
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
void TaskDisplay::updateTaskListModel()
|
void TaskDisplay::updateTaskListModel()
|
||||||
{
|
{
|
||||||
// generate task monitoring topics from solution topic
|
// generate task monitoring topics from solution topic
|
||||||
std::string solution_topic = task_solution_topic_property_->getStdString();
|
std::string solution_topic = task_solution_topic_property_->getStdString();
|
||||||
auto lastSep = solution_topic.find_last_of('/');
|
auto last_sep = solution_topic.find_last_of('/');
|
||||||
std::string base_ns = solution_topic.substr(0, lastSep);
|
if (last_sep == std::string::npos)
|
||||||
|
return;
|
||||||
|
|
||||||
|
std::string base_ns = solution_topic.substr(0, last_sep+1);
|
||||||
task_model_ = TaskListModelCache::instance().getModel(base_ns);
|
task_model_ = TaskListModelCache::instance().getModel(base_ns);
|
||||||
|
|
||||||
if (task_model_) {
|
if (task_model_) {
|
||||||
// listen to task descriptions updates
|
// listen to task descriptions updates
|
||||||
boost::function<void(const moveit_task_constructor::TaskDescriptionConstPtr &)> taskDescCB
|
task_description_sub = update_nh_.subscribe(base_ns + DESCRIPTION_TOPIC, 2, &TaskDisplay::taskDescriptionCB, this);
|
||||||
([this](const moveit_task_constructor::TaskDescriptionConstPtr &msg){
|
|
||||||
mainloop_jobs_.addJob([this, msg]() { task_model_->processTaskDescriptionMessage(*msg); });
|
|
||||||
});
|
|
||||||
task_description_sub = update_nh_.subscribe(base_ns + DESCRIPTION_TOPIC, 2, taskDescCB);
|
|
||||||
|
|
||||||
// listen to task statistics updates
|
// listen to task statistics updates
|
||||||
boost::function<void(const moveit_task_constructor::TaskDescriptionConstPtr &)> taskStatCB
|
task_statistics_sub = update_nh_.subscribe(base_ns + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this);
|
||||||
([this](const moveit_task_constructor::TaskDescriptionConstPtr &msg){
|
|
||||||
mainloop_jobs_.addJob([this, msg]() { task_model_->processTaskDescriptionMessage(*msg); });
|
|
||||||
});
|
|
||||||
task_description_sub = update_nh_.subscribe(base_ns + STATISTICS_TOPIC, 2, taskStatCB);
|
|
||||||
} else {
|
} else {
|
||||||
setStatus(rviz::StatusProperty::Error, "Task Monitor", "failed to create TaskListModel");
|
setStatus(rviz::StatusProperty::Error, "Task Monitor", "failed to create TaskListModel");
|
||||||
}
|
}
|
||||||
|
|
||||||
// listen to task solutions
|
// listen to task solutions
|
||||||
boost::function<void(const moveit_task_constructor::SolutionConstPtr &)> solCB
|
task_solution_sub = update_nh_.subscribe(solution_topic, 2, &TaskDisplay::taskSolutionCB, this);
|
||||||
([this](const moveit_task_constructor::SolutionConstPtr &msg){
|
|
||||||
mainloop_jobs_.addJob([this, msg]() {
|
|
||||||
if (task_model_) task_model_->processSolutionMessage(*msg);
|
|
||||||
// TODO: use already processed trajectory (e.g. by ID)
|
|
||||||
trajectory_visual_->showTrajectory(*msg);
|
|
||||||
});
|
|
||||||
});
|
|
||||||
task_solution_sub = update_nh_.subscribe(solution_topic, 2, solCB);
|
|
||||||
|
|
||||||
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "Connected");
|
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "Connected");
|
||||||
}
|
}
|
||||||
|
|||||||
@ -44,6 +44,9 @@
|
|||||||
#include "job_queue.h"
|
#include "job_queue.h"
|
||||||
#include <moveit/macros/class_forward.h>
|
#include <moveit/macros/class_forward.h>
|
||||||
#include <ros/subscriber.h>
|
#include <ros/subscriber.h>
|
||||||
|
#include <moveit_task_constructor/TaskDescription.h>
|
||||||
|
#include <moveit_task_constructor/TaskStatistics.h>
|
||||||
|
#include <moveit_task_constructor/Solution.h>
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
namespace rviz
|
namespace rviz
|
||||||
@ -86,6 +89,10 @@ private Q_SLOTS:
|
|||||||
void changedRobotDescription();
|
void changedRobotDescription();
|
||||||
void changedTaskSolutionTopic();
|
void changedTaskSolutionTopic();
|
||||||
|
|
||||||
|
void taskDescriptionCB(const ros::MessageEvent<moveit_task_constructor::TaskDescription const>& event);
|
||||||
|
void taskStatisticsCB(const ros::MessageEvent<const moveit_task_constructor::TaskStatistics> &event);
|
||||||
|
void taskSolutionCB(const ros::MessageEvent<moveit_task_constructor::Solution const>& event);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
void updateTaskListModel();
|
void updateTaskListModel();
|
||||||
|
|
||||||
|
|||||||
@ -256,12 +256,13 @@ bool TaskListModel::removeRows(int row, int count, const QModelIndex &parent)
|
|||||||
|
|
||||||
// process a task description message:
|
// process a task description message:
|
||||||
// update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one
|
// update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one
|
||||||
void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor::TaskDescription &msg)
|
void TaskListModel::processTaskDescriptionMessage(const std::string& id,
|
||||||
|
const moveit_task_constructor::TaskDescription &msg)
|
||||||
{
|
{
|
||||||
Q_D(TaskListModel);
|
Q_D(TaskListModel);
|
||||||
|
|
||||||
// retrieve existing or insert new remote task for given id
|
// retrieve existing or insert new remote task for given id
|
||||||
auto it_inserted = d->remote_tasks_.insert(std::make_pair(msg.id, nullptr));
|
auto it_inserted = d->remote_tasks_.insert(std::make_pair(id, nullptr));
|
||||||
bool created = it_inserted.second;
|
bool created = it_inserted.second;
|
||||||
RemoteTaskModel*& remote_task = it_inserted.first->second;
|
RemoteTaskModel*& remote_task = it_inserted.first->second;
|
||||||
|
|
||||||
@ -297,11 +298,12 @@ void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor:
|
|||||||
}
|
}
|
||||||
|
|
||||||
// process a task statistics message
|
// process a task statistics message
|
||||||
void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor::TaskStatistics &msg)
|
void TaskListModel::processTaskStatisticsMessage(const std::string &id,
|
||||||
|
const moveit_task_constructor::TaskStatistics &msg)
|
||||||
{
|
{
|
||||||
Q_D(TaskListModel);
|
Q_D(TaskListModel);
|
||||||
|
|
||||||
auto it = d->remote_tasks_.find(msg.id);
|
auto it = d->remote_tasks_.find(id);
|
||||||
if (it == d->remote_tasks_.cend())
|
if (it == d->remote_tasks_.cend())
|
||||||
return; // unkown task
|
return; // unkown task
|
||||||
|
|
||||||
@ -312,7 +314,8 @@ void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor::
|
|||||||
remote_task->processStageStatistics(msg.stages);
|
remote_task->processStageStatistics(msg.stages);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskListModel::processSolutionMessage(const moveit_task_constructor::Solution &msg)
|
void TaskListModel::processSolutionMessage(const std::string &id,
|
||||||
|
const moveit_task_constructor::Solution &msg)
|
||||||
{
|
{
|
||||||
// TODO
|
// TODO
|
||||||
}
|
}
|
||||||
|
|||||||
@ -91,11 +91,11 @@ public:
|
|||||||
bool removeRows(int row, int count, const QModelIndex &parent) override;
|
bool removeRows(int row, int count, const QModelIndex &parent) override;
|
||||||
|
|
||||||
/// process an incoming task description message - only call in Qt's main loop
|
/// process an incoming task description message - only call in Qt's main loop
|
||||||
void processTaskDescriptionMessage(const moveit_task_constructor::TaskDescription &msg);
|
void processTaskDescriptionMessage(const std::__cxx11::string &id, const moveit_task_constructor::TaskDescription &msg);
|
||||||
/// process an incoming task description message - only call in Qt's main loop
|
/// process an incoming task description message - only call in Qt's main loop
|
||||||
void processTaskStatisticsMessage(const moveit_task_constructor::TaskStatistics &msg);
|
void processTaskStatisticsMessage(const std::__cxx11::string &id, const moveit_task_constructor::TaskStatistics &msg);
|
||||||
/// process an incoming solution message - only call in Qt's main loop
|
/// process an incoming solution message - only call in Qt's main loop
|
||||||
void processSolutionMessage(const moveit_task_constructor::Solution &msg);
|
void processSolutionMessage(const std::__cxx11::string &id, const moveit_task_constructor::Solution &msg);
|
||||||
|
|
||||||
/// retrieve TaskModel in given row
|
/// retrieve TaskModel in given row
|
||||||
BaseTaskModel* getTask(int row) const;
|
BaseTaskModel* getTask(int row) const;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user