diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 1c2e5ecb..37fb228f 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -238,10 +238,10 @@ void TaskDisplay::clearMarkers() { marker_visual_->clearMarkers(); } -void TaskDisplay::showMarkers(const DisplaySolutionPtr &s) { +void TaskDisplay::addMarkers(const DisplaySolutionPtr &s) { if (!s) return; for (size_t i=0, end = s->numSubSolutions(); i != end; ++i) { - marker_visual_->showMarkers(s->markersOfSubTrajectory(i)); + marker_visual_->addMarkers(s->markersOfSubTrajectory(i)); } } diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index b6c9c0b3..a62a6f62 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -86,7 +86,7 @@ public: TaskListModel& getTaskListModel() { return *task_list_model_; } TaskSolutionVisualization* visualization() const { return trajectory_visual_.get(); } void clearMarkers(); - void showMarkers(const DisplaySolutionPtr &s); + void addMarkers(const DisplaySolutionPtr &s); protected: void onInitialize() override; diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 2a008bda..03bc7498 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -310,7 +310,7 @@ void TaskView::onSolutionSelectionChanged(const QItemSelection &selected, const for (const auto& index : selected_rows) { const DisplaySolutionPtr &solution = task->getSolution(index); display->setSolutionStatus(bool(solution)); - display->showMarkers(solution); + display->addMarkers(solution); } } diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h index 5eedefa8..96292670 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h @@ -22,6 +22,11 @@ namespace moveit_rviz_plugin { MOVEIT_CLASS_FORWARD(MarkerVisualization) +/** Container for all markers created from a vector of Marker messages + * + * Markers within a specific namespace are created as children of a + * corresponding scene node, which allows for fast toggling of visibility. + */ class MarkerVisualization { // list of all markers, attached to scene nodes in namespaces_ @@ -41,6 +46,11 @@ public: }; +/** rviz property allowing to group markers by their namespace + * + * The class remembers which MarkerVisualization instances are currently hosted + * and provides the user interaction to toggle marker visibility by namespace. + */ class MarkerVisualizationProperty: public rviz::BoolProperty { Q_OBJECT @@ -48,8 +58,8 @@ class MarkerVisualizationProperty: public rviz::BoolProperty rviz::DisplayContext* context_ = nullptr; Ogre::SceneNode* parent_scene_node_ = nullptr; // scene node provided externally Ogre::SceneNode* marker_scene_node_ = nullptr; // scene node all markers are attached to - std::map namespaces_; - std::list visible_markers_; + std::map namespaces_; // rviz properties for encountered namespaces + std::list hosted_markers_; // list of hosted MarkerVisualization instances public: MarkerVisualizationProperty(const QString& name, Property* parent = nullptr); @@ -57,8 +67,10 @@ public: void onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context); + /// remove all hosted markers from display void clearMarkers(); - void showMarkers(MarkerVisualizationPtr markers); + /// add all markers in MarkerVisualization for display + void addMarkers(MarkerVisualizationPtr markers); public Q_SLOTS: void onEnableChanged(); diff --git a/visualization/visualization_tools/src/marker_visualization.cpp b/visualization/visualization_tools/src/marker_visualization.cpp index 2ae67417..80702edd 100644 --- a/visualization/visualization_tools/src/marker_visualization.cpp +++ b/visualization/visualization_tools/src/marker_visualization.cpp @@ -126,20 +126,20 @@ void MarkerVisualizationProperty::clearMarkers() { // detach all existing scene nodes marker_scene_node_->removeAllChildren(); - // clear list of memorized visible markers - visible_markers_.clear(); + // clear list of hosted markers + hosted_markers_.clear(); } -void MarkerVisualizationProperty::showMarkers(MarkerVisualizationPtr markers) +void MarkerVisualizationProperty::addMarkers(MarkerVisualizationPtr markers) { if (!markers) return; - // memorize that those markers are visible - visible_markers_.push_back(markers); + // remember that those markers are hosted + hosted_markers_.push_back(markers); // attach all scene nodes from markers for (const auto& pair : markers->namespaces()) { - // create namespace sub property + // create sub property for newly encountered namespace, enabling visibility by default auto ns_it = namespaces_.insert(std::make_pair(pair.first, nullptr)).first; if (ns_it->second == nullptr) { ns_it->second = new rviz::BoolProperty(pair.first, true, "Show/hide markers of this namespace", this, @@ -166,7 +166,8 @@ void MarkerVisualizationProperty::onNSEnableChanged() rviz::BoolProperty *ns_property = static_cast(sender()); const QString &ns = ns_property->getName(); bool visible = ns_property->getBool(); - for (const auto& markers : visible_markers_) + // for all hosted markers, set visibility of given namespace + for (const auto& markers : hosted_markers_) markers->setVisible(ns, marker_scene_node_, visible); }