diff --git a/include/ignition/gazebo/components/Sensor.hh b/include/ignition/gazebo/components/Sensor.hh index f4eafd6a29..97ca8d74a0 100644 --- a/include/ignition/gazebo/components/Sensor.hh +++ b/include/ignition/gazebo/components/Sensor.hh @@ -17,8 +17,10 @@ #ifndef IGNITION_GAZEBO_COMPONENTS_SENSOR_HH_ #define IGNITION_GAZEBO_COMPONENTS_SENSOR_HH_ +#include #include #include +#include #include namespace ignition @@ -32,6 +34,15 @@ namespace components /// \brief A component that identifies an entity as being a link. using Sensor = Component; 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; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.SensorTopic", + SensorTopic) } } } diff --git a/include/ignition/gazebo/components/Serialization.hh b/include/ignition/gazebo/components/Serialization.hh index 749ca0e467..fd1d958afe 100644 --- a/include/ignition/gazebo/components/Serialization.hh +++ b/include/ignition/gazebo/components/Serialization.hh @@ -19,6 +19,7 @@ #include +#include #include #include @@ -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 &_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(_in), {}); + return _in; + } + }; } } } diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 57faba701f..ee4ce1902d 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/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(this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::WindMode::typeId) { auto comp = _ecm.Component(this->dataPtr->entity); diff --git a/src/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc index 051fdfa196..49c2467aeb 100644 --- a/src/systems/air_pressure/AirPressure.cc +++ b/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 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( _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))); diff --git a/src/systems/altimeter/Altimeter.cc b/src/systems/altimeter/Altimeter.cc index ea75360784..a4aee29a25 100644 --- a/src/systems/altimeter/Altimeter.cc +++ b/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 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( _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))); diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index 09a4aaa66f..f2116d17f4 100644 --- a/src/systems/imu/Imu.cc +++ b/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 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( _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))); diff --git a/src/systems/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc index 263f396084..ac8305943c 100644 --- a/src/systems/logical_camera/LogicalCamera.cc +++ b/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 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( _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))); diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index 523580cb8b..9b2c9b8565 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/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 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( _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))); diff --git a/test/integration/air_pressure_system.cc b/test/integration/air_pressure_system.cc index 9bd751e60d..33ccfc9136 100644 --- a/test/integration/air_pressure_system.cc +++ b/test/integration/air_pressure_system.cc @@ -22,9 +22,14 @@ #include #include +#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( + [&](const ignition::gazebo::Entity &_entity, + const components::AirPressureSensor *, + const components::Name *_name) -> bool + { + EXPECT_EQ(_name->Data(), sensorName); + + auto sensorComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, sensorComp); + + auto topicComp = _ecm.Component(_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) diff --git a/test/integration/altimeter_system.cc b/test/integration/altimeter_system.cc index e295358111..ddde3b23f8 100644 --- a/test/integration/altimeter_system.cc +++ b/test/integration/altimeter_system.cc @@ -25,10 +25,11 @@ #include #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 poses; @@ -87,7 +91,7 @@ TEST_F(AltimeterTest, ModelFalling) _ecm.Each( - [&](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(_entity); + EXPECT_NE(nullptr, sensorComp); + + auto topicComp = _ecm.Component(_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; diff --git a/test/integration/imu_system.cc b/test/integration/imu_system.cc index 746b196e89..a7b439bbe7 100644 --- a/test/integration/imu_system.cc +++ b/test/integration/imu_system.cc @@ -26,9 +26,10 @@ #include "ignition/gazebo/components/AngularVelocity.hh" #include "ignition/gazebo/components/Gravity.hh" #include "ignition/gazebo/components/Imu.hh" -#include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/LinearAcceleration.hh" +#include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" @@ -84,6 +85,9 @@ TEST_F(ImuTest, ModelFalling) const std::string sensorName = "imu_sensor"; + auto topic = + "world/imu_sensor/model/imu_model/link/link/sensor/imu_sensor/imu"; + // Create a system that records imu data test::Relay testSystem; math::Vector3d worldGravity; @@ -98,7 +102,7 @@ TEST_F(ImuTest, ModelFalling) components::WorldPose, components::AngularVelocity, components::LinearAcceleration>( - [&](const ignition::gazebo::Entity &, + [&](const ignition::gazebo::Entity &_entity, const components::Imu *, const components::Name *_name, const components::WorldPose *_worldPose, @@ -111,6 +115,16 @@ TEST_F(ImuTest, ModelFalling) accelerations.push_back(_linearAcc->Data()); angularVelocities.push_back(_angularVel->Data()); + auto sensorComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, sensorComp); + + auto topicComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic, topicComp->Data()); + } + return true; }); @@ -130,9 +144,7 @@ TEST_F(ImuTest, ModelFalling) // subscribe to imu topic transport::Node node; - node.Subscribe( - "/world/imu_sensor/model/imu_model/link/link/sensor/imu_sensor/imu", - &imuCb); + node.Subscribe(topic, &imuCb); // step world and verify imu's linear acceleration is zero on free fall // Run server diff --git a/test/integration/logical_camera_system.cc b/test/integration/logical_camera_system.cc index 872977f286..2dc0241419 100644 --- a/test/integration/logical_camera_system.cc +++ b/test/integration/logical_camera_system.cc @@ -24,13 +24,14 @@ #include #include "ignition/gazebo/components/LogicalCamera.hh" -#include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" #include "ignition/gazebo/test_config.hh" -#include "plugins/MockSystem.hh" +#include "../helpers/Relay.hh" using namespace ignition; using namespace gazebo; @@ -72,10 +73,45 @@ TEST_F(LogicalCameraTest, LogicalCameraBox) EXPECT_FALSE(server.Running()); EXPECT_FALSE(*server.Running(0)); + const std::string sensorName = "logical_camera"; + + auto topic = "world/logical_camera_sensor/model/logical_camera/link/" + "logical_camera_link/sensor/logical_camera/logical_camera"; + + bool updateChecked{false}; + + // Create a system that checks sensor topic + test::Relay testSystem; + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::LogicalCamera *, + const components::Name *_name) -> bool + { + EXPECT_EQ(_name->Data(), sensorName); + + auto sensorComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, sensorComp); + + auto topicComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic, topicComp->Data()); + } + + updateChecked = true; + + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + // subscribe to logical camera topic transport::Node node; - node.Subscribe("/logical_camera", - &logicalCameraCb); node.Subscribe(std::string("/world/logical_camera_sensor/") + "model/logical_camera/link/logical_camera_link" + "/sensor/logical_camera/logical_camera", &logicalCameraCb); @@ -83,6 +119,7 @@ TEST_F(LogicalCameraTest, LogicalCameraBox) // Run server and verify that we are receiving messages size_t iters100 = 100u; server.Run(true, iters100, false); + EXPECT_TRUE(updateChecked); mutex.lock(); EXPECT_GT(logicalCameraMsgs.size(), 0u); mutex.unlock(); diff --git a/test/integration/magnetometer_system.cc b/test/integration/magnetometer_system.cc index 7016fd321a..cf55eec35c 100644 --- a/test/integration/magnetometer_system.cc +++ b/test/integration/magnetometer_system.cc @@ -23,10 +23,11 @@ #include #include -#include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/MagneticField.hh" #include "ignition/gazebo/components/Magnetometer.hh" +#include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" @@ -78,6 +79,9 @@ TEST_F(MagnetometerTest, RotatedMagnetometer) const std::string sensorName = "magnetometer_sensor"; + auto topic = "world/magnetometer_sensor/model/magnetometer_model/link/link/" + "sensor/magnetometer_sensor/magnetometer"; + // Create a system that records magnetometer data test::Relay testSystem; @@ -88,7 +92,7 @@ TEST_F(MagnetometerTest, RotatedMagnetometer) _ecm.Each( - [&](const ignition::gazebo::Entity &, + [&](const ignition::gazebo::Entity &_entity, const components::Magnetometer *, const components::Name *_name, const components::WorldPose *_worldPose) -> bool @@ -96,6 +100,16 @@ TEST_F(MagnetometerTest, RotatedMagnetometer) EXPECT_EQ(_name->Data(), sensorName); poses.push_back(_worldPose->Data()); + auto sensorComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, sensorComp); + + auto topicComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic, topicComp->Data()); + } + return true; }); }); @@ -104,10 +118,7 @@ TEST_F(MagnetometerTest, RotatedMagnetometer) // subscribe to magnetometer topic transport::Node node; - node.Subscribe( - "/world/magnetometer_sensor/model/magnetometer_model/link/link/" - "sensor/magnetometer_sensor/magnetometer", - &magnetometerCb); + node.Subscribe(topic, &magnetometerCb); // step world and verify magnetometer's detected field // Run server