Added MarkerManager Plugin (#247)

Signed-off-by: ahcorde <[email protected]>

examples/config/scene3d.config

@@ -32,6 +32,20 @@
   <deletion_topic>/example/delete</deletion_topic>
   <scene_topic>/example/scene</scene_topic>
 </plugin>
+<plugin filename="MarkerManager" name="Marker Manager">
+  <ignition-gui>
+    <anchors target="View 1">
+      <line own="right" target="right"/>
+      <line own="top" target="top"/>
+    </anchors>
+    <property key="resizable" type="bool">false</property>
+    <property key="width" type="double">5</property>
+    <property key="height" type="double">5</property>
+    <property key="state" type="string">floating</property>
+    <property key="showTitleBar" type="bool">false</property>
+  </ignition-gui>
+  <stats_topic>/example/stats</stats_topic>
+</plugin>
 <plugin filename="CameraTracking" name="Camera tracking">
   <ignition-gui>
     <anchors target="View 1">

examples/standalone/marker/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
+
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+  find_package(ignition-transport11 QUIET REQUIRED OPTIONAL_COMPONENTS log)
+  set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR})
+
+  find_package(ignition-common4 REQUIRED)
+  set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR})
+
+  find_package(ignition-msgs8 REQUIRED)
+  set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR})
+
+  add_executable(marker marker.cc)
+  target_link_libraries(marker
+    ignition-transport${IGN_TRANSPORT_VER}::core
+    ignition-msgs${IGN_MSGS_VER}
+    ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+    )
+endif()

examples/standalone/marker/README.md

@@ -0,0 +1,22 @@
+# Ignition Visualization Marker Example
+
+This example demonstrates how to create, modify, and delete visualization
+markers in Ignition GUI.
+
+## Build Instructions
+
+From this directory:
+
+    mkdir build
+    cd build
+    cmake ..
+    make
+
+## Execute Instructions
+
+Launch ign gazebo unpaused then from the build directory above:
+
+    ./marker
+
+The terminal will output messages indicating visualization changes that
+will occur in Ignition GUI's render window.

examples/standalone/marker/marker.cc

