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(SolutionBase)
|
||||
|
||||
/** The Introspection class provides publishing of task state and solutions.
|
||||
*
|
||||
* It is interlinked to its task.
|
||||
*/
|
||||
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_statistics_publisher_;
|
||||
// publish new solutions
|
||||
/// publish new solutions
|
||||
ros::Publisher solution_publisher_;
|
||||
// services to provide an individual Solution
|
||||
/// services to provide an individual Solution
|
||||
ros::ServiceServer get_solution_service_;
|
||||
|
||||
public:
|
||||
Introspection();
|
||||
Introspection(const Task &task);
|
||||
Introspection(const Introspection &other) = delete;
|
||||
static Introspection &instance();
|
||||
|
||||
// publish detailed task description
|
||||
void publishTaskDescription(const Task &t);
|
||||
void publishTaskDescription(const ::moveit_task_constructor::TaskDescription &msg) {
|
||||
task_description_publisher_.publish(msg);
|
||||
}
|
||||
/// publish detailed task description
|
||||
void publishTaskDescription();
|
||||
|
||||
// publish the current state of given task
|
||||
void publishTaskState(const Task &t);
|
||||
void publishTaskState(const ::moveit_task_constructor::TaskStatistics &msg) {
|
||||
task_statistics_publisher_.publish(msg);
|
||||
}
|
||||
/// publish the current state of task
|
||||
void publishTaskState();
|
||||
|
||||
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 ::moveit_task_constructor::Solution &msg) {
|
||||
solution_publisher_.publish(msg);
|
||||
}
|
||||
/// operator version for use in stage callbacks
|
||||
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,
|
||||
moveit_task_constructor::GetSolution::Response &res);
|
||||
};
|
||||
|
||||
void publishAllPlans(const Task &task, bool wait = true);
|
||||
|
||||
} }
|
||||
|
||||
@ -5,11 +5,14 @@
|
||||
|
||||
#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/TaskStatistics.h>
|
||||
#include <moveit_task_constructor/Solution.h>
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
|
||||
namespace robot_model_loader {
|
||||
MOVEIT_CLASS_FORWARD(RobotModelLoader)
|
||||
}
|
||||
@ -31,16 +34,22 @@ MOVEIT_CLASS_FORWARD(Task)
|
||||
* which are provided by the wrappers end_ and start_ and interfaces respectively. */
|
||||
class Task : protected WrapperBase {
|
||||
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,
|
||||
const std::string &ns = "move_group",
|
||||
const std::string &planning_plugin_param_name = "planning_plugin",
|
||||
const std::string &adapter_plugins_param_name = "request_adapters");
|
||||
~Task();
|
||||
|
||||
std::string id() const;
|
||||
|
||||
void add(Stage::pointer &&stage);
|
||||
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::list<SolutionCallback> SolutionCallbackList;
|
||||
/// add function to be called for every newly found solution
|
||||
@ -55,6 +64,9 @@ public:
|
||||
/// remove function callback
|
||||
void erase(TaskCallbackList::const_iterator which);
|
||||
|
||||
void reset() override;
|
||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
||||
|
||||
bool plan();
|
||||
/// print current state std::cout
|
||||
static void printState(const Task &t);
|
||||
@ -74,12 +86,14 @@ public:
|
||||
/// process all solutions
|
||||
void processSolutions(const Task::SolutionProcessor &processor) const;
|
||||
|
||||
/// publish all top-level solutions
|
||||
void publishAllSolutions(bool wait = true);
|
||||
|
||||
/// access stage tree
|
||||
ContainerBase *stages();
|
||||
const ContainerBase *stages() const;
|
||||
|
||||
protected:
|
||||
void reset() override;
|
||||
void initModel();
|
||||
void initScene();
|
||||
bool canCompute() const override;
|
||||
@ -88,15 +102,18 @@ protected:
|
||||
void onNewSolution(SolutionBase &s) override;
|
||||
|
||||
private:
|
||||
size_t id_; // unique task ID
|
||||
std::string id_;
|
||||
robot_model_loader::RobotModelLoaderPtr rml_;
|
||||
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()
|
||||
InterfacePtr task_starts_;
|
||||
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/introspection.h>
|
||||
|
||||
#include <moveit_task_constructor/stages/current_state.h>
|
||||
#include <moveit_task_constructor/stages/gripper.h>
|
||||
@ -108,8 +107,7 @@ int main(int argc, char** argv){
|
||||
}
|
||||
|
||||
t.plan();
|
||||
|
||||
publishAllPlans(t);
|
||||
t.publishAllSolutions();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -1,5 +1,4 @@
|
||||
#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/gripper.h>
|
||||
@ -106,8 +105,7 @@ int main(int argc, char** argv){
|
||||
}
|
||||
|
||||
t.plan();
|
||||
|
||||
publishAllPlans(t);
|
||||
t.publishAllSolutions();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -7,40 +7,58 @@
|
||||
|
||||
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_ = n.advertise<moveit_task_constructor::TaskDescription>(DESCRIPTION_TOPIC, 1);
|
||||
task_statistics_publisher_ = n.advertise<moveit_task_constructor::TaskStatistics>(STATISTICS_TOPIC, 1);
|
||||
solution_publisher_ = n.advertise<::moveit_task_constructor::Solution>(SOLUTION_TOPIC, 1, true);
|
||||
task_description_publisher_ = nh_.advertise<moveit_task_constructor::TaskDescription>(DESCRIPTION_TOPIC, 1, true);
|
||||
task_statistics_publisher_ = nh_.advertise<moveit_task_constructor::TaskStatistics>(STATISTICS_TOPIC, 1);
|
||||
solution_publisher_ = nh_.advertise<::moveit_task_constructor::Solution>(SOLUTION_TOPIC, 1, true);
|
||||
|
||||
n = ros::NodeHandle("~"); // services are advertised in private namespace
|
||||
get_solution_service_ = n.advertiseService("get_solution", &Introspection::getSolution, this);
|
||||
get_solution_service_ = nh_.advertiseService("get_solution", &Introspection::getSolution, this);
|
||||
}
|
||||
|
||||
Introspection &Introspection::instance()
|
||||
{
|
||||
static Introspection instance_;
|
||||
return instance_;
|
||||
}
|
||||
|
||||
void Introspection::publishTaskDescription(const Task &t)
|
||||
void Introspection::publishTaskDescription()
|
||||
{
|
||||
::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;
|
||||
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)
|
||||
{
|
||||
::moveit_task_constructor::Solution 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,
|
||||
@ -53,21 +71,4 @@ bool Introspection::getSolution(moveit_task_constructor::GetSolution::Request &
|
||||
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 {
|
||||
|
||||
static size_t g_task_id = 0;
|
||||
|
||||
Task::Task(ContainerBase::pointer &&container)
|
||||
: WrapperBase(std::string()), id_(++g_task_id)
|
||||
Task::Task(const std::string& id, ContainerBase::pointer &&container)
|
||||
: WrapperBase(std::string()), id_(id)
|
||||
{
|
||||
task_starts_.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
|
||||
addTaskCallback(&printState);
|
||||
// publish state
|
||||
addTaskCallback(std::ref(Introspection::instance()));
|
||||
// publish new solutions
|
||||
addSolutionCallback(std::ref(Introspection::instance()));
|
||||
// enable introspection by default
|
||||
enableIntrospection(true);
|
||||
}
|
||||
|
||||
void Task::initModel () {
|
||||
@ -90,6 +86,11 @@ Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::str
|
||||
return planner;
|
||||
}
|
||||
|
||||
Task::~Task()
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
void Task::add(pointer &&stage) {
|
||||
if (!stage)
|
||||
throw std::runtime_error("stage insertion failed: invalid stage pointer");
|
||||
@ -101,7 +102,14 @@ void Task::add(pointer &&stage) {
|
||||
void Task::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)
|
||||
@ -128,6 +136,10 @@ void Task::erase(TaskCallbackList::const_iterator which)
|
||||
|
||||
void Task::reset()
|
||||
{
|
||||
// signal introspection, that this task was reset
|
||||
if (introspection_)
|
||||
introspection_->reset();
|
||||
|
||||
task_starts_->clear();
|
||||
task_ends_->clear();
|
||||
WrapperBase::reset();
|
||||
@ -138,6 +150,13 @@ void Task::reset()
|
||||
impl->setNextStarts(task_starts_);
|
||||
}
|
||||
|
||||
void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
{
|
||||
WrapperBase::init(scene);
|
||||
if (introspection_)
|
||||
introspection_->publishTaskDescription();
|
||||
}
|
||||
|
||||
bool Task::canCompute() const
|
||||
{
|
||||
return stages()->canCompute();
|
||||
@ -148,7 +167,8 @@ bool Task::compute()
|
||||
return stages()->compute();
|
||||
}
|
||||
|
||||
bool Task::plan(){
|
||||
bool Task::plan()
|
||||
{
|
||||
reset();
|
||||
initScene();
|
||||
init(scene_);
|
||||
@ -157,6 +177,8 @@ bool Task::plan(){
|
||||
if (compute()) {
|
||||
for (const auto& cb : task_cbs_)
|
||||
cb(*this);
|
||||
if (introspection_)
|
||||
introspection_->publishTaskState();
|
||||
} else
|
||||
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)
|
||||
{
|
||||
for (const auto& cb : solution_cbs_)
|
||||
cb(s);
|
||||
if (introspection_)
|
||||
introspection_->publishSolution(s);
|
||||
}
|
||||
|
||||
inline ContainerBase* Task::stages()
|
||||
@ -204,8 +234,7 @@ inline const ContainerBase* Task::stages() const
|
||||
|
||||
std::string Task::id() const
|
||||
{
|
||||
ros::NodeHandle n("~");
|
||||
return std::to_string(id_) + '@' + n.getNamespace();
|
||||
return id_;
|
||||
}
|
||||
|
||||
void Task::printState(const Task &t){
|
||||
|
||||
@ -125,7 +125,7 @@ protected:
|
||||
SCOPED_TRACE("first i=" + std::to_string(i));
|
||||
num_inserts = 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
|
||||
else EXPECT_EQ(num_inserts, 0);
|
||||
@ -137,7 +137,7 @@ protected:
|
||||
SCOPED_TRACE("second i=" + std::to_string(i));
|
||||
num_inserts = 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);
|
||||
else EXPECT_EQ(num_inserts, 0);
|
||||
@ -190,13 +190,13 @@ TEST_F(TaskListModelTest, threeChildren) {
|
||||
TEST_F(TaskListModelTest, visitedPopulate) {
|
||||
// first population without children
|
||||
children = 0;
|
||||
model.processTaskDescriptionMessage(genMsg("first"));
|
||||
model.processTaskDescriptionMessage("1", genMsg("first"));
|
||||
validate(model, {"first"}); // validation visits root node
|
||||
EXPECT_EQ(num_inserts, 1);
|
||||
|
||||
children = 3;
|
||||
num_inserts = 0;
|
||||
model.processTaskDescriptionMessage(genMsg("first"));
|
||||
model.processTaskDescriptionMessage("1", genMsg("first"));
|
||||
validate(model, {"first"});
|
||||
// second population with children should emit insert notifies for them
|
||||
EXPECT_EQ(num_inserts, 3);
|
||||
|
||||
@ -141,43 +141,58 @@ void TaskDisplay::changedRobotDescription()
|
||||
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()
|
||||
{
|
||||
// generate task monitoring topics from solution topic
|
||||
std::string solution_topic = task_solution_topic_property_->getStdString();
|
||||
auto lastSep = solution_topic.find_last_of('/');
|
||||
std::string base_ns = solution_topic.substr(0, lastSep);
|
||||
auto last_sep = solution_topic.find_last_of('/');
|
||||
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);
|
||||
|
||||
if (task_model_) {
|
||||
// listen to task descriptions updates
|
||||
boost::function<void(const moveit_task_constructor::TaskDescriptionConstPtr &)> taskDescCB
|
||||
([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);
|
||||
task_description_sub = update_nh_.subscribe(base_ns + DESCRIPTION_TOPIC, 2, &TaskDisplay::taskDescriptionCB, this);
|
||||
|
||||
// listen to task statistics updates
|
||||
boost::function<void(const moveit_task_constructor::TaskDescriptionConstPtr &)> taskStatCB
|
||||
([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);
|
||||
task_statistics_sub = update_nh_.subscribe(base_ns + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this);
|
||||
} else {
|
||||
setStatus(rviz::StatusProperty::Error, "Task Monitor", "failed to create TaskListModel");
|
||||
}
|
||||
|
||||
// listen to task solutions
|
||||
boost::function<void(const moveit_task_constructor::SolutionConstPtr &)> solCB
|
||||
([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);
|
||||
task_solution_sub = update_nh_.subscribe(solution_topic, 2, &TaskDisplay::taskSolutionCB, this);
|
||||
|
||||
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "Connected");
|
||||
}
|
||||
|
||||
@ -44,6 +44,9 @@
|
||||
#include "job_queue.h"
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <ros/subscriber.h>
|
||||
#include <moveit_task_constructor/TaskDescription.h>
|
||||
#include <moveit_task_constructor/TaskStatistics.h>
|
||||
#include <moveit_task_constructor/Solution.h>
|
||||
#endif
|
||||
|
||||
namespace rviz
|
||||
@ -86,6 +89,10 @@ private Q_SLOTS:
|
||||
void changedRobotDescription();
|
||||
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:
|
||||
void updateTaskListModel();
|
||||
|
||||
|
||||
@ -256,12 +256,13 @@ bool TaskListModel::removeRows(int row, int count, const QModelIndex &parent)
|
||||
|
||||
// process a task description message:
|
||||
// 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);
|
||||
|
||||
// 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;
|
||||
RemoteTaskModel*& remote_task = it_inserted.first->second;
|
||||
|
||||
@ -297,11 +298,12 @@ void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor:
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
auto it = d->remote_tasks_.find(msg.id);
|
||||
auto it = d->remote_tasks_.find(id);
|
||||
if (it == d->remote_tasks_.cend())
|
||||
return; // unkown task
|
||||
|
||||
@ -312,7 +314,8 @@ void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor::
|
||||
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
|
||||
}
|
||||
|
||||
@ -91,11 +91,11 @@ public:
|
||||
bool removeRows(int row, int count, const QModelIndex &parent) override;
|
||||
|
||||
/// 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
|
||||
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
|
||||
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
|
||||
BaseTaskModel* getTask(int row) const;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user