Sensor topics available through components and GUI

include/ignition/gazebo/components/Sensor.hh

@@ -17,8 +17,10 @@
 #ifndef IGNITION_GAZEBO_COMPONENTS_SENSOR_HH_
 #define IGNITION_GAZEBO_COMPONENTS_SENSOR_HH_
 
+#include <string>
 #include <ignition/gazebo/components/Factory.hh>
 #include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Serialization.hh>
 #include <ignition/gazebo/config.hh>
 
 namespace ignition
@@ -32,6 +34,15 @@ namespace components
   /// \brief A component that identifies an entity as being a link.
   using Sensor = Component<NoData, class SensorTag>;
   IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Sensor", Sensor)
+
+  /// \brief Name of the transport topic where a sensor is publishing its
+  /// data.
+  /// For sensors that publish on more than one topic, this will usually be the
+  /// prefix common to all topics of that sensor.
+  using SensorTopic = Component<std::string, class SensorTopicTag,
+      serializers::StringSerializer>;
+  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.SensorTopic",
+      SensorTopic)
 }
 }
 }

include/ignition/gazebo/components/Serialization.hh

@@ -19,6 +19,7 @@
 
 #include <ignition/msgs/double_v.pb.h>
 
+#include <string>
 #include <vector>
 #include <sdf/Sensor.hh>
 
@@ -104,7 +105,7 @@ namespace serializers
   {
     /// \brief Serialization
     /// \param[in] _out Output stream.
-    /// \param[in] _geometry Geometry to stream
+    /// \param[in] _vec Vector to stream
     /// \return The stream.
     public: static std::ostream &Serialize(std::ostream &_out,
                                            const std::vector<double> &_vec)
@@ -155,6 +156,32 @@ namespace serializers
       return _in;
     }
   };
+
+  /// \brief Serializer for components that hold std::string.
+  class StringSerializer
+  {
+    /// \brief Serialization
+    /// \param[in] _out Output stream.
+    /// \param[in] _data Data to serialize.
+    /// \return The stream.
+    public: static std::ostream &Serialize(std::ostream &_out,
+        const std::string &_data)
+    {
+      _out << _data;
+      return _out;
+    }
+
+    /// \brief Deserialization
+    /// \param[in] _in Input stream.
+    /// \param[in] _data Data to populate.
+    /// \return The stream.
+    public: static std::istream &Deserialize(std::istream &_in,
+        std::string &_data)
+    {
+      _data = std::string(std::istreambuf_iterator<char>(_in), {});
+      return _in;
+    }
+  };
 }
 }
 }

src/gui/plugins/component_inspector/ComponentInspector.cc

@@ -499,6 +499,13 @@ void ComponentInspector::Update(const UpdateInfo &,
       if (comp)
         setData(item, comp->Data());
     }
+    else if (typeId == components::SensorTopic::typeId)
+    {
+      auto comp =
+          _ecm.Component<components::SensorTopic>(this->dataPtr->entity);
+      if (comp)
+        setData(item, comp->Data());
+    }
     else if (typeId == components::WindMode::typeId)
     {
       auto comp = _ecm.Component<components::WindMode>(this->dataPtr->entity);

src/systems/air_pressure/AirPressure.cc

@@ -31,6 +31,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/EntityComponentManager.hh"
 #include "ignition/gazebo/Util.hh"
 
@@ -136,6 +137,13 @@ void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm)
         std::unique_ptr<sensors::AirPressureSensor> sensor =
             this->sensorFactory.CreateSensor<
             sensors::AirPressureSensor>(data);
+        if (nullptr == sensor)
+        {
+          ignerr << "Failed to create sensor [" << sensorScopedName << "]"
+                 << std::endl;
+          return true;
+        }
+
         // set sensor parent
         std::string parentName = _ecm.Component<components::Name>(
             _parent->Data())->Data();
@@ -147,6 +155,9 @@ void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm)
         math::Pose3d sensorWorldPose = worldPose(_entity, _ecm);
         sensor->SetPose(sensorWorldPose);
 
+        // Set topic
+        _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));
+
         this->entitySensorMap.insert(
             std::make_pair(_entity, std::move(sensor)));
 

src/systems/altimeter/Altimeter.cc

@@ -33,6 +33,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/World.hh"
 #include "ignition/gazebo/EntityComponentManager.hh"
 #include "ignition/gazebo/Util.hh"
@@ -138,6 +139,13 @@ void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm)
         std::unique_ptr<sensors::AltimeterSensor> sensor =
             this->sensorFactory.CreateSensor<
             sensors::AltimeterSensor>(data);
