diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 1dc10990c6..096fda5417 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -79,6 +79,7 @@ #include "ignition/gazebo/components/Scene.hh" #include "ignition/gazebo/components/SegmentationCamera.hh" #include "ignition/gazebo/components/SemanticLabel.hh" +#include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/Temperature.hh" #include "ignition/gazebo/components/TemperatureRange.hh" @@ -288,6 +289,9 @@ class ignition::gazebo::RenderUtilPrivate public: std::unordered_map newParticleEmittersCmds; + /// \brief New sensor topics that should be added to ECM as new components + public: std::unordered_map newSensorTopics; + /// \brief A list of entities with particle emitter cmds to remove public: std::vector particleCmdsToRemove; @@ -759,6 +763,16 @@ void RenderUtil::UpdateECM(const UpdateInfo &/*_info*/, _ecm.RemoveComponent(entity); } } + + // create sensor topic components + { + for (const auto &it : this->dataPtr->newSensorTopics) + { + // Set topic + _ecm.CreateComponent(it.first, components::SensorTopic(it.second)); + } + this->dataPtr->newSensorTopics.clear(); + } } ////////////////////////////////////////////////// @@ -1629,6 +1643,7 @@ void RenderUtilPrivate::AddNewSensor(const EntityComponentManager &_ecm, { sdfDataCopy.SetTopic(scopedName(_entity, _ecm) + _topicSuffix); } + this->newSensorTopics[_entity] = sdfDataCopy.Topic(); this->newSensors.push_back( std::make_tuple(_entity, std::move(sdfDataCopy), _parent)); this->sensorEntities.insert(_entity); diff --git a/test/integration/sensors_system.cc b/test/integration/sensors_system.cc index b843da142b..673f3bd6d5 100644 --- a/test/integration/sensors_system.cc +++ b/test/integration/sensors_system.cc @@ -94,6 +94,39 @@ void testSensorEntityIds(const gazebo::EntityComponentManager &_ecm, } } +///////////////////////////////////////////////// +void testSensorTopicComponents(const gazebo::EntityComponentManager &_ecm, + const std::unordered_set &_ids, + const std::vector &_topics) +{ + EXPECT_FALSE(_ids.empty()); + for (const auto & id : _ids) + { + auto sensorTopicComp = _ecm.Component(id); + EXPECT_TRUE(sensorTopicComp); + std::string topicStr = "/" + sensorTopicComp->Data(); + EXPECT_FALSE(topicStr.empty()); + + // verify that the topic string stored in sensor topic component + // exits in the list of topics + // For rendering sensors, they may advertize more than one topics but + // the the sensor topic component will only contain one of them, e.g. + // * /image - stored in sensor topic component + // * /camera_info + bool foundTopic = false; + for (auto it = _topics.begin(); it != _topics.end(); ++it) + { + std::string topic = *it; + if (topic.find(topicStr) == 0u) + { + foundTopic = true; + break; + } + } + EXPECT_TRUE(foundTopic); + } +} + ////////////////////////////////////////////////// class SensorsFixture : public InternalFixture> { @@ -118,21 +151,10 @@ class SensorsFixture : public InternalFixture> }; ////////////////////////////////////////////////// -void testDefaultTopics() +void testDefaultTopics(const std::vector &_topics) { // TODO(anyone) This should be a new test, but running multiple tests with // sensors is not currently working - std::string prefix{"/world/camera_sensor/model/default_topics/"}; - std::vector topics{ - prefix + "link/camera_link/sensor/camera/image", - prefix + "link/camera_link/sensor/camera/camera_info", - prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan", - prefix + "link/depth_camera_link/sensor/depth_camera/depth_image", - prefix + "link/depth_camera_link/sensor/depth_camera/camera_info", - prefix + "link/rgbd_camera_link/sensor/rgbd_camera/image", - prefix + "link/rgbd_camera_link/sensor/rgbd_camera/depth_image" - }; - std::vector publishers; transport::Node node; @@ -140,14 +162,14 @@ void testDefaultTopics() // time int sleep{0}; int maxSleep{30}; - for (; sleep < maxSleep && !node.TopicInfo(topics.front(), publishers); + for (; sleep < maxSleep && !node.TopicInfo(_topics.front(), publishers); ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } ASSERT_LT(sleep, maxSleep); - for (const std::string &topic : topics) + for (const std::string &topic : _topics) { bool result = node.TopicInfo(topic, publishers); @@ -199,9 +221,30 @@ TEST_F(SensorsFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(HandleRemovedEntities)) server.Run(true, 50, false); ASSERT_NE(nullptr, ecm); - testDefaultTopics(); + std::string prefix{"/world/camera_sensor/model/default_topics/"}; + std::vector topics{ + prefix + "link/camera_link/sensor/camera/image", + prefix + "link/camera_link/sensor/camera/camera_info", + prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan", + prefix + "link/depth_camera_link/sensor/depth_camera/depth_image", + prefix + "link/depth_camera_link/sensor/depth_camera/camera_info", + prefix + "link/rgbd_camera_link/sensor/rgbd_camera/image", + prefix + "link/rgbd_camera_link/sensor/rgbd_camera/depth_image", + prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan", + prefix + "link/thermal_camera_link/sensor/thermal_camera/image", + prefix + "link/segmentation_camera_link/sensor/segmentation_camera/" + + "segmentation/colored_map", + prefix + "link/segmentation_camera_link/sensor/segmentation_camera/" + + "segmentation/labels_map", + prefix + "link/segmentation_camera_link/sensor/segmentation_camera/" + + "segmentation/camera_info", + "/camera" + }; + testDefaultTopics(topics); testSensorEntityIds(*ecm, g_sensorEntityIds); + testSensorTopicComponents(*ecm, g_sensorEntityIds, topics); + g_sensorEntityIds.clear(); g_scene.reset();