@@ -0,0 +1,315 @@
+/*
+ * Copyright (C) 2019 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <ignition/transport.hh>
+#include <ignition/math.hh>
+#include <ignition/msgs.hh>
+
+#include <iostream>
+
+/////////////////////////////////////////////////
+int main(int _argc, char **_argv)
+{
+  ignition::transport::Node node;
+
+  // Create the marker message
+  ignition::msgs::Marker markerMsg;
+  ignition::msgs::Material matMsg;
+  markerMsg.set_ns("default");
+  markerMsg.set_id(0);
+  markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY);
+  markerMsg.set_type(ignition::msgs::Marker::SPHERE);
+  markerMsg.set_visibility(ignition::msgs::Marker::GUI);
+
+  // Set color to Blue
+  markerMsg.mutable_material()->mutable_ambient()->set_r(0);
+  markerMsg.mutable_material()->mutable_ambient()->set_g(0);
+  markerMsg.mutable_material()->mutable_ambient()->set_b(1);
+  markerMsg.mutable_material()->mutable_ambient()->set_a(1);
+  markerMsg.mutable_material()->mutable_diffuse()->set_r(0);
+  markerMsg.mutable_material()->mutable_diffuse()->set_g(0);
+  markerMsg.mutable_material()->mutable_diffuse()->set_b(1);
+  markerMsg.mutable_material()->mutable_diffuse()->set_a(1);
+  markerMsg.mutable_lifetime()->set_sec(2);
+  markerMsg.mutable_lifetime()->set_nsec(0);
+  ignition::msgs::Set(markerMsg.mutable_scale(),
+                    ignition::math::Vector3d(1.0, 1.0, 1.0));
+
+  // The rest of this function adds different shapes and/or modifies shapes.
+  // Read the terminal statements to figure out what each node.Request
+  // call accomplishes.
+  std::cout << "Spawning a blue sphere with lifetime 2s\n";
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+  ignition::msgs::Set(markerMsg.mutable_pose(),
+                      ignition::math::Pose3d(2, 2, 0, 0, 0, 0));
+  node.Request("/marker", markerMsg);
+  std::cout << "Sleeping for 2 seconds\n";
+  std::this_thread::sleep_for(std::chrono::seconds(2));
+
+  std::cout << "Spawning a black sphere at the origin with no lifetime\n";
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+  markerMsg.set_id(1);
+  ignition::msgs::Set(markerMsg.mutable_pose(),
+                      ignition::math::Pose3d::Zero);
+  markerMsg.mutable_material()->mutable_ambient()->set_b(0);
+  markerMsg.mutable_material()->mutable_diffuse()->set_b(0);
+  markerMsg.mutable_lifetime()->set_sec(0);
+  node.Request("/marker", markerMsg);
+
+  std::cout << "Moving the black sphere to x=0, y=1, z=1\n";
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+  ignition::msgs::Set(markerMsg.mutable_pose(),
+                      ignition::math::Pose3d(0, 1, 1, 0, 0, 0));
+  node.Request("/marker", markerMsg);
+
+  std::cout << "Shrinking the black sphere\n";
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+  ignition::msgs::Set(markerMsg.mutable_scale(),
+                    ignition::math::Vector3d(0.2, 0.2, 0.2));
+  node.Request("/marker", markerMsg);
+
+  std::cout << "Changing the black sphere to red\n";
+  markerMsg.mutable_material()->mutable_ambient()->set_r(1);
+  markerMsg.mutable_material()->mutable_ambient()->set_g(0);
+  markerMsg.mutable_material()->mutable_ambient()->set_b(0);
+  markerMsg.mutable_material()->mutable_diffuse()->set_r(1);
+  markerMsg.mutable_material()->mutable_diffuse()->set_g(0);
+  markerMsg.mutable_material()->mutable_diffuse()->set_b(0);
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+  node.Request("/marker", markerMsg);
+
+  std::cout << "Adding a green ellipsoid\n";
+  markerMsg.mutable_material()->mutable_ambient()->set_r(0);
+  markerMsg.mutable_material()->mutable_ambient()->set_g(1);
+  markerMsg.mutable_material()->mutable_ambient()->set_b(0);
+  markerMsg.mutable_material()->mutable_diffuse()->set_r(0);
+  markerMsg.mutable_material()->mutable_diffuse()->set_g(1);
+  markerMsg.mutable_material()->mutable_diffuse()->set_b(0);
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+  markerMsg.set_id(2);
+  markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY);
+  markerMsg.set_type(ignition::msgs::Marker::SPHERE);
+  ignition::msgs::Set(markerMsg.mutable_scale(),
+                    ignition::math::Vector3d(0.5, 1.0, 1.5));
+  ignition::msgs::Set(markerMsg.mutable_pose(),
+                    ignition::math::Pose3d(2, 0, .5, 0, 0, 0));
+  node.Request("/marker", markerMsg);
+
+  std::cout << "Changing the green ellipsoid to a cylinder\n";
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+  markerMsg.set_type(ignition::msgs::Marker::CYLINDER);
+  ignition::msgs::Set(markerMsg.mutable_scale(),
+                    ignition::math::Vector3d(0.5, 0.5, 1.5));
+  node.Request("/marker", markerMsg);
+
+  std::cout << "Connecting the sphere and cylinder with a line\n";
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+  markerMsg.set_id(3);
+  ignition::msgs::Set(markerMsg.mutable_pose(),
+                    ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
+  markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY);
+  markerMsg.set_type(ignition::msgs::Marker::LINE_LIST);
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(0.0, 1.0, 1.0));
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(2, 0, 0.5));
+  node.Request("/marker", markerMsg);
+
+  std::cout << "Adding a square around the origin\n";
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+  markerMsg.set_id(4);
+  markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY);
+  markerMsg.set_type(ignition::msgs::Marker::LINE_STRIP);
+  ignition::msgs::Set(markerMsg.mutable_point(0),
+      ignition::math::Vector3d(0.5, 0.5, 0.05));
+  ignition::msgs::Set(markerMsg.mutable_point(1),
+      ignition::math::Vector3d(0.5, -0.5, 0.05));
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(-0.5, -0.5, 0.05));
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(-0.5, 0.5, 0.05));
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(0.5, 0.5, 0.05));
+  node.Request("/marker", markerMsg);
+
+  std::cout << "Adding 100 points inside the square\n";
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+  markerMsg.set_id(5);
+  markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY);
+  markerMsg.set_type(ignition::msgs::Marker::POINTS);
+  markerMsg.clear_point();
+  for (int i = 0; i < 100; ++i)
+  {
+    ignition::msgs::Set(markerMsg.add_point(),
+        ignition::math::Vector3d(
+          ignition::math::Rand::DblUniform(-0.49, 0.49),
+          ignition::math::Rand::DblUniform(-0.49, 0.49),
+          0.05));
+  }
+  node.Request("/marker", markerMsg);
+
+  std::cout << "Adding a semi-circular triangle fan\n";
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+  markerMsg.set_id(6);
+  markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY);
+  markerMsg.set_type(ignition::msgs::Marker::TRIANGLE_FAN);
+  markerMsg.clear_point();
+  ignition::msgs::Set(markerMsg.mutable_pose(),
+                    ignition::math::Pose3d(0, 1.5, 0, 0, 0, 0));
+  ignition::msgs::Set(markerMsg.add_point(),
+        ignition::math::Vector3d(0, 0, 0.05));
+  double radius = 2;
+  for (double t = 0; t <= M_PI; t+= 0.01)
+  {
+    ignition::msgs::Set(markerMsg.add_point(),
+        ignition::math::Vector3d(radius * cos(t), radius * sin(t), 0.05));
+  }
+  node.Request("/marker", markerMsg);
+
+  std::cout << "Adding two triangles using a triangle list\n";
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+  markerMsg.set_id(7);
+  markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY);
+  markerMsg.set_type(ignition::msgs::Marker::TRIANGLE_LIST);
+  markerMsg.clear_point();
+  ignition::msgs::Set(markerMsg.mutable_pose(),
+                    ignition::math::Pose3d(0, -1.5, 0, 0, 0, 0));
+  ignition::msgs::Set(markerMsg.add_point(),
+        ignition::math::Vector3d(0, 0, 0.05));
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(1, 0, 0.05));
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(1, 1, 0.05));
+
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(1, 1, 0.05));
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(2, 1, 0.05));
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(2, 2, 0.05));
+
+  node.Request("/marker", markerMsg);
+
+  std::cout << "Adding a rectangular triangle strip\n";
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+  markerMsg.set_id(8);
+  markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY);
+  markerMsg.set_type(ignition::msgs::Marker::TRIANGLE_STRIP);
+  markerMsg.clear_point();
+  ignition::msgs::Set(markerMsg.mutable_pose(),
+                    ignition::math::Pose3d(-2, -2, 0, 0, 0, 0));
+  ignition::msgs::Set(markerMsg.add_point(),
+        ignition::math::Vector3d(0, 0, 0.05));
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(1, 0, 0.05));
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(0, 1, 0.05));
+
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(1, 1, 0.05));
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(0, 2, 0.05));
+  ignition::msgs::Set(markerMsg.add_point(),
+      ignition::math::Vector3d(1, 2, 0.05));
+
+  node.Request("/marker", markerMsg);
+  std::cout << "Adding multiple markers via /marker_array\n";
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+
+  ignition::msgs::Marker_V markerMsgs;
+  ignition::msgs::Boolean res;
+  bool result;
+  unsigned int timeout = 5000;
+
+  // Create first blue sphere marker
+  auto markerMsg1 = markerMsgs.add_marker();
+  markerMsg1->set_ns("default");
+  markerMsg1->set_id(0);
+  markerMsg1->set_action(ignition::msgs::Marker::ADD_MODIFY);
+  markerMsg1->set_type(ignition::msgs::Marker::SPHERE);
+  markerMsg1->set_visibility(ignition::msgs::Marker::GUI);
+
+  // Set color to Blue
+  markerMsg1->mutable_material()->mutable_ambient()->set_r(0);
+  markerMsg1->mutable_material()->mutable_ambient()->set_g(0);
+  markerMsg1->mutable_material()->mutable_ambient()->set_b(1);
+  markerMsg1->mutable_material()->mutable_ambient()->set_a(1);
+  markerMsg1->mutable_material()->mutable_diffuse()->set_r(0);
+  markerMsg1->mutable_material()->mutable_diffuse()->set_g(0);
+  markerMsg1->mutable_material()->mutable_diffuse()->set_b(1);
+  markerMsg1->mutable_material()->mutable_diffuse()->set_a(1);
+  ignition::msgs::Set(markerMsg1->mutable_scale(),
+                    ignition::math::Vector3d(1.0, 1.0, 1.0));
+  ignition::msgs::Set(markerMsg1->mutable_pose(),
+                      ignition::math::Pose3d(3, 3, 0, 0, 0, 0));
+
+  // Create second red box marker
+  auto markerMsg2 = markerMsgs.add_marker();
+  markerMsg2->set_ns("default");
+  markerMsg2->set_id(0);
+  markerMsg2->set_action(ignition::msgs::Marker::ADD_MODIFY);
+  markerMsg2->set_type(ignition::msgs::Marker::BOX);
+  markerMsg2->set_visibility(ignition::msgs::Marker::GUI);
+
+  // Set color to Red
+  markerMsg2->mutable_material()->mutable_ambient()->set_r(1);
+  markerMsg2->mutable_material()->mutable_ambient()->set_g(0);
+  markerMsg2->mutable_material()->mutable_ambient()->set_b(0);
+  markerMsg2->mutable_material()->mutable_ambient()->set_a(1);
+  markerMsg2->mutable_material()->mutable_diffuse()->set_r(1);
+  markerMsg2->mutable_material()->mutable_diffuse()->set_g(0);
+  markerMsg2->mutable_material()->mutable_diffuse()->set_b(0);
+  markerMsg2->mutable_material()->mutable_diffuse()->set_a(1);
+  markerMsg2->mutable_lifetime()->set_sec(2);
+  markerMsg2->mutable_lifetime()->set_nsec(0);
+  ignition::msgs::Set(markerMsg2->mutable_scale(),
+                    ignition::math::Vector3d(1.0, 1.0, 1.0));
+  ignition::msgs::Set(markerMsg2->mutable_pose(),
+                      ignition::math::Pose3d(3, 3, 2, 0, 0, 0));
+
+  // Create green capsule marker
+  auto markerMsg3 = markerMsgs.add_marker();
+  markerMsg3->set_ns("default");
+  markerMsg3->set_id(0);
+  markerMsg3->set_action(ignition::msgs::Marker::ADD_MODIFY);
+  markerMsg3->set_type(ignition::msgs::Marker::CAPSULE);
+  markerMsg3->set_visibility(ignition::msgs::Marker::GUI);
+
+  // Set color to Green
+  markerMsg3->mutable_material()->mutable_ambient()->set_r(0);
+  markerMsg3->mutable_material()->mutable_ambient()->set_g(1);
+  markerMsg3->mutable_material()->mutable_ambient()->set_b(0);
+  markerMsg3->mutable_material()->mutable_ambient()->set_a(1);
+  markerMsg3->mutable_material()->mutable_diffuse()->set_r(0);
+  markerMsg3->mutable_material()->mutable_diffuse()->set_g(1);
+  markerMsg3->mutable_material()->mutable_diffuse()->set_b(0);
+  markerMsg3->mutable_material()->mutable_diffuse()->set_a(1);
+  markerMsg3->mutable_lifetime()->set_sec(2);
+  markerMsg3->mutable_lifetime()->set_nsec(0);
+  ignition::msgs::Set(markerMsg3->mutable_scale(),
+                    ignition::math::Vector3d(1.0, 1.0, 1.0));
+  ignition::msgs::Set(markerMsg3->mutable_pose(),
+                      ignition::math::Pose3d(3, 3, 4, 0, 0, 0));
+
+  // Publish the three created markers above simultaneously
+  node.Request("/marker_array", markerMsgs, timeout, res, result);
+
+  std::cout << "Deleting all the markers\n";
+  std::this_thread::sleep_for(std::chrono::seconds(4));
+  markerMsg.set_action(ignition::msgs::Marker::DELETE_ALL);
+  node.Request("/marker", markerMsg);
+}