From 30f6ade636b07081202e038f30c175272dd2af4d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 29 Nov 2017 11:21:24 +0100 Subject: [PATCH] marker visualization --- .../src/task_display.cpp | 18 +- .../motion_planning_tasks/src/task_display.h | 4 + .../motion_planning_tasks/src/task_panel.cpp | 2 + .../visualization_tools/CMakeLists.txt | 3 + .../visualization_tools/display_solution.h | 19 ++ .../marker_visualization.h | 68 +++++++ .../src/display_solution.cpp | 11 ++ .../src/marker_visualization.cpp | 171 ++++++++++++++++++ .../src/task_solution_visualization.cpp | 5 +- 9 files changed, 299 insertions(+), 2 deletions(-) create mode 100644 visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h create mode 100644 visualization/visualization_tools/src/marker_visualization.cpp diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 4a73dd43..c6af95b6 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -41,6 +41,8 @@ #include "meta_task_list_model.h" #include #include +#include +#include #include #include @@ -77,6 +79,8 @@ TaskDisplay::TaskDisplay() : Display() trajectory_visual_.reset(new TaskSolutionVisualization(this, this)); + marker_visual_ = new MarkerVisualizationProperty("Markers", this); + tasks_property_ = new rviz::Property("Tasks", QVariant(), "Tasks received on monitored topic", this); } @@ -89,6 +93,7 @@ void TaskDisplay::onInitialize() { Display::onInitialize(); trajectory_visual_->onInitialize(scene_node_, context_); + marker_visual_->onInitialize(scene_node_, context_); } void TaskDisplay::loadRobotModel() @@ -179,7 +184,7 @@ void TaskDisplay::taskSolutionCB(const ros::MessageEventtask_id; mainloop_jobs_.addJob([this, id, msg]() { const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg); - trajectory_visual_->showTrajectory(s); + if (s) trajectory_visual_->showTrajectory(s); return; }); } @@ -189,6 +194,17 @@ void TaskDisplay::showTrajectory(const DisplaySolutionPtr &s) const { trajectory_visual_->showTrajectory(s); } +void TaskDisplay::clearMarkers() { + marker_visual_->clearMarkers(); +} + +void TaskDisplay::showMarkers(const DisplaySolutionPtr &s) { + if (!s) return; + for (size_t i=0, end = s->numSubSolutions(); i != end; ++i) { + marker_visual_->showMarkers(s->markersOfSubTrajectory(i)); + } +} + void TaskDisplay::changedTaskSolutionTopic() { task_description_sub.shutdown(); diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index 5158cc32..3ee11aa4 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -64,6 +64,7 @@ namespace moveit_rviz_plugin MOVEIT_CLASS_FORWARD(DisplaySolution) class TaskSolutionVisualization; +class MarkerVisualizationProperty; class TaskListModel; class TaskDisplay : public rviz::Display @@ -87,6 +88,8 @@ public: TaskListModel& getTaskListModel() { return *task_list_model_; } void showTrajectory(const DisplaySolutionPtr& s) const; + void clearMarkers(); + void showMarkers(const DisplaySolutionPtr &s); private Q_SLOTS: /** @@ -123,6 +126,7 @@ protected: // Properties rviz::StringProperty* robot_description_property_; rviz::RosTopicProperty* task_solution_topic_property_; + MarkerVisualizationProperty* marker_visual_; rviz::Property* tasks_property_; }; diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 2cffd95f..7abc7569 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -219,6 +219,8 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QMode return; display->showTrajectory(solution); + display->clearMarkers(); + display->showMarkers(solution); } } diff --git a/visualization/visualization_tools/CMakeLists.txt b/visualization/visualization_tools/CMakeLists.txt index fa05e39e..364d0fbf 100644 --- a/visualization/visualization_tools/CMakeLists.txt +++ b/visualization/visualization_tools/CMakeLists.txt @@ -4,6 +4,7 @@ set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/visualization_too set(HEADERS ${PROJECT_INCLUDE}/display_solution.h + ${PROJECT_INCLUDE}/marker_visualization.h ${PROJECT_INCLUDE}/task_solution_panel.h ${PROJECT_INCLUDE}/task_solution_visualization.h ) @@ -12,6 +13,7 @@ add_library(${MOVEIT_LIB_NAME} ${HEADERS} src/display_solution.cpp + src/marker_visualization.cpp src/task_solution_panel.cpp src/task_solution_visualization.cpp ) @@ -19,6 +21,7 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} + ${rviz_DEFAULT_PLUGIN_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h index 8719d276..17d2c50f 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h @@ -48,10 +48,18 @@ MOVEIT_CLASS_FORWARD(PlanningScene) namespace robot_trajectory { MOVEIT_CLASS_FORWARD(RobotTrajectory) } +namespace Ogre +{ +class SceneNode; +} +namespace rviz { +class DisplayContext; +} namespace moveit_rviz_plugin { MOVEIT_CLASS_FORWARD(DisplaySolution) +MOVEIT_CLASS_FORWARD(MarkerVisualization) /** Class representing a task solution for display */ class DisplaySolution @@ -68,6 +76,8 @@ class DisplaySolution robot_trajectory::RobotTrajectoryPtr trajectory_; /// optional name of the trajectory std::string name_; + /// rviz markers + MarkerVisualizationPtr markers_; }; std::vector data_; @@ -76,6 +86,8 @@ public: /// create DisplaySolution for given sub trajectory of master DisplaySolution(const DisplaySolution& master, uint32_t sub); + size_t numSubSolutions() const { return data_.size(); } + size_t getWayPointCount() const { return steps_; } bool empty() const { return steps_ == 0; } @@ -101,6 +113,13 @@ public: const std::string& name(size_t index) const { return name(indexPair(index)); } + const MarkerVisualizationPtr markers(const IndexPair& idx_pair) const; + const MarkerVisualizationPtr markers(size_t index) const { + return markers(indexPair(index)); + } + const MarkerVisualizationPtr markersOfSubTrajectory(size_t index) const { + return data_.at(index).markers_; + } void setFromMessage(const planning_scene::PlanningScenePtr &start_scene, const moveit_task_constructor_msgs::Solution& msg); diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h new file mode 100644 index 00000000..5eedefa8 --- /dev/null +++ b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h @@ -0,0 +1,68 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace Ogre +{ +class SceneNode; +} + +namespace rviz +{ +class DisplayContext; +class MarkerBase; +} + +namespace moveit_rviz_plugin { + +MOVEIT_CLASS_FORWARD(MarkerVisualization) + +class MarkerVisualization +{ + // list of all markers, attached to scene nodes in namespaces_ + typedef std::pair> MarkerData; + std::deque markers_; + // markers grouped by their namespace + std::map namespaces_; + +public: + MarkerVisualization(const std::vector& markers); + ~MarkerVisualization(); + + void createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* scene_node); + const std::map& namespaces() const { return namespaces_; } + + void setVisible(const QString &ns, Ogre::SceneNode* parent_scene_node, bool visible); +}; + + +class MarkerVisualizationProperty: public rviz::BoolProperty +{ + Q_OBJECT + + 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_; + +public: + MarkerVisualizationProperty(const QString& name, Property* parent = nullptr); + ~MarkerVisualizationProperty(); + + void onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context); + + void clearMarkers(); + void showMarkers(MarkerVisualizationPtr markers); + +public Q_SLOTS: + void onEnableChanged(); + void onNSEnableChanged(); +}; + +} // namespace moveit_rviz_plugin diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp index 1a6cf3d8..bc71c19b 100644 --- a/visualization/visualization_tools/src/display_solution.cpp +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -35,6 +35,7 @@ /* Author: Robert Haschke */ #include +#include #include #include #include @@ -83,6 +84,11 @@ const std::string &DisplaySolution::name(const IndexPair &idx_pair) const return data_[idx_pair.first].name_; } +const MarkerVisualizationPtr DisplaySolution::markers(const DisplaySolution::IndexPair &idx_pair) const +{ + return data_[idx_pair.first].markers_; +} + void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& start_scene, const moveit_task_constructor_msgs::Solution &msg) { @@ -113,6 +119,11 @@ void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& sta // create new reference scene for next iteration ref_scene = ref_scene->diff(); + + if (sub.markers.size()) + data_[i].markers_.reset(new MarkerVisualization(sub.markers)); + else + data_[i].markers_.reset(); ++i; } } diff --git a/visualization/visualization_tools/src/marker_visualization.cpp b/visualization/visualization_tools/src/marker_visualization.cpp new file mode 100644 index 00000000..345a2878 --- /dev/null +++ b/visualization/visualization_tools/src/marker_visualization.cpp @@ -0,0 +1,171 @@ +#include + +#include +#include "rviz/default_plugin/markers/arrow_marker.h" +#include "rviz/default_plugin/markers/line_list_marker.h" +#include "rviz/default_plugin/markers/line_strip_marker.h" +#include "rviz/default_plugin/markers/mesh_resource_marker.h" +#include "rviz/default_plugin/markers/points_marker.h" +#include "rviz/default_plugin/markers/shape_marker.h" +#include "rviz/default_plugin/markers/text_view_facing_marker.h" +#include "rviz/default_plugin/markers/triangle_list_marker.h" +#include + +namespace moveit_rviz_plugin { + +rviz::MarkerBase* createMarker(int marker_type, rviz::DisplayContext* context, Ogre::SceneNode* node) +{ + switch (marker_type) { + case visualization_msgs::Marker::CUBE: + case visualization_msgs::Marker::CYLINDER: + case visualization_msgs::Marker::SPHERE: + return new rviz::ShapeMarker(nullptr, context, node); + + case visualization_msgs::Marker::ARROW: + return new rviz::ArrowMarker(nullptr, context, node); + + case visualization_msgs::Marker::LINE_STRIP: + return new rviz::LineStripMarker(nullptr, context, node); + + case visualization_msgs::Marker::LINE_LIST: + return new rviz::LineListMarker(nullptr, context, node); + + case visualization_msgs::Marker::SPHERE_LIST: + case visualization_msgs::Marker::CUBE_LIST: + case visualization_msgs::Marker::POINTS: + return new rviz::PointsMarker(nullptr, context, node); + + case visualization_msgs::Marker::TEXT_VIEW_FACING: + return new rviz::TextViewFacingMarker(nullptr, context, node); + + case visualization_msgs::Marker::MESH_RESOURCE: + return new rviz::MeshResourceMarker(nullptr, context, node); + + case visualization_msgs::Marker::TRIANGLE_LIST: + return new rviz::TriangleListMarker(nullptr, context, node); + + default: + ROS_ERROR("Unknown marker type: %d", marker_type); + return nullptr; + } +} + +MarkerVisualization::MarkerVisualization(const std::vector &markers) +{ + // remember marker message, postpone rviz::MarkerBase creation until later + for (const auto& marker : markers) { + // create MarkerData with nil Marker pointer + MarkerData data; + data.first.reset(new visualization_msgs::Marker(marker)); + markers_.push_back(std::move(data)); + // remember namespace name + namespaces_.insert(std::make_pair(QString::fromStdString(marker.ns), nullptr)); + } +} + +MarkerVisualization::~MarkerVisualization() +{ + for (const auto& pair : namespaces_) + pair.second->getCreator()->destroySceneNode(pair.second); +} + +void setVisibility(Ogre::SceneNode *node, Ogre::SceneNode *parent, bool visible) +{ + if (visible && node->getParent() != parent) + parent->addChild(node); + else if (!visible && node->getParent()) + node->getParent()->removeChild(node); +} + +void MarkerVisualization::setVisible(const QString &ns, Ogre::SceneNode* parent_scene_node, bool visible) +{ + auto it = namespaces_.find(ns); + if (it == namespaces_.end()) + return; + setVisibility(it->second, parent_scene_node, visible); +} + +void MarkerVisualization::createMarkers(rviz::DisplayContext *context, Ogre::SceneNode *parent_scene_node) +{ + for (MarkerData& data : markers_) { + if (data.second) continue; + + auto ns_it = namespaces_.find(QString::fromStdString(data.first->ns)); + Q_ASSERT(ns_it != namespaces_.end()); // we have added all namespaces before! + if (ns_it->second == nullptr) // create scene node for this namespace + ns_it->second = parent_scene_node->getCreator()->createSceneNode(); + + data.second.reset(createMarker(data.first->type, context, ns_it->second)); + if (data.second) data.second->setMessage(data.first); + } +} + + +MarkerVisualizationProperty::MarkerVisualizationProperty(const QString &name, rviz::Property *parent) + : rviz::BoolProperty(name, true, "Enable/disable markers", parent) +{ + connect(this, SIGNAL(changed()), this, SLOT(onEnableChanged())); +} + +MarkerVisualizationProperty::~MarkerVisualizationProperty() +{ + if (marker_scene_node_) + marker_scene_node_->getCreator()->destroySceneNode(marker_scene_node_); +} + +void MarkerVisualizationProperty::onInitialize(Ogre::SceneNode *scene_node, rviz::DisplayContext *context) +{ + context_ = context; + parent_scene_node_ = scene_node; + marker_scene_node_ = parent_scene_node_->createChildSceneNode(); +} + +void MarkerVisualizationProperty::clearMarkers() +{ + // detach all existing scene nodes + marker_scene_node_->removeAllChildren(); + // clear list of memorized visible markers + visible_markers_.clear(); +} + +void MarkerVisualizationProperty::showMarkers(MarkerVisualizationPtr markers) +{ + if (!markers) return; + + // memorize that those markers are visible + visible_markers_.push_back(markers); + + // attach all scene nodes from markers + for (const auto& pair : markers->namespaces()) { + // create namespace sub property + 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, + SLOT(onNSEnableChanged()), this); + } + if (!ns_it->second->getBool()) + continue; // nothing to show for this namespace + + if (!pair.second) // invalid scene node indicates that we still need to create the rviz markers + markers->createMarkers(context_, marker_scene_node_); + + Q_ASSERT(pair.second); + marker_scene_node_->addChild(pair.second); + } +} + +void MarkerVisualizationProperty::onEnableChanged() +{ + setVisibility(marker_scene_node_, parent_scene_node_, getBool()); +} + +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_) + markers->setVisible(ns, marker_scene_node_, visible); +} + +} // namespace moveit_rviz_plugin diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index cd6923c8..03366b30 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Dave Coleman, Robert Haschke */ +/* Author: Robert Haschke */ #include #include @@ -164,6 +164,9 @@ TaskSolutionVisualization::~TaskSolutionVisualization() robot_render_.reset(); if (slider_dock_panel_) delete slider_dock_panel_; + + if (main_scene_node_) + main_scene_node_->getCreator()->destroySceneNode(main_scene_node_); } void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context)