From a43692fc25e601d884ca165558d22e4115c349ae Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 8 Nov 2017 17:41:04 +0100 Subject: [PATCH] renamed Task, Stage msgs - Stage -> StageStatistics + StageDescription - Task -> TaskStatistics + TaskDescription - removed GetInterfaceState.srv --- CMakeLists.txt | 9 +- .../moveit_task_constructor/introspection.h | 42 ++++--- include/moveit_task_constructor/task.h | 9 +- msg/Solution.msg | 4 +- msg/StageDescription.msg | 17 +++ msg/{Stage.msg => StageStatistics.msg} | 8 +- msg/SubSolution.msg | 4 - msg/SubTrajectory.msg | 18 ++- msg/TaskDescription.msg | 7 ++ msg/{Task.msg => TaskStatistics.msg} | 2 +- src/introspection.cpp | 31 +++-- src/storage.cpp | 3 +- src/task.cpp | 110 ++++++++++++------ src/test/test_task_model.cpp | 38 +++--- srv/GetInterfaceState.srv | 7 -- .../remote_task_model.cpp | 11 +- .../motion_planning_tasks/remote_task_model.h | 3 +- .../motion_planning_tasks/task_display.cpp | 77 ++++++------ .../motion_planning_tasks/task_display.h | 5 +- .../motion_planning_tasks/task_list_model.cpp | 30 +++-- .../motion_planning_tasks/task_list_model.h | 9 +- .../task_list_model_cache.cpp | 7 +- .../task_list_model_cache.h | 7 +- .../src/task_solution_visualization.cpp | 5 +- 24 files changed, 268 insertions(+), 195 deletions(-) create mode 100644 msg/StageDescription.msg rename msg/{Stage.msg => StageStatistics.msg} (87%) create mode 100644 msg/TaskDescription.msg rename msg/{Task.msg => TaskStatistics.msg} (77%) delete mode 100644 srv/GetInterfaceState.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index f51a95bc..21b46611 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,15 +18,16 @@ find_package(catkin REQUIRED COMPONENTS # ROS messages, services and actions add_message_files(DIRECTORY msg FILES - Stage.msg - Task.msg + StageDescription.msg + StageStatistics.msg + TaskDescription.msg + TaskStatistics.msg + Solution.msg SubSolution.msg SubTrajectory.msg - Solution.msg ) add_service_files(DIRECTORY srv FILES - GetInterfaceState.srv GetSolution.srv ) generate_messages(DEPENDENCIES ${MSG_DEPS}) diff --git a/include/moveit_task_constructor/introspection.h b/include/moveit_task_constructor/introspection.h index 3839c84b..cd05e4c4 100644 --- a/include/moveit_task_constructor/introspection.h +++ b/include/moveit_task_constructor/introspection.h @@ -3,40 +3,47 @@ #include #include #include -#include +#include +#include #include -#include #include +#define DESCRIPTION_TOPIC "description" +#define STATISTICS_TOPIC "statistics" +#define SOLUTION_TOPIC "solution" + namespace moveit { namespace task_constructor { MOVEIT_CLASS_FORWARD(Task) MOVEIT_CLASS_FORWARD(SolutionBase) -#define DEFAULT_TASK_MONITOR_TOPIC "task_monitoring" -#define DEFAULT_TASK_SOLUTION_TOPIC "task_solutions" - class Introspection { - // publish Task state and (new) Solutions - ros::Publisher task_publisher_; + // publish task detailed description and current state + ros::Publisher task_description_publisher_; + ros::Publisher task_statistics_publisher_; + // publish new solutions ros::Publisher solution_publisher_; - // services to provide InterfaceState and Solution - ros::ServiceServer get_interface_state_service_; + // services to provide an individual Solution ros::ServiceServer get_solution_service_; public: - Introspection(const std::string &task_topic = DEFAULT_TASK_MONITOR_TOPIC, - const std::string &solution_topic = DEFAULT_TASK_SOLUTION_TOPIC); + Introspection(); Introspection(const Introspection &other) = delete; static Introspection &instance(); - // publish the current state of given task - void publishTask(const Task &t); - void publishTask(const ::moveit_task_constructor::Task &msg) { - task_publisher_.publish(msg); + // publish detailed task description + void publishTaskDescription(const Task &t); + void publishTaskDescription(const ::moveit_task_constructor::TaskDescription &msg) { + task_description_publisher_.publish(msg); } - void operator()(const Task &t) { publishTask(t); } + // 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); + } + + void operator()(const Task &t) { publishTaskState(t); } // publish the given solution void publishSolution(const SolutionBase &s); @@ -45,9 +52,6 @@ public: } void operator()(const SolutionBase &s) { publishSolution(s); } - // get interface state - bool getInterfaceState(moveit_task_constructor::GetInterfaceState::Request &req, - moveit_task_constructor::GetInterfaceState::Response &res); // get solution bool getSolution(moveit_task_constructor::GetSolution::Request &req, moveit_task_constructor::GetSolution::Response &res); diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 3849505a..02dcfe39 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -6,7 +6,8 @@ #include "container.h" #include -#include +#include +#include #include namespace robot_model_loader { @@ -57,8 +58,10 @@ public: bool plan(); /// print current state std::cout static void printState(const Task &t); - /// fill task message for publishing the current state - moveit_task_constructor::Task& fillMessage(moveit_task_constructor::Task& msg) const; + /// fill task description message for publishing the task configuration + moveit_task_constructor::TaskDescription& fillTaskDescription(moveit_task_constructor::TaskDescription& msg) const; + /// fill task state message for publishing the current task state + moveit_task_constructor::TaskStatistics& fillTaskStatistics(moveit_task_constructor::TaskStatistics& msg) const; size_t numSolutions() const override; diff --git a/msg/Solution.msg b/msg/Solution.msg index eb232ce6..8a8c8982 100644 --- a/msg/Solution.msg +++ b/msg/Solution.msg @@ -1,8 +1,8 @@ # id of associated task string task_id -# id of robot model -string model_id +# planning scene of start state +moveit_msgs/PlanningScene start_scene # set of all sub solutions involved SubSolution[] sub_solution diff --git a/msg/StageDescription.msg b/msg/StageDescription.msg new file mode 100644 index 00000000..9d7c577d --- /dev/null +++ b/msg/StageDescription.msg @@ -0,0 +1,17 @@ +# static description of a stage + +# unique id within task +uint32 id + +# parent id, parent_id == id means root +uint32 parent_id + +# name of this stage +string name + +# flags: interface, ... +uint32 flags + +# properties +string property_name +string property_value diff --git a/msg/Stage.msg b/msg/StageStatistics.msg similarity index 87% rename from msg/Stage.msg rename to msg/StageStatistics.msg index 001e2457..36652bf7 100644 --- a/msg/Stage.msg +++ b/msg/StageStatistics.msg @@ -1,12 +1,8 @@ +# dynamically changing information for a stage + # unique id within task uint32 id -# name of this stage -string name - -# flags: interface, ... -uint32 flags - # parent id, parent_id == id means root uint32 parent_id diff --git a/msg/SubSolution.msg b/msg/SubSolution.msg index db733bb5..148b1314 100644 --- a/msg/SubSolution.msg +++ b/msg/SubSolution.msg @@ -4,9 +4,5 @@ uint32 id # associated cost float32 cost -# IDs of start and end state? -#uint32 start_id -#uint32 end_id - # IDs of subsolutions uint32[] sub_solution_id diff --git a/msg/SubTrajectory.msg b/msg/SubTrajectory.msg index 8d66b149..b6db3a02 100644 --- a/msg/SubTrajectory.msg +++ b/msg/SubTrajectory.msg @@ -1,22 +1,18 @@ # unique id within task uint32 id -# associated names -string name - # associated cost float32 cost -# name of task stage -string stage - -# IDs of start and end state? -# Or have an array of them in Solution.msg? -#uint32 start_id -#uint32 end_id +# optional associated name of sub trajectory +string name # trajectory moveit_msgs/RobotTrajectory trajectory +# markers, e.g. providing additional hints or illustrating failure visualization_msgs/MarkerArray markers -# TODO: add PlanningScene diff w.r.t. previous end state? +# ID of end state +uint32 end_id +# planning scene of end state as diff w.r.t. start state +moveit_msgs/PlanningScene scene_diff diff --git a/msg/TaskDescription.msg b/msg/TaskDescription.msg new file mode 100644 index 00000000..32fb8ca7 --- /dev/null +++ b/msg/TaskDescription.msg @@ -0,0 +1,7 @@ +# unique ID of this task +string id + +# list of all stages, including the task stage itself +StageDescription[] description +StageStatistics[] statistics + diff --git a/msg/Task.msg b/msg/TaskStatistics.msg similarity index 77% rename from msg/Task.msg rename to msg/TaskStatistics.msg index 1c641868..985fe16d 100644 --- a/msg/Task.msg +++ b/msg/TaskStatistics.msg @@ -2,4 +2,4 @@ string id # list of all stages, including the task stage itself -Stage[] stages +StageStatistics[] stages diff --git a/src/introspection.cpp b/src/introspection.cpp index 87cfb52d..594b3ded 100644 --- a/src/introspection.cpp +++ b/src/introspection.cpp @@ -7,15 +7,14 @@ namespace moveit { namespace task_constructor { -Introspection::Introspection(const std::string &task_topic, - const std::string &solution_topic) +Introspection::Introspection() { ros::NodeHandle n; - task_publisher_ = n.advertise(task_topic, 1); - solution_publisher_ = n.advertise<::moveit_task_constructor::Solution>(solution_topic, 1, true); + 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); n = ros::NodeHandle("~"); // services are advertised in private namespace - get_interface_state_service_ = n.advertiseService("get_interface_state", &Introspection::getInterfaceState, this); get_solution_service_ = n.advertiseService("get_solution", &Introspection::getSolution, this); } @@ -25,10 +24,16 @@ Introspection &Introspection::instance() return instance_; } -void Introspection::publishTask(const Task &t) +void Introspection::publishTaskDescription(const Task &t) { - ::moveit_task_constructor::Task msg; - task_publisher_.publish(t.fillMessage(msg)); + ::moveit_task_constructor::TaskDescription msg; + task_description_publisher_.publish(t.fillTaskDescription(msg)); +} + +void Introspection::publishTaskState(const Task &t) +{ + ::moveit_task_constructor::TaskStatistics msg; + task_statistics_publisher_.publish(t.fillTaskStatistics(msg)); } void Introspection::publishSolution(const SolutionBase &s) @@ -38,16 +43,6 @@ void Introspection::publishSolution(const SolutionBase &s) publishSolution(msg); } -bool Introspection::getInterfaceState(moveit_task_constructor::GetInterfaceState::Request &req, - moveit_task_constructor::GetInterfaceState::Response &res) -{ - const InterfaceState& state = Repository::instance()[req.state_id]; - moveit_msgs::PlanningScene msg; - state.scene()->getPlanningSceneDiffMsg(msg); - res.scene = msg; - return true; -} - bool Introspection::getSolution(moveit_task_constructor::GetSolution::Request &req, moveit_task_constructor::GetSolution::Response &res) { diff --git a/src/storage.cpp b/src/storage.cpp index a20da89d..90f2a3cd 100644 --- a/src/storage.cpp +++ b/src/storage.cpp @@ -68,9 +68,8 @@ void SubTrajectory::fillMessage(moveit_task_constructor::Solution &msg) const { msg.sub_trajectory.emplace_back(); moveit_task_constructor::SubTrajectory& t = msg.sub_trajectory.back(); t.id = this->id(); - t.name = this->name(); t.cost = this->cost(); - t.stage = this->creator()->name(); + t.name = this->name(); if (trajectory()) trajectory()->getRobotTrajectoryMsg(t.trajectory); t.markers = this->markers(); diff --git a/src/task.cpp b/src/task.cpp index a124a00b..ab831c7d 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -216,7 +216,7 @@ void Task::printState(const Task &t){ t.stages()->traverseRecursively(processor); } -void fillStateList(moveit_task_constructor::Stage::_received_starts_type &c, +void fillInterfaceList(moveit_task_constructor::StageStatistics::_received_starts_type &c, const InterfaceConstPtr& interface) { c.clear(); if (!interface) return; @@ -224,50 +224,94 @@ void fillStateList(moveit_task_constructor::Stage::_received_starts_type &c, c.push_back(state.id()); } -moveit_task_constructor::Task& Task::fillMessage(moveit_task_constructor::Task &msg) const +void fillStageStatistics(const Stage& stage, moveit_task_constructor::StageStatistics& s) { - std::map stage_to_id_map; + const StagePrivate *simpl = stage.pimpl(); + + fillInterfaceList(s.received_starts, simpl->starts()); + fillInterfaceList(s.received_ends, simpl->ends()); + fillInterfaceList(s.generated_starts, simpl->nextStarts()); + fillInterfaceList(s.generated_ends, simpl->prevEnds()); + + // insert solution IDs + Stage::SolutionProcessor solutionProcessor = [&s](const SolutionBase& solution) { + s.solved.push_back(solution.id()); + return true; + }; + stage.processSolutions(solutionProcessor); + + solutionProcessor = [&s](const SolutionBase& solution) { + s.failed.push_back(solution.id()); + return true; + }; + stage.processFailures(solutionProcessor); +} + +moveit_task_constructor::TaskDescription& Task::fillTaskDescription(moveit_task_constructor::TaskDescription &msg) const +{ + std::map stage_to_id_map; + stage_to_id_map[this] = 0; // ID for root + ContainerBase::StageCallback stageProcessor = [&stage_to_id_map, &msg](const Stage& stage, int) -> bool { // this method is called for each child stage of a given parent - const StagePrivate *simpl = stage.pimpl(); + moveit_task_constructor::StageDescription desc; + moveit_task_constructor::StageStatistics stat; + desc.id = stat.id = stage_to_id_map.size(); + stage_to_id_map[&stage] = stat.id; - moveit_task_constructor::Stage s; // create new Stage msg - s.id = stage_to_id_map.size(); - stage_to_id_map[&stage] = s.id; - s.name = stage.name(); - s.flags = stage.pimpl()->interfaceFlags(); - auto it = stage_to_id_map.find(simpl->parent()); + desc.name = stage.name(); + desc.flags = stage.pimpl()->interfaceFlags(); + // TODO fill stage properties + + auto it = stage_to_id_map.find(stage.pimpl()->parent()); assert (it != stage_to_id_map.cend()); - s.parent_id = it->second; + desc.parent_id = stat.parent_id = it->second; - fillStateList(s.received_starts, simpl->starts()); - fillStateList(s.received_ends, simpl->ends()); - fillStateList(s.generated_starts, simpl->nextStarts()); - fillStateList(s.generated_ends, simpl->prevEnds()); + fillStageStatistics(stage, stat); - // insert solution IDs - Stage::SolutionProcessor solutionProcessor = [&s](const SolutionBase& solution) { - s.solved.push_back(solution.id()); - return true; - }; - stage.processSolutions(solutionProcessor); - - solutionProcessor = [&s](const SolutionBase& solution) { - s.failed.push_back(solution.id()); - return true; - }; - stage.processFailures(solutionProcessor); - - // finally store in msg.stages - msg.stages.push_back(std::move(s)); + // finally store in msg + msg.description.push_back(std::move(desc)); + msg.statistics.push_back(std::move(stat)); return true; }; - msg.id = id(); - msg.stages.clear(); - stage_to_id_map[this] = 0; // ID for root + msg.description.clear(); + msg.statistics.clear(); stages()->traverseRecursively(stageProcessor); + + msg.id = id(); + return msg; +} + +moveit_task_constructor::TaskStatistics& Task::fillTaskStatistics(moveit_task_constructor::TaskStatistics &msg) const +{ + std::map stage_to_id_map; + stage_to_id_map[this] = 0; // ID for root + + ContainerBase::StageCallback stageProcessor = + [&stage_to_id_map, &msg](const Stage& stage, int) -> bool { + // this method is called for each child stage of a given parent + + moveit_task_constructor::StageStatistics stat; // create new Stage msg + stat.id = stage_to_id_map.size(); + stage_to_id_map[&stage] = stat.id; + + auto it = stage_to_id_map.find(stage.pimpl()->parent()); + assert (it != stage_to_id_map.cend()); + stat.parent_id = it->second; + + fillStageStatistics(stage, stat); + + // finally store in msg.stages + msg.stages.push_back(std::move(stat)); + return true; + }; + + msg.stages.clear(); + stages()->traverseRecursively(stageProcessor); + + msg.id = id(); return msg; } diff --git a/src/test/test_task_model.cpp b/src/test/test_task_model.cpp index 33635625..d42059b8 100644 --- a/src/test/test_task_model.cpp +++ b/src/test/test_task_model.cpp @@ -53,23 +53,25 @@ protected: int num_inserts = 0; int num_updates = 0; - moveit_task_constructor::Task genMsg(const std::string &name) { - moveit_task_constructor::Task t; + moveit_task_constructor::TaskDescription genMsg(const std::string &name) { + moveit_task_constructor::TaskDescription t; t.id = name; - uint id = 0; + uint id = 0, root_id; - moveit_task_constructor::Stage root; - root.parent_id = id; - root.id = ++id; - root.name = name; - t.stages.push_back(root); + moveit_task_constructor::StageDescription desc; + moveit_task_constructor::StageStatistics stat; + desc.parent_id = stat.parent_id = id; + desc.id = stat.id = root_id = ++id; + desc.name = name; + t.description.push_back(desc); + t.statistics.push_back(stat); for (int i = 0; i != children; ++i) { - moveit_task_constructor::Stage child; - child.parent_id = root.id; - child.id = ++id; - child.name = std::to_string(i); - t.stages.push_back(child); + desc.parent_id = stat.parent_id = root_id; + desc.id = stat.id = ++id; + desc.name = std::to_string(i); + t.description.push_back(desc); + t.statistics.push_back(stat); } return t; } @@ -123,7 +125,7 @@ protected: SCOPED_TRACE("first i=" + std::to_string(i)); num_inserts = 0; num_updates = 0; - model.processTaskMessage(genMsg("first")); + model.processTaskDescriptionMessage(genMsg("first")); if (i == 0) EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task else EXPECT_EQ(num_inserts, 0); @@ -135,7 +137,7 @@ protected: SCOPED_TRACE("second i=" + std::to_string(i)); num_inserts = 0; num_updates = 0; - model.processTaskMessage(genMsg("second")); // 1 notify for inserted task + model.processTaskDescriptionMessage(genMsg("second")); // 1 notify for inserted task if (i == 0) EXPECT_EQ(num_inserts, 1); else EXPECT_EQ(num_inserts, 0); @@ -157,7 +159,7 @@ protected: TEST_F(TaskListModelTest, remoteTaskModel) { children = 3; moveit_rviz_plugin::RemoteTaskModel m; - m.processTaskMessage(genMsg("first")); + m.processStageDescriptions(genMsg("first").description); SCOPED_TRACE("first"); validate(m, {"first"}); } @@ -188,13 +190,13 @@ TEST_F(TaskListModelTest, threeChildren) { TEST_F(TaskListModelTest, visitedPopulate) { // first population without children children = 0; - model.processTaskMessage(genMsg("first")); + model.processTaskDescriptionMessage(genMsg("first")); validate(model, {"first"}); // validation visits root node EXPECT_EQ(num_inserts, 1); children = 3; num_inserts = 0; - model.processTaskMessage(genMsg("first")); + model.processTaskDescriptionMessage(genMsg("first")); validate(model, {"first"}); // second population with children should emit insert notifies for them EXPECT_EQ(num_inserts, 3); diff --git a/srv/GetInterfaceState.srv b/srv/GetInterfaceState.srv deleted file mode 100644 index 33f7f7af..00000000 --- a/srv/GetInterfaceState.srv +++ /dev/null @@ -1,7 +0,0 @@ -# ID of InterfaceState -uint32 state_id - ---- - -# planning scene diff to tasks' main planning scene -moveit_msgs/PlanningScene scene diff --git a/visualization/motion_planning_tasks/remote_task_model.cpp b/visualization/motion_planning_tasks/remote_task_model.cpp index 9bf24582..fad9c4eb 100644 --- a/visualization/motion_planning_tasks/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/remote_task_model.cpp @@ -182,11 +182,10 @@ bool RemoteTaskModel::setData(const QModelIndex &index, const QVariant &value, i return n->setName(value.toString()); } -typedef moveit_task_constructor::Stage StageMsg; -void RemoteTaskModel::processTaskMessage(const moveit_task_constructor::Task &msg) +void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor::TaskDescription::_description_type &msg) { - // iterate over msg.stages and create new nodes where needed - for (const StageMsg &s : msg.stages) { + // iterate over descriptions and create new / update existing nodes where needed + for (const auto &s : msg) { // find parent node for stage s, this should always exist auto parent_it = id_to_stage_.find(s.parent_id); if (parent_it == id_to_stage_.end()) { @@ -229,4 +228,8 @@ void RemoteTaskModel::processTaskMessage(const moveit_task_constructor::Task &ms } } +void RemoteTaskModel::processStageStatistics(const moveit_task_constructor::TaskDescription::_statistics_type &msg) +{ +} + } diff --git a/visualization/motion_planning_tasks/remote_task_model.h b/visualization/motion_planning_tasks/remote_task_model.h index e094f45e..590c626e 100644 --- a/visualization/motion_planning_tasks/remote_task_model.h +++ b/visualization/motion_planning_tasks/remote_task_model.h @@ -62,7 +62,8 @@ public: QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override; bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override; - void processTaskMessage(const moveit_task_constructor::Task &msg); + void processStageDescriptions(const moveit_task_constructor::TaskDescription::_description_type &msg); + void processStageStatistics(const moveit_task_constructor::TaskDescription::_statistics_type &msg); }; } diff --git a/visualization/motion_planning_tasks/task_display.cpp b/visualization/motion_planning_tasks/task_display.cpp index 460440a0..b71fa9a6 100644 --- a/visualization/motion_planning_tasks/task_display.cpp +++ b/visualization/motion_planning_tasks/task_display.cpp @@ -57,19 +57,12 @@ TaskDisplay::TaskDisplay() : Display() "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded", this, SLOT(changedRobotDescription()), this); - task_monitor_topic_property_ = - new rviz::RosTopicProperty("Task Monitor Topic", DEFAULT_TASK_MONITOR_TOPIC, - ros::message_traits::datatype(), - "The topic on which task updates (moveit_msgs::Task messages) are received", - this, SLOT(changedTaskMonitorTopic()), this); - task_solution_topic_property_ = - new rviz::RosTopicProperty("Task Solution Topic", DEFAULT_TASK_SOLUTION_TOPIC, + new rviz::RosTopicProperty("Task Solution Topic", "", ros::message_traits::datatype(), "The topic on which task solutions (moveit_msgs::Solution messages) are received", this, SLOT(changedTaskSolutionTopic()), this); - trajectory_visual_.reset(new TaskSolutionVisualization(this, this)); } @@ -122,7 +115,8 @@ void TaskDisplay::onDisable() trajectory_visual_->onDisable(); // don't monitor topics when disabled - task_monitor_sub.shutdown(); + task_description_sub.shutdown(); + task_statistics_sub.shutdown(); task_solution_sub.shutdown(); } @@ -149,44 +143,49 @@ void TaskDisplay::changedRobotDescription() void TaskDisplay::updateTaskListModel() { - task_model_ = TaskListModelCache::instance().getModel( - task_monitor_topic_property_->getString(), - task_solution_topic_property_->getString()); + // 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); - if (!task_model_) { - if (task_monitor_topic_property_->getString().isEmpty()) - setStatus(rviz::StatusProperty::Warn, "Task Monitor", "invalid task monitor topic"); - else if (task_solution_topic_property_->getString().isEmpty()) - setStatus(rviz::StatusProperty::Warn, "Task Monitor", "invalid task solution topic"); - else - setStatus(rviz::StatusProperty::Error, "Task Monitor", "failed to create TaskListModel"); + 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); + + // 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); } else { - boost::function taskCB - ([this](const moveit_task_constructor::TaskConstPtr &msg){ - mainloop_jobs_.addJob([this, msg]() { task_model_->processTaskMessage(*msg); }); - }); - task_monitor_sub = update_nh_.subscribe(task_monitor_topic_property_->getStdString(), 2, taskCB); - - boost::function solCB - ([this](const moveit_task_constructor::SolutionConstPtr &msg){ - mainloop_jobs_.addJob([this, msg]() { task_model_->processSolutionMessage(*msg); }); - // TODO: use already processed trajectory (e.g. by ID) - mainloop_jobs_.addJob([this, msg]() {trajectory_visual_->showTrajectory(*msg);}); - }); - task_solution_sub = update_nh_.subscribe(task_solution_topic_property_->getStdString(), 2, solCB); - - setStatus(rviz::StatusProperty::Ok, "Task Monitor", "Connected"); + setStatus(rviz::StatusProperty::Error, "Task Monitor", "failed to create TaskListModel"); } -} -void TaskDisplay::changedTaskMonitorTopic() -{ - task_monitor_sub.shutdown(); - updateTaskListModel(); + // 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); + + setStatus(rviz::StatusProperty::Ok, "Task Monitor", "Connected"); } void TaskDisplay::changedTaskSolutionTopic() { + task_description_sub.shutdown(); + task_statistics_sub.shutdown(); task_solution_sub.shutdown(); updateTaskListModel(); } diff --git a/visualization/motion_planning_tasks/task_display.h b/visualization/motion_planning_tasks/task_display.h index 86fc52fe..9c08f6f8 100644 --- a/visualization/motion_planning_tasks/task_display.h +++ b/visualization/motion_planning_tasks/task_display.h @@ -84,15 +84,15 @@ private Q_SLOTS: * \brief Slot Event Functions */ void changedRobotDescription(); - void changedTaskMonitorTopic(); void changedTaskSolutionTopic(); protected: void updateTaskListModel(); protected: - ros::Subscriber task_monitor_sub; ros::Subscriber task_solution_sub; + ros::Subscriber task_description_sub; + ros::Subscriber task_statistics_sub; // handle processing of task+solution messages in Qt mainloop moveit::tools::JobQueue mainloop_jobs_; @@ -107,7 +107,6 @@ protected: // Properties rviz::StringProperty* robot_description_property_; - rviz::RosTopicProperty* task_monitor_topic_property_; rviz::RosTopicProperty* task_solution_topic_property_; }; diff --git a/visualization/motion_planning_tasks/task_list_model.cpp b/visualization/motion_planning_tasks/task_list_model.cpp index 0f685073..245dba8b 100644 --- a/visualization/motion_planning_tasks/task_list_model.cpp +++ b/visualization/motion_planning_tasks/task_list_model.cpp @@ -254,10 +254,9 @@ bool TaskListModel::removeRows(int row, int count, const QModelIndex &parent) return false; } -// process a task monitoring message: -// update existing RemoteTask, create a new one, -// or (if msg.stages is empty) delete an existing one -void TaskListModel::processTaskMessage(const moveit_task_constructor::Task &msg) +// 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) { Q_D(TaskListModel); @@ -266,8 +265,8 @@ void TaskListModel::processTaskMessage(const moveit_task_constructor::Task &msg) bool created = it_inserted.second; RemoteTaskModel*& remote_task = it_inserted.first->second; - // empty stages list indicates, that this remote task is not available anymore - if (msg.stages.empty()) { + // empty list indicates, that this remote task is not available anymore + if (msg.description.empty()) { if (!remote_task) { // task was already deleted locally // we can now remove it from remote_tasks_ d->remote_tasks_.erase(it_inserted.first); @@ -287,7 +286,8 @@ void TaskListModel::processTaskMessage(const moveit_task_constructor::Task &msg) if (!remote_task) return; // task is not in use anymore - remote_task->processTaskMessage(msg); + remote_task->processStageDescriptions(msg.description); + remote_task->processStageStatistics(msg.statistics); // insert newly created model into this' model instance if (created) { @@ -296,6 +296,22 @@ void TaskListModel::processTaskMessage(const moveit_task_constructor::Task &msg) } } +// process a task statistics message +void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor::TaskStatistics &msg) +{ + Q_D(TaskListModel); + + auto it = d->remote_tasks_.find(msg.id); + if (it == d->remote_tasks_.cend()) + return; // unkown task + + RemoteTaskModel* remote_task = it->second; + if (!remote_task) + return; // task is not in use anymore + + remote_task->processStageStatistics(msg.stages); +} + void TaskListModel::processSolutionMessage(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 61e7a688..a066f7be 100644 --- a/visualization/motion_planning_tasks/task_list_model.h +++ b/visualization/motion_planning_tasks/task_list_model.h @@ -37,7 +37,8 @@ #pragma once #include -#include +#include +#include #include #include @@ -89,8 +90,10 @@ public: bool removeRows(int row, int count, const QModelIndex &parent) override; - /// process an incoming task message - only call in Qt's main loop - void processTaskMessage(const moveit_task_constructor::Task &msg); + /// process an incoming task description message - only call in Qt's main loop + void processTaskDescriptionMessage(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); /// process an incoming solution message - only call in Qt's main loop void processSolutionMessage(const moveit_task_constructor::Solution &msg); diff --git a/visualization/motion_planning_tasks/task_list_model_cache.cpp b/visualization/motion_planning_tasks/task_list_model_cache.cpp index 42840178..c1ee349f 100644 --- a/visualization/motion_planning_tasks/task_list_model_cache.cpp +++ b/visualization/motion_planning_tasks/task_list_model_cache.cpp @@ -49,14 +49,13 @@ TaskListModelCache &TaskListModelCache::instance() return instance_; } -TaskListModelPtr TaskListModelCache::getModel(const QString &task_monitor_topic, - const QString &task_solution_topic) +TaskListModelPtr TaskListModelCache::getModel(const std::string& ns) { - if (task_monitor_topic.isEmpty() || task_solution_topic.isEmpty()) { + if (ns.empty()) { return TaskListModelPtr(); } else { // retrieve existing model for given topic pair - TaskListModelWeakPtr& model = cache_[std::make_pair(task_monitor_topic, task_solution_topic)]; + TaskListModelWeakPtr& model = cache_[ns]; TaskListModelPtr result; if (model.expired()) { diff --git a/visualization/motion_planning_tasks/task_list_model_cache.h b/visualization/motion_planning_tasks/task_list_model_cache.h index dabda262..d9695ce0 100644 --- a/visualization/motion_planning_tasks/task_list_model_cache.h +++ b/visualization/motion_planning_tasks/task_list_model_cache.h @@ -45,7 +45,7 @@ namespace moveit_rviz_plugin { * * This global model instance is used by TaskPanels and can be retrieved via getGlobalModel(). * Additionally, this instance maintains a cache for all TaskListModels used e.g. by TaskDisplays. - * Displays subscribing to the same (monitor, solution) topic pair, will share the same model. + * Displays subscribing to the same topic namespace, will share the same model. * * This is a singleton instance. */ @@ -53,7 +53,7 @@ class TaskListModelCache : public QObject { Q_OBJECT TaskListModelPtr global_model_; - std::map, TaskListModelWeakPtr> cache_; + std::map cache_; /// class is singleton TaskListModelCache(); @@ -68,8 +68,7 @@ public: static TaskListModelCache& instance(); /// get TaskListModel for a TaskDisplay - TaskListModelPtr getModel(const QString& task_monitor_topic, - const QString& task_solution_topic); + TaskListModelPtr getModel(const std::__cxx11::string &ns); /// get global TaskListModel instance used for panels TaskListModelPtr getGlobalModel(); diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index 99f100b3..12217898 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -431,8 +431,9 @@ void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor::So return; } - if (!msg.model_id.empty() && msg.model_id != robot_model_->getName()) - ROS_WARN("Received a trajectory to display for model '%s' but model '%s' was expected", msg.model_id.c_str(), + if (msg.start_scene.robot_model_name != robot_model_->getName()) + ROS_WARN("Received a trajectory to display for model '%s' but model '%s' was expected", + msg.start_scene.robot_model_name .c_str(), robot_model_->getName().c_str()); trajectory_message_to_display_.reset();