+        if (nullptr == sensor)
+        {
+          ignerr << "Failed to create sensor [" << sensorScopedName << "]"
+                 << std::endl;
+          return true;
+        }
+
         // set sensor parent
         std::string parentName = _ecm.Component<components::Name>(
             _parent->Data())->Data();
@@ -150,6 +158,9 @@ void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm)
         sensor->SetVerticalReference(verticalReference);
         sensor->SetPosition(verticalReference);
 
+        // Set topic
+        _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));
+
         this->entitySensorMap.insert(
             std::make_pair(_entity, std::move(sensor)));
 

src/systems/imu/Imu.cc

@@ -33,6 +33,7 @@
 #include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/Pose.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
+#include "ignition/gazebo/components/Sensor.hh"
 #include "ignition/gazebo/components/World.hh"
 #include "ignition/gazebo/EntityComponentManager.hh"
 #include "ignition/gazebo/Util.hh"
@@ -157,6 +158,13 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
         std::unique_ptr<sensors::ImuSensor> sensor =
             this->sensorFactory.CreateSensor<
             sensors::ImuSensor>(data);
+        if (nullptr == sensor)
+        {
+          ignerr << "Failed to create sensor [" << sensorScopedName << "]"
+                 << std::endl;
+          return true;
+        }
+
         // set sensor parent
         std::string parentName = _ecm.Component<components::Name>(
             _parent->Data())->Data();
@@ -171,6 +179,9 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
         math::Pose3d p = worldPose(_entity, _ecm);
         sensor->SetOrientationReference(p.Rot());
 
+        // Set topic
+        _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));
+
         this->entitySensorMap.insert(
             std::make_pair(_entity, std::move(sensor)));
 

src/systems/logical_camera/LogicalCamera.cc

@@ -33,6 +33,7 @@
 #include "ignition/gazebo/components/Model.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
 #include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/Sensor.hh"
 #include "ignition/gazebo/components/World.hh"
 #include "ignition/gazebo/EntityComponentManager.hh"
 #include "ignition/gazebo/Util.hh"
@@ -140,6 +141,13 @@ void LogicalCameraPrivate::CreateLogicalCameraEntities(
         std::unique_ptr<sensors::LogicalCameraSensor> sensor =
             this->sensorFactory.CreateSensor<
             sensors::LogicalCameraSensor>(data);
+        if (nullptr == sensor)
+        {
+          ignerr << "Failed to create sensor [" << sensorScopedName << "]"
+                 << std::endl;
+          return true;
+        }
+
         // set sensor parent
         std::string parentName = _ecm.Component<components::Name>(
             _parent->Data())->Data();
@@ -149,6 +157,9 @@ void LogicalCameraPrivate::CreateLogicalCameraEntities(
         math::Pose3d sensorWorldPose = worldPose(_entity, _ecm);
         sensor->SetPose(sensorWorldPose);
 
+        // Set topic
+        _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));
+
         this->entitySensorMap.insert(
             std::make_pair(_entity, std::move(sensor)));
 

src/systems/magnetometer/Magnetometer.cc

@@ -31,6 +31,7 @@
 #include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/Pose.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
+#include "ignition/gazebo/components/Sensor.hh"
 #include "ignition/gazebo/components/World.hh"
 #include "ignition/gazebo/EntityComponentManager.hh"
 #include "ignition/gazebo/Util.hh"
@@ -153,6 +154,13 @@ void MagnetometerPrivate::CreateMagnetometerEntities(
         std::unique_ptr<sensors::MagnetometerSensor> sensor =
             this->sensorFactory.CreateSensor<
             sensors::MagnetometerSensor>(data);
+        if (nullptr == sensor)
+        {
+          ignerr << "Failed to create sensor [" << sensorScopedName << "]"
+                 << std::endl;
+          return true;
+        }
+
         // set sensor parent
         std::string parentName = _ecm.Component<components::Name>(
             _parent->Data())->Data();
@@ -168,6 +176,9 @@ void MagnetometerPrivate::CreateMagnetometerEntities(
         math::Pose3d p = worldPose(_entity, _ecm);
         sensor->SetWorldPose(p);
 
+        // Set topic
+        _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));
+
         this->entitySensorMap.insert(
             std::make_pair(_entity, std::move(sensor)));
 

test/integration/air_pressure_system.cc

@@ -22,9 +22,14 @@
 #include <ignition/common/Console.hh>
 #include <ignition/transport/Node.hh>
 
+#include "ignition/gazebo/components/AirPressureSensor.hh"
+#include "ignition/gazebo/components/Name.hh"
+#include "ignition/gazebo/components/Sensor.hh"
 #include "ignition/gazebo/Server.hh"
 #include "ignition/gazebo/test_config.hh"
 
+#include "../helpers/Relay.hh"
+
 using namespace ignition;
 using namespace gazebo;
 
