Add support for more Marker geometry types

ign_rviz/CMakeLists.txt

@@ -29,6 +29,7 @@ find_package(nav_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(visualization_msgs REQUIRED)
 
 # Ignition libraries
 find_package(ign_rviz_common REQUIRED)
@@ -75,6 +76,7 @@ ament_target_dependencies(ign_rviz
  sensor_msgs
  tf2_msgs
  tf2_ros
+ visualization_msgs
 )
 
 target_link_libraries(ign_rviz Qt5::Core Qt5::Qml Qt5::Quick Qt5::QuickControls2)

ign_rviz/include/ignition/rviz/RVizDrawer.qml

@@ -239,6 +239,10 @@ Rectangle {
  RViz.addLaserScanDisplay(_name)
  break;
  }
+ case "visualization_msgs/msg/Marker": {
+ RViz.addMarkerDisplay(_name)
+ break;
+ }
  }
  }
 

ign_rviz/package.xml

@@ -21,6 +21,7 @@
  <depend>tf2_geometry_msgs</depend>
  <depend>tf2_msgs</depend>
  <depend>tf2_ros</depend>
+ <depend>visualization_msgs</depend>
 
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

ign_rviz/src/rviz/rviz.cpp

@@ -23,6 +23,7 @@
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <tf2_msgs/msg/tf_message.hpp>
+#include <visualization_msgs/msg/marker.hpp>
 
 #include <memory>
 #include <string>
@@ -82,7 +83,8 @@ RViz::RViz()
  "geometry_msgs/msg/PoseArray",
  "nav_msgs/msg/Path",
  "sensor_msgs/msg/Image",
- "sensor_msgs/msg/LaserScan"
+ "sensor_msgs/msg/LaserScan",
+ "visualization_msgs/msg/Marker"
  };
 }
 
