Scenebroadcaster sensors (#698)

* Add sensors to scene broadcaster Signed-off-by: Nate Koenig <[email protected]> * Update src/systems/scene_broadcaster/SceneBroadcaster.cc Co-authored-by: Michael Carroll <[email protected]> * Fix codecheck Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]> Co-authored-by: Michael Carroll <[email protected]>
gazebosim · Mar 22, 2021 · f422c02 · f422c02
1 parent dc52dfc
commit f422c02
Show file tree

Hide file tree

Showing 3 changed files with 193 additions and 0 deletions.
diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc
@@ -38,6 +38,7 @@
 #include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
 #include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/Sensor.hh"
 #include "ignition/gazebo/components/Static.hh"
 #include "ignition/gazebo/components/Visual.hh"
 #include "ignition/gazebo/components/World.hh"
@@ -124,6 +125,15 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate
   public: static void AddVisuals(msgs::Link *_msg, const Entity _entity,
                                  const SceneGraphType &_graph);
 
+  /// \brief Adds sensors to a msgs::Link object based on the contents of
+  /// the scene graph
+  /// \param[inout] _msg Pointer to msg object to which the sensors will be
+  /// added.
+  /// \param[in] _entity Parent entity in the graph
+  /// \param[in] _graph Scene graph
+  public: static void AddSensors(msgs::Link *_msg, const Entity _entity,
+                                 const SceneGraphType &_graph);
+
   /// \brief Recursively remove entities from the graph
   /// \param[in] _entity Entity
   /// \param[in/out] _graph Scene graph
@@ -734,6 +744,26 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities(
         return true;
       });
 
+  // Sensors
+  _manager.EachNew<components::Sensor, components::Name,
+                   components::ParentEntity, components::Pose>(
+      [&](const Entity &_entity, const components::Sensor *,
+          const components::Name *_nameComp,
+          const components::ParentEntity *_parentComp,
+          const components::Pose *_poseComp) -> bool
+      {
+        auto sensorMsg = std::make_shared<msgs::Sensor>();
+        sensorMsg->set_id(_entity);
+        sensorMsg->set_parent_id(_parentComp->Data());
+        sensorMsg->set_name(_nameComp->Data());
+        sensorMsg->mutable_pose()->CopyFrom(msgs::Convert(_poseComp->Data()));
+
+        // Add to graph
+        newGraph.AddVertex(_nameComp->Data(), sensorMsg, _entity);
+        newGraph.AddEdge({_parentComp->Data(), _entity}, true);
+        newEntity = true;
+        return true;
+      });
 
   // Update the whole scene graph from the new graph
   {
@@ -878,6 +908,24 @@ void SceneBroadcasterPrivate::AddVisuals(msgs::Link *_msg, const Entity _entity,
   }
 }
 
+//////////////////////////////////////////////////
+void SceneBroadcasterPrivate::AddSensors(msgs::Link *_msg, const Entity _entity,
+    const SceneGraphType &_graph)
+{
+  if (!_msg)
+    return;
+
+  for (const auto &vertex : _graph.AdjacentsFrom(_entity))
+  {
+    auto sensorMsg = std::dynamic_pointer_cast<msgs::Sensor>(
+        vertex.second.get().Data());
+    if (!sensorMsg)
+      continue;
+
+    _msg->add_sensor()->CopyFrom(*sensorMsg);
+  }
+}
+
 //////////////////////////////////////////////////
 void SceneBroadcasterPrivate::AddLinks(msgs::Model *_msg, const Entity _entity,
                                        const SceneGraphType &_graph)
@@ -900,6 +948,9 @@ void SceneBroadcasterPrivate::AddLinks(msgs::Model *_msg, const Entity _entity,
 
     // Lights
     AddLights(msgOut, vertex.second.get().Id(), _graph);
+
+    // Sensors
+    AddSensors(msgOut, vertex.second.get().Id(), _graph);
   }
 }
 

diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc
@@ -220,6 +220,57 @@ TEST_P(SceneBroadcasterTest, SceneTopic)
   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(msg, scene));
 }
 
+/////////////////////////////////////////////////
+/// Test whether the scene topic is published only when new entities are added
+TEST_P(SceneBroadcasterTest, SceneTopicSensors)
+{
+  // Start server
+  ignition::gazebo::ServerConfig serverConfig;
+  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
+                          "/test/worlds/altimeter_with_pose.sdf");
+
+  gazebo::Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+  EXPECT_EQ(12u, *server.EntityCount());
+
+  // Create requester
+  transport::Node node;
+
+  std::vector<msgs::Scene> sceneMsgs;
+  std::function<void(const msgs::Scene &)> collectMsgs =
+      [&sceneMsgs](const msgs::Scene &_msg)
+      {
+        sceneMsgs.push_back(_msg);
+      };
+
+  node.Subscribe("/world/altimeter_sensor/scene/info", collectMsgs);
+
+  // Run server
+  server.Run(true, 10, false);
+
+  // Should only have one scene even though the simulation ran multiple times
+  ASSERT_EQ(1u, sceneMsgs.size());
+
+  // Compare this scene with one from a service request
+  msgs::Scene &scene = sceneMsgs.front();
+
+  bool result{false};
+  unsigned int timeout{5000};
+  ignition::msgs::Scene msg;
+
+  EXPECT_TRUE(node.Request("/world/altimeter_sensor/scene/info",
+        timeout, msg, result));
+  EXPECT_TRUE(result);
+  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(msg, scene));
+
+  EXPECT_EQ(1, msg.model(1).link(0).sensor_size());
+  EXPECT_EQ("altimeter_sensor", msg.model(1).link(0).sensor(0).name());
+  EXPECT_DOUBLE_EQ(0.1, msg.model(1).link(0).sensor(0).pose().position().x());
+  EXPECT_DOUBLE_EQ(0.2, msg.model(1).link(0).sensor(0).pose().position().y());
+  EXPECT_DOUBLE_EQ(0.3, msg.model(1).link(0).sensor(0).pose().position().z());
+}
+
 /////////////////////////////////////////////////
 /// Test whether the scene topic is published only when new entities are added
 TEST_P(SceneBroadcasterTest, DeletedTopic)

diff --git a/test/worlds/altimeter_with_pose.sdf b/test/worlds/altimeter_with_pose.sdf
@@ -0,0 +1,91 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="altimeter_sensor">
+    <physics name="1ms" type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-altimeter-system"
+      name="ignition::gazebo::systems::Altimeter">
+    </plugin>
+    <plugin
+      filename="libignition-gazebo-scene-broadcaster-system.so"
+      name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="altimeter_model">
+      <pose>4 0 3.0 0 0.0 3.14</pose>
+      <link name="link">
+        <pose>0.05 0.05 0.05 0 0 0</pose>
+        <inertial>
+          <mass>0.1</mass>
+          <inertia>
+            <ixx>0.000166667</ixx>
+            <iyy>0.000166667</iyy>
+            <izz>0.000166667</izz>
+          </inertia>
+        </inertial>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+        <sensor name="altimeter_sensor" type="altimeter">
+          <pose>0.1 0.2 0.3 0 0 0</pose>
+          <always_on>1</always_on>
+          <update_rate>30</update_rate>
+          <visualize>true</visualize>
+        </sensor>
+      </link>
+
+      <plugin
+        filename="ignition-gazebo-kinetic-energy-monitor-system"
+        name="ignition::gazebo::systems::KineticEnergyMonitor">
+        <link_name>link</link_name>
+        <kinetic_energy_threshold>2</kinetic_energy_threshold>
+      </plugin>
+    </model>
+
+  </world>
+</sdf>