@@ -53,6 +58,43 @@ TEST_F(AirPressureTest, AirPressure)
   EXPECT_FALSE(server.Running());
   EXPECT_FALSE(*server.Running(0));
 
+  const std::string sensorName = "air_pressure_sensor";
+
+  auto topic = "world/air_pressure_sensor/model/air_pressure_model/link/link/"
+      "sensor/air_pressure_sensor/air_pressure";
+
+  bool updateChecked{false};
+
+  // Create a system that checks sensor topic
+  test::Relay testSystem;
+  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &,
+                              const gazebo::EntityComponentManager &_ecm)
+      {
+        _ecm.Each<components::AirPressureSensor, components::Name>(
+            [&](const ignition::gazebo::Entity &_entity,
+                const components::AirPressureSensor *,
+                const components::Name *_name) -> bool
+            {
+              EXPECT_EQ(_name->Data(), sensorName);
+
+              auto sensorComp = _ecm.Component<components::Sensor>(_entity);
+              EXPECT_NE(nullptr, sensorComp);
+
+              auto topicComp = _ecm.Component<components::SensorTopic>(_entity);
+              EXPECT_NE(nullptr, topicComp);
+              if (topicComp)
+              {
+                EXPECT_EQ(topic, topicComp->Data());
+              }
+
+              updateChecked = true;
+
+              return true;
+            });
+      });
+
+  server.AddSystem(testSystem.systemPtr);
+
   // Subscribe to air_pressure topic
   bool received{false};
   msgs::FluidPressure msg;
@@ -69,12 +111,11 @@ TEST_F(AirPressureTest, AirPressure)
   };
 
   transport::Node node;
-  node.Subscribe(std::string("/world/air_pressure_sensor/model/") +
-      "air_pressure_model/link/link/sensor/air_pressure_sensor/air_pressure",
-      cb);
+  node.Subscribe(topic, cb);
 
   // Run server
   server.Run(true, 100, false);
+  EXPECT_TRUE(updateChecked);
 
   // Wait for message to be received
   for (int sleep = 0; !received && sleep < 30; ++sleep)

test/integration/altimeter_system.cc

@@ -25,10 +25,11 @@
 #include <ignition/transport/Node.hh>
 
 #include "ignition/gazebo/components/Altimeter.hh"
-#include "ignition/gazebo/components/Name.hh"
+#include "ignition/gazebo/components/LinearVelocity.hh"
 #include "ignition/gazebo/components/Model.hh"
+#include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/Pose.hh"
-#include "ignition/gazebo/components/LinearVelocity.hh"
+#include "ignition/gazebo/components/Sensor.hh"
 #include "ignition/gazebo/Server.hh"
 #include "ignition/gazebo/SystemLoader.hh"
 #include "ignition/gazebo/test_config.hh"
@@ -77,6 +78,9 @@ TEST_F(AltimeterTest, ModelFalling)
 
   const std::string sensorName = "altimeter_sensor";
 
+  auto topic = "world/altimeter_sensor/"
+      "model/altimeter_model/link/link/sensor/altimeter_sensor/altimeter";
+
   // Create a system that records altimeter data
   test::Relay testSystem;
   std::vector<math::Pose3d> poses;
@@ -87,7 +91,7 @@ TEST_F(AltimeterTest, ModelFalling)
         _ecm.Each<components::Altimeter, components::Name,
                   components::WorldPose,
                   components::WorldLinearVelocity>(
-            [&](const ignition::gazebo::Entity &,
+            [&](const ignition::gazebo::Entity &_entity,
                 const components::Altimeter *,
                 const components::Name *_name,
                 const components::WorldPose *_worldPose,
@@ -98,6 +102,16 @@ TEST_F(AltimeterTest, ModelFalling)
               poses.push_back(_worldPose->Data());
               velocities.push_back(_worldLinearVel->Data());
 
+              auto sensorComp = _ecm.Component<components::Sensor>(_entity);
+              EXPECT_NE(nullptr, sensorComp);
+
+              auto topicComp = _ecm.Component<components::SensorTopic>(_entity);
+              EXPECT_NE(nullptr, topicComp);
+              if (topicComp)
+              {
+                EXPECT_EQ(topic, topicComp->Data());
+              }
+
               return true;
             });
       });
@@ -106,9 +120,7 @@ TEST_F(AltimeterTest, ModelFalling)
 
   // subscribe to altimeter topic
   transport::Node node;
-  node.Subscribe(std::string("/world/altimeter_sensor/") +
-      "model/altimeter_model/link/link/sensor/altimeter_sensor/altimeter",
-      &altimeterCb);
+  node.Subscribe(topic, &altimeterCb);
 
   // Run server
   size_t iters100 = 100u;