From cb85e1b864f3694a2716cb26f1e53d81931893ba Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Nov 2017 12:38:29 +0100 Subject: [PATCH] publish task info in private namespace: ~/taskid - default task id is empty - introspection becomes member of task, created with Task::enableIntrospection(true) --- .../moveit_task_constructor/introspection.h | 47 ++++++------ include/moveit_task_constructor/task.h | 29 ++++++-- src/demo/plan_pick_trixi.cpp | 4 +- src/demo/plan_pick_ur5.cpp | 4 +- src/introspection.cpp | 71 ++++++++++--------- src/task.cpp | 53 ++++++++++---- src/test/test_task_model.cpp | 8 +-- .../motion_planning_tasks/task_display.cpp | 57 +++++++++------ .../motion_planning_tasks/task_display.h | 7 ++ .../motion_planning_tasks/task_list_model.cpp | 13 ++-- .../motion_planning_tasks/task_list_model.h | 6 +- 11 files changed, 184 insertions(+), 115 deletions(-) diff --git a/include/moveit_task_constructor/introspection.h b/include/moveit_task_constructor/introspection.h index cd05e4c4..972d76d1 100644 --- a/include/moveit_task_constructor/introspection.h +++ b/include/moveit_task_constructor/introspection.h @@ -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); - } } diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 02dcfe39..8e52a982 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -5,11 +5,14 @@ #include "container.h" -#include +#include + #include #include #include +#include + 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("task pipeline")); + Task(const std::string& id = "", + Stage::pointer &&container = std::make_unique("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 SolutionCallback; typedef std::list 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 solution_cbs_; // functions called for each new solution - std::list 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_; + std::list solution_cbs_; // functions called for each new solution + std::list task_cbs_; // functions to monitor task's planning progress }; } } diff --git a/src/demo/plan_pick_trixi.cpp b/src/demo/plan_pick_trixi.cpp index e6ad107f..28fef053 100644 --- a/src/demo/plan_pick_trixi.cpp +++ b/src/demo/plan_pick_trixi.cpp @@ -1,5 +1,4 @@ #include -#include #include #include @@ -108,8 +107,7 @@ int main(int argc, char** argv){ } t.plan(); - - publishAllPlans(t); + t.publishAllSolutions(); return 0; } diff --git a/src/demo/plan_pick_ur5.cpp b/src/demo/plan_pick_ur5.cpp index 8ca5dd6f..83bedf4a 100644 --- a/src/demo/plan_pick_ur5.cpp +++ b/src/demo/plan_pick_ur5.cpp @@ -1,5 +1,4 @@ #include -#include #include #include @@ -106,8 +105,7 @@ int main(int argc, char** argv){ } t.plan(); - - publishAllPlans(t); + t.publishAllSolutions(); return 0; } diff --git a/src/introspection.cpp b/src/introspection.cpp index 594b3ded..83058188 100644 --- a/src/introspection.cpp +++ b/src/introspection.cpp @@ -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(DESCRIPTION_TOPIC, 1); - task_statistics_publisher_ = n.advertise(STATISTICS_TOPIC, 1); - solution_publisher_ = n.advertise<::moveit_task_constructor::Solution>(SOLUTION_TOPIC, 1, true); + task_description_publisher_ = nh_.advertise(DESCRIPTION_TOPIC, 1, true); + task_statistics_publisher_ = nh_.advertise(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 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 to continue ..." << std::endl; - int ch = getchar(); - if (ch == 'q' || ch == 'Q') - return false; - } - return true; - }; - - task.processSolutions(processor); -} - } } diff --git a/src/task.cpp b/src/task.cpp index ab831c7d..6a485af9 100644 --- a/src/task.cpp +++ b/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){ diff --git a/src/test/test_task_model.cpp b/src/test/test_task_model.cpp index d42059b8..41e6e3b3 100644 --- a/src/test/test_task_model.cpp +++ b/src/test/test_task_model.cpp @@ -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); diff --git a/visualization/motion_planning_tasks/task_display.cpp b/visualization/motion_planning_tasks/task_display.cpp index b71fa9a6..1a37d174 100644 --- a/visualization/motion_planning_tasks/task_display.cpp +++ b/visualization/motion_planning_tasks/task_display.cpp @@ -141,43 +141,58 @@ void TaskDisplay::changedRobotDescription() loadRobotModel(); } +void TaskDisplay::taskDescriptionCB(const ros::MessageEvent &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 &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 &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 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 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 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"); } diff --git a/visualization/motion_planning_tasks/task_display.h b/visualization/motion_planning_tasks/task_display.h index 9c08f6f8..1319033b 100644 --- a/visualization/motion_planning_tasks/task_display.h +++ b/visualization/motion_planning_tasks/task_display.h @@ -44,6 +44,9 @@ #include "job_queue.h" #include #include +#include +#include +#include #endif namespace rviz @@ -86,6 +89,10 @@ private Q_SLOTS: void changedRobotDescription(); void changedTaskSolutionTopic(); + void taskDescriptionCB(const ros::MessageEvent& event); + void taskStatisticsCB(const ros::MessageEvent &event); + void taskSolutionCB(const ros::MessageEvent& event); + protected: void updateTaskListModel(); diff --git a/visualization/motion_planning_tasks/task_list_model.cpp b/visualization/motion_planning_tasks/task_list_model.cpp index 245dba8b..07111f34 100644 --- a/visualization/motion_planning_tasks/task_list_model.cpp +++ b/visualization/motion_planning_tasks/task_list_model.cpp @@ -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 } diff --git a/visualization/motion_planning_tasks/task_list_model.h b/visualization/motion_planning_tasks/task_list_model.h index a066f7be..59c5152f 100644 --- a/visualization/motion_planning_tasks/task_list_model.h +++ b/visualization/motion_planning_tasks/task_list_model.h @@ -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;