Added MarkerManager Plugin

Signed-off-by: ahcorde <ahcorde@gmail.com>
gazebosim · Jul 12, 2021 · 43bd730 · 43bd730
1 parent bbad5be
commit 43bd730
Show file tree

Hide file tree

Showing 11 changed files with 1,369 additions and 0 deletions.
diff --git a/examples/standalone/marker/CMakeLists.txt b/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()
diff --git a/examples/standalone/marker/README.md b/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 Gazebo.
+
+## 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 Gazebo's render window.
diff --git a/examples/standalone/marker/marker.cc b/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);
+}