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:
Robert Haschke 2017-11-09 12:38:29 +01:00
parent af2009c19b
commit cb85e1b864
11 changed files with 184 additions and 115 deletions

View File

@ -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);
} } } }

View File

@ -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
}; };
} } } }

View File

@ -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;
} }

View File

@ -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;
} }

View File

@ -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);
}
} } } }

View File

@ -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){

View File

@ -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);

View File

@ -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");
} }

View File

@ -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();

View File

@ -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
} }

View File

@ -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;