Add marker array service (#302)

Signed-off-by: John Shepherd <[email protected]> Signed-off-by: Ian Chen <[email protected]> Co-authored-by: Ian Chen <[email protected]>
gazebosim · Aug 27, 2020 · 2d752ce · 2d752ce
1 parent 4b11a02
commit 2d752ce
Show file tree

Hide file tree

Showing 2 changed files with 106 additions and 0 deletions.
diff --git a/examples/standalone/marker/marker.cc b/examples/standalone/marker/marker.cc
@@ -226,6 +226,86 @@ int main(int _argc, char **_argv)
       ignition::math::Vector3d(1, 2, 0.05));
 
   node.Request("/marker", markerMsg);
+  std::cout << "Adding multiple markers via /marker_array\n";
+  ignition::common::Time::Sleep(ignition::common::Time(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 third green cylinder 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::CYLINDER);
+  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";
   ignition::common::Time::Sleep(ignition::common::Time(4));

diff --git a/src/rendering/MarkerManager.cc b/src/rendering/MarkerManager.cc
@@ -64,6 +64,13 @@ class ignition::gazebo::MarkerManagerPrivate
   /// \param[in] _req The marker message.
   public: void OnMarkerMsg(const ignition::msgs::Marker &_req);
 
+  /// \brief Callback that receives multiple marker messages.
+  /// \param[in] _req The vector of marker messages
+  /// \param[in] _res Response data
+  /// \return True if the request is received
+  public: bool OnMarkerMsgArray(const ignition::msgs::Marker_V &_req,
+              ignition::msgs::Boolean &_res);
+
   /// \brief Services callback that returns a list of markers.
   /// \param[out] _rep Service reply
   /// \return True on success.
@@ -174,6 +181,14 @@ bool MarkerManager::Init(const ignition::rendering::ScenePtr &_scene)
            << " service.\n";
   }
 
+  // Advertise to the marker_array service
+  if (!this->dataPtr->node.Advertise(this->dataPtr->topicName + "_array",
+        &MarkerManagerPrivate::OnMarkerMsgArray, this->dataPtr.get()))
+  {
+    ignerr << "Unable to advertise to the " << this->dataPtr->topicName
+           << "_array service.\n";
+  }
+
   return true;
 }
 
@@ -601,3 +616,14 @@ void MarkerManagerPrivate::OnMarkerMsg(const ignition::msgs::Marker &_req)
   std::lock_guard<std::mutex> lock(this->mutex);
   this->markerMsgs.push_back(_req);
 }
+
+/////////////////////////////////////////////////
+bool MarkerManagerPrivate::OnMarkerMsgArray(
+    const ignition::msgs::Marker_V&_req, ignition::msgs::Boolean &_res)
+{
+  std::lock_guard<std::mutex> lock(this->mutex);
+  std::copy(_req.marker().begin(), _req.marker().end(),
+            std::back_inserter(this->markerMsgs));
+  _res.set_data(true);
+  return true;
+}