@@ -156,7 +158,7 @@ void RViz::addMarkerDisplay(const QString & _topic) const
  // Load plugin
  if (ignition::gui::App()->LoadPlugin("MarkerDisplay")) {
  auto markerDisplay =
- ignition::gui::App()->findChildren<DisplayPlugin<geometry_msgs::msg::PointStamped> *>();
+ ignition::gui::App()->findChildren<DisplayPlugin<visualization_msgs::msg::Marker> *>();
  int pluginCount = markerDisplay.size() - 1;
 
  // Set frame manager and install event filter for recently added plugin

ign_rviz_plugins/include/ignition/rviz/plugins/MarkerManager.hpp

@@ -17,6 +17,7 @@
 
 #include <ignition/rendering.hh>
 
+#include <geometry_msgs/msg/pose.hpp>
 #include <std_msgs/msg/color_rgba.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
@@ -80,12 +81,49 @@ class MarkerManager
  void createListGeometry(
  const visualization_msgs::msg::Marker::SharedPtr _msg, rendering::MarkerType _geometryType);
 
+ /**
+ * @brief Create arrow marker
+ * @param[in] _msg Marker message
+ */
+ void createArrowMarker(const visualization_msgs::msg::Marker::SharedPtr _msg);
+
+ /**
+ * @brief Create a text marker
+ * @param[in] _msg Marker message
+ */
+ void createTextMarker(const visualization_msgs::msg::Marker::SharedPtr _msg);
+
+ /**
+ * @brief Create mesh marker
+ * @param[in] _msg Marker message
+ */
+ void createMeshMarker(const visualization_msgs::msg::Marker::SharedPtr _msg);
+
+ /**
+ * @brief Create list visual
+ *
+ * Create marker geometry using points array availabe in message.
+ * Handles the following geometry types:
+ * Cube List and Sphere List
+ *
+ * @param[in] _msg Marker message
+ */
+ void createListVisual(const visualization_msgs::msg::Marker::SharedPtr _msg);
+
  /**
  * @brief Create a new material from message
  * @param[in] _color Color message
+ * @return Material Material created using color message
  */
  rendering::MaterialPtr createMaterial(const std_msgs::msg::ColorRGBA & _color);
 
+ /**
+ * @brief Convert pose message
+ * @param[in] _pose Pose message
+ * @return Pose Converted pose
+ */
+ math::Pose3d msgToPose(const geometry_msgs::msg::Pose & _pose);
+
  /**
  * @brief Delete a specific marker from scene
  * @param[in] _id Marker ID

ign_rviz_plugins/package.xml

@@ -21,6 +21,7 @@
  <depend>tf2_msgs</depend>
  <depend>tf2_ros</depend>
  <depend>urdf</depend>
+ <depend>visualization_msgs</depend>
 
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

ign_rviz_plugins/src/rviz/plugins/MarkerManager.cpp

@@ -14,9 +14,13 @@
 
 #include "ignition/rviz/plugins/MarkerManager.hpp"
 
+#include <ament_index_cpp/get_package_prefix.hpp>
+#include <ament_index_cpp/get_package_share_directory.hpp>
 #include <rclcpp/logger.hpp>
 #include <rclcpp/logging.hpp>
 
+#include <string>
+
 namespace ignition
 {
 namespace rviz
@@ -65,19 +69,7 @@ void MarkerManager::createMarker(const visualization_msgs::msg::Marker::SharedPt
 {
  switch (_msg->type) {
  case visualization_msgs::msg::Marker::ARROW: {
- auto visual = this->scene->CreateArrowVisual();
- insertOrUpdateVisual(_msg->id, visual);
-
- visual->SetMaterial(createMaterial(_msg->color));
- visual->SetLocalScale(_msg->scale.x, _msg->scale.y, _msg->scale.z);
-
- math::Pose3d pose(_msg->pose.position.x, _msg->pose.position.y, _msg->pose.position.z,
- _msg->pose.orientation.w, _msg->pose.orientation.x, _msg->pose.orientation.y,
- _msg->pose.orientation.z);
- visual->SetLocalPosition(pose.Pos());
- visual->SetLocalRotation(pose.Rot() * math::Quaterniond(0, 1.57, 0));
-
- this->rootVisual->AddChild(visual);
+ createArrowMarker(_msg);
  break;
  }
  case visualization_msgs::msg::Marker::CUBE: {
@@ -108,17 +100,20 @@ void MarkerManager::createMarker(const visualization_msgs::msg::Marker::SharedPt
  createListGeometry(_msg, rendering::MarkerType::MT_POINTS);
  break;
  }
- // TODO(Sarathkrishnan Ramesh): Add support for following marker types
  case visualization_msgs::msg::Marker::CUBE_LIST: {
+ createListVisual(_msg);
  break;
  }
  case visualization_msgs::msg::Marker::SPHERE_LIST: {
+ createListVisual(_msg);
  break;
  }
  case visualization_msgs::msg::Marker::TEXT_VIEW_FACING: {
+ createTextMarker(_msg);
  break;
  }
  case visualization_msgs::msg::Marker::MESH_RESOURCE: {
+ createMeshMarker(_msg);
  break;
  }
  }
@@ -142,11 +137,7 @@ void MarkerManager::createBasicGeometry(
  // Add geometry and set scale
  visual->AddGeometry(marker);
  visual->SetLocalScale(_msg->scale.x, _msg->scale.y, _msg->scale.z);
- visual->SetLocalPose(
- math::Pose3d(
- _msg->pose.position.x, _msg->pose.position.y, _msg->pose.position.z,
- _msg->pose.orientation.w, _msg->pose.orientation.x, _msg->pose.orientation.y,
- _msg->pose.orientation.z));
+ visual->SetLocalPose(msgToPose(_msg->pose));
 
  this->rootVisual->AddChild(visual);
 }
@@ -172,7 +163,7 @@ void MarkerManager::createListGeometry(
  } else {
  if (_msg->colors.size() != 0) {
  RCLCPP_WARN(
- rclcpp::get_logger("MarkerManager"), "Marker color and point array size doesn't.");
+ rclcpp::get_logger("MarkerManager"), "Marker color and point array size doesn't match.");
  }
  const auto color = math::Color(_msg->color.r, _msg->color.g, _msg->color.b, _msg->color.a);
  for (const auto & point : _msg->points) {
@@ -185,11 +176,140 @@ void MarkerManager::createListGeometry(
  marker->SetMaterial(this->scene->Material("Default/TransGreen"));
 
  visual->AddGeometry(marker);
- visual->SetLocalPose(
- math::Pose3d(
- _msg->pose.position.x, _msg->pose.position.y, _msg->pose.position.z,
- _msg->pose.orientation.w, _msg->pose.orientation.x, _msg->pose.orientation.y,
- _msg->pose.orientation.z));
+ visual->SetLocalPose(msgToPose(_msg->pose));
+
+ this->rootVisual->AddChild(visual);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void MarkerManager::createArrowMarker(const visualization_msgs::msg::Marker::SharedPtr _msg)
+{
+ auto visual = this->scene->CreateArrowVisual();
+ insertOrUpdateVisual(_msg->id, visual);
+
+ visual->SetMaterial(createMaterial(_msg->color));
+ visual->SetLocalScale(_msg->scale.x, _msg->scale.y, _msg->scale.z);
+
+ math::Pose3d pose = msgToPose(_msg->pose);
+ visual->SetLocalPosition(pose.Pos());
+ visual->SetLocalRotation(pose.Rot() * math::Quaterniond(0, 1.57, 0));
+
+ this->rootVisual->AddChild(visual);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void MarkerManager::createTextMarker(const visualization_msgs::msg::Marker::SharedPtr _msg)
+{
+ rendering::VisualPtr visual = this->scene->CreateVisual();
+ insertOrUpdateVisual(_msg->id, visual);
+
+ // Create text marker
+ auto textMarker = this->scene->CreateText();
+ textMarker->SetTextString(_msg->text);
+ textMarker->SetShowOnTop(true);
+ textMarker->SetTextAlignment(
+ rendering::TextHorizontalAlign::CENTER,
+ rendering::TextVerticalAlign::CENTER);
+ textMarker->SetCharHeight(0.15);
+ textMarker->SetMaterial(createMaterial(_msg->color));
+
+ // Add geometry and set scale
+ visual->AddGeometry(textMarker);
+ visual->SetLocalScale(_msg->scale.x, _msg->scale.y, _msg->scale.z);
+
+ visual->SetLocalPose(msgToPose(_msg->pose));
+
+ this->rootVisual->AddChild(visual);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void MarkerManager::createMeshMarker(const visualization_msgs::msg::Marker::SharedPtr _msg)
+{
+ rendering::MeshDescriptor descriptor;
+
+ if (_msg->mesh_resource.rfind("package://") == 0) {
+ int p = _msg->mesh_resource.find_first_of('/', 10);
+ auto package_name = _msg->mesh_resource.substr(10, p - 10);
+
+ try {
+ std::string filepath = ament_index_cpp::get_package_share_directory(package_name);
+ filepath += _msg->mesh_resource.substr(p);
+ descriptor.meshName = filepath;
+ } catch (ament_index_cpp::PackageNotFoundError & e) {
+ RCLCPP_ERROR(rclcpp::get_logger("MarkerManager"), e.what());
+ return;
+ }
+ } else if (_msg->mesh_resource.rfind("file://") == 0) {
+ descriptor.meshName = _msg->mesh_resource.substr(7);
+ } else {
+ RCLCPP_ERROR(
+ rclcpp::get_logger(
+ "MarkerManager"), "Unable to find file %s", _msg->mesh_resource.c_str());
+ return;
+ }
+
+ // Load Mesh
+ ignition::common::MeshManager * meshManager = ignition::common::MeshManager::Instance();
+ descriptor.mesh = meshManager->Load(descriptor.meshName);
+
+ // Error loading mesh
+ if (descriptor.mesh == nullptr) {
+ return;
+ }
+
+ rendering::MeshPtr mesh = this->scene->CreateMesh(descriptor);
+
+ rendering::VisualPtr visual = this->scene->CreateVisual();
+ insertOrUpdateVisual(_msg->id, visual);
+
+ if (!_msg->mesh_use_embedded_materials) {
+ mesh->SetMaterial(createMaterial(_msg->color));
+ }
+
+ visual->AddGeometry(mesh);
+ visual->SetLocalScale(_msg->scale.x, _msg->scale.y, _msg->scale.z);
+
+ visual->SetLocalPose(msgToPose(_msg->pose));
+
+ this->rootVisual->AddChild(visual);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void MarkerManager::createListVisual(const visualization_msgs::msg::Marker::SharedPtr _msg)
+{
+ rendering::VisualPtr visual = this->scene->CreateVisual();
+ insertOrUpdateVisual(_msg->id, visual);
+
+ if (_msg->colors.size() == _msg->points.size()) {
+ for (unsigned int i = 0; i < _msg->points.size(); ++i) {
+ auto geometry = (_msg->type == visualization_msgs::msg::Marker::CUBE_LIST) ?
+ this->scene->CreateBox() : this->scene->CreateSphere();
+
+ geometry->SetMaterial(createMaterial(_msg->colors[i]));
+ auto marker = this->scene->CreateVisual();
+ marker->SetLocalPosition(_msg->points[i].x, _msg->points[i].y, _msg->points[i].z);
+ marker->SetLocalScale(_msg->scale.x, _msg->scale.y, _msg->scale.z);
+ marker->AddGeometry(geometry);
+
+ visual->AddChild(marker);
+ }
+ } else {
+ auto mat = createMaterial(_msg->color);
+ for (const auto & point : _msg->points) {
+ auto geometry = (_msg->type == visualization_msgs::msg::Marker::CUBE_LIST) ?
+ this->scene->CreateBox() : this->scene->CreateSphere();
+
+ geometry->SetMaterial(mat, false);
+ auto marker = this->scene->CreateVisual();
+ marker->SetLocalPosition(point.x, point.y, point.z);
+ marker->SetLocalScale(_msg->scale.x, _msg->scale.y, _msg->scale.z);
+ marker->AddGeometry(geometry);
+
+ visual->AddChild(marker);
+ }
+ }
+
+ visual->SetLocalPose(msgToPose(_msg->pose));
 
  this->rootVisual->AddChild(visual);
 }
@@ -205,6 +325,14 @@ rendering::MaterialPtr MarkerManager::createMaterial(const std_msgs::msg::ColorR
  return mat;
 }
 
+////////////////////////////////////////////////////////////////////////////////
+math::Pose3d MarkerManager::msgToPose(const geometry_msgs::msg::Pose & _pose)
+{
+ return math::Pose3d(
+ _pose.position.x, _pose.position.y, _pose.position.z,
+ _pose.orientation.w, _pose.orientation.x, _pose.orientation.y, _pose.orientation.z);
+}
+
 ////////////////////////////////////////////////////////////////////////////////
 void MarkerManager::insertOrUpdateVisual(unsigned int _id, rendering::VisualPtr _visual)
 {

ign_rviz_plugins/src/rviz/plugins/RobotModelDisplay.cpp

@@ -458,7 +458,7 @@ rendering::VisualPtr RobotModelDisplay::createLinkGeometry(
  } else if (meshInfo->filename.rfind("file://") == 0) {
  // Load Mesh
  rendering::MeshDescriptor descriptor;
- descriptor.meshName = meshInfo->filename.substr(6);
+ descriptor.meshName = meshInfo->filename.substr(7);
  ignition::common::MeshManager * meshManager = ignition::common::MeshManager::Instance();
  descriptor.mesh = meshManager->Load(descriptor.meshName);
  rendering::MeshPtr mesh = this->scene->CreateMesh(descriptor);