From 9ecebc1152c16a1d6ae88ff50cb97b322cd3c25d Mon Sep 17 00:00:00 2001 From: AmrElsersy <35613645+AmrElsersy@users.noreply.github.com> Date: Thu, 23 Sep 2021 00:56:32 +0200 Subject: [PATCH] Label Component & System, segmentation camera support (#853) Signed-off-by: AmrElsersy Signed-off-by: Ashton Larkin Co-authored-by: Ashton Larkin Co-authored-by: Louise Poubel --- CMakeLists.txt | 1 + examples/worlds/segmentation_camera.sdf | 362 +++++ .../gazebo/components/SegmentationCamera.hh | 45 + .../gazebo/components/SemanticLabel.hh | 43 + src/SdfEntityCreator.cc | 6 + src/rendering/RenderUtil.cc | 1171 ++++++++++------- src/systems/CMakeLists.txt | 1 + src/systems/label/CMakeLists.txt | 4 + src/systems/label/Label.cc | 106 ++ src/systems/label/Label.hh | 59 + src/systems/sensors/CMakeLists.txt | 1 + src/systems/sensors/Sensors.cc | 11 +- 12 files changed, 1301 insertions(+), 509 deletions(-) create mode 100644 examples/worlds/segmentation_camera.sdf create mode 100644 include/ignition/gazebo/components/SegmentationCamera.hh create mode 100644 include/ignition/gazebo/components/SemanticLabel.hh create mode 100644 src/systems/label/CMakeLists.txt create mode 100644 src/systems/label/Label.cc create mode 100644 src/systems/label/Label.hh diff --git a/CMakeLists.txt b/CMakeLists.txt index 8ea9b91fe8..07f5177a4b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -122,6 +122,7 @@ ign_find_package(ignition-sensors6 REQUIRED # cameras camera + segmentation_camera depth_camera rgbd_camera thermal_camera diff --git a/examples/worlds/segmentation_camera.sdf b/examples/worlds/segmentation_camera.sdf new file mode 100644 index 0000000000..095c05fb4f --- /dev/null +++ b/examples/worlds/segmentation_camera.sdf @@ -0,0 +1,362 @@ + + + + + + + + + + + + ogre2 + + + + + + + + 3D View + false + docked + + + ogre + scene + 1.0 1.0 1.0 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + semantic/colored_map + + + + + panoptic/colored_map + + + + + semantic/labels_map + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + Car1 + -2 -2 0 0 0 0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Hatchback blue + + + + + + + + Car2 + -3 -5 0 0 0 0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pickup + + + + + + + + Car3 + -4 3 0 0 0 -1.57 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/SUV + + + + + + + + + tree1 + -2 5 0 0 0 0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pine Tree + + + + + + + + tree2 + -7 2 0 0 0 0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pine Tree + + + + + + + + tree3 + -7 -4 0 0 0 0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pine Tree + + + + + + + + + home + -15 0 0 0 0 1.57 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Collapsed House + + + + + + cone1 + 0 1 0 0 0 1.570796 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + + + + + + + + cone2 + 0 4 0 0 0 1.570796 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + + + + + + + + cone3 + 2 -2 0 0 0.0 1.57 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + + + + + + + + + 6 0 2.0 0 0.0 3.14 + + 0 0 0 0 0 0 + + 0.5 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + panoptic + + instance + 1.57 + + 800 + 600 + + + 0.1 + 100 + + + segmentation_data/instance_camera + + + 1 + 30 + true + + + + + + + 6 0 2.0 0 0.0 3.14 + + 0 0 0 0 0 0 + + 0.5 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + semantic + + semantic + 1.57 + + 800 + 600 + + + 0.1 + 100 + + + segmentation_data/semantic_camera + + + 1 + 30 + true + + + + + + diff --git a/include/ignition/gazebo/components/SegmentationCamera.hh b/include/ignition/gazebo/components/SegmentationCamera.hh new file mode 100644 index 0000000000..c476fc8ba8 --- /dev/null +++ b/include/ignition/gazebo/components/SegmentationCamera.hh @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_COMPONENTS_SEGMENTATIONCAMERA_HH_ +#define IGNITION_GAZEBO_COMPONENTS_SEGMENTATIONCAMERA_HH_ + +#include + +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains a Segmentation camera sensor, + /// sdf::Camera, information. + using SegmentationCamera = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.SegmentationCamera", + SegmentationCamera) +} +} +} +} +#endif diff --git a/include/ignition/gazebo/components/SemanticLabel.hh b/include/ignition/gazebo/components/SemanticLabel.hh new file mode 100644 index 0000000000..3139b75310 --- /dev/null +++ b/include/ignition/gazebo/components/SemanticLabel.hh @@ -0,0 +1,43 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_COMPONENTS_LABEL_HH_ +#define IGNITION_GAZEBO_COMPONENTS_LABEL_HH_ + +#include "ignition/gazebo/Export.hh" +#include "ignition/gazebo/components/Component.hh" +#include "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/config.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component that holds the label of an entity. One example use + /// case of the Label component is with Segmentation & Bounding box + /// sensors to generate dataset annotations. + using SemanticLabel = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.SemanticLabel", + SemanticLabel) +} +} +} +} +#endif diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 85df246e1e..f5dd6fc7e7 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -64,6 +64,7 @@ #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/RgbdCamera.hh" #include "ignition/gazebo/components/Scene.hh" +#include "ignition/gazebo/components/SegmentationCamera.hh" #include "ignition/gazebo/components/SelfCollide.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/SourceFilePath.hh" @@ -823,6 +824,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) this->dataPtr->ecm->CreateComponent(sensorEntity, components::ThermalCamera(*_sensor)); } + else if (_sensor->Type() == sdf::SensorType::SEGMENTATION_CAMERA) + { + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::SegmentationCamera(*_sensor)); + } else if (_sensor->Type() == sdf::SensorType::AIR_PRESSURE) { this->dataPtr->ecm->CreateComponent(sensorEntity, diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 2a2224255a..ed56e56e2d 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -76,6 +76,8 @@ #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/RgbdCamera.hh" #include "ignition/gazebo/components/Scene.hh" +#include "ignition/gazebo/components/SegmentationCamera.hh" +#include "ignition/gazebo/components/SemanticLabel.hh" #include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/Temperature.hh" #include "ignition/gazebo/components/TemperatureRange.hh" @@ -103,9 +105,26 @@ class ignition::gazebo::RenderUtilPrivate /// \brief Create rendering entities /// \param[in] _ecm The entity-component manager + /// \param[in] _info Update information public: void CreateRenderingEntities(const EntityComponentManager &_ecm, const UpdateInfo &_info); + /// \brief Create rendering entities during the first update. This + /// is called by CreateRenderingEntities. This calls _ecm.Each + /// \param[in] _ecm The entity-component manager + /// \param[in] _info Update information + /// \TODO(anyone) Combine with CreateEntitiesRuntime + public: void CreateEntitiesFirstUpdate(const EntityComponentManager &_ecm, + const UpdateInfo &_info); + + /// \brief Create rendering entities during subsequent updates. This + /// is called by CreateRenderingEntities. This calls _ecm.EachNew + /// \param[in] _ecm The entity-component manager + /// \param[in] _info Update information + /// \TODO(anyone) Combine with CreateEntitiesFirstUpdate + public: void CreateEntitiesRuntime(const EntityComponentManager &_ecm, + const UpdateInfo &_info); + /// \brief Remove rendering entities /// \param[in] _ecm The entity-component manager public: void RemoveRenderingEntities(const EntityComponentManager &_ecm, @@ -115,6 +134,17 @@ class ignition::gazebo::RenderUtilPrivate /// \param[in] _ecm The entity-component manager public: void UpdateRenderingEntities(const EntityComponentManager &_ecm); + /// \breif Helper function to add new sensors + /// \param[in] _ecm The entity-component manager + /// \param[in] _entity Sensor entity + /// \param[in] _sdfData Sensor data + /// \param[in] _parent Parent entity + /// \param[in] _topicSuffix Suffix for sensor topic + public: void AddNewSensor( + const EntityComponentManager &_ecm, Entity _entity, + const sdf::Sensor &_sdfData, Entity _parent, + const std::string &_topicSuffix); + /// \brief Total time elapsed in simulation. This will not increase while /// paused. public: std::chrono::steady_clock::duration simTime{0}; @@ -232,6 +262,9 @@ class ignition::gazebo::RenderUtilPrivate /// All temperatures are in Kelvin. public: std::map> entityTemp; + /// \brief A map of entity ids and label data for datasets annotations + public: std::unordered_map entityLabel; + /// \brief A map of entity ids and wire boxes public: std::unordered_map wireBoxes; @@ -445,6 +478,11 @@ class ignition::gazebo::RenderUtilPrivate public: std::unordered_map> thermalCameraData; + /// \brief Update the visuals with label user data + /// \param[in] _entityLabel Map with key visual entity id and value label + public: void UpdateVisualLabels( + const std::unordered_map &_entityLabel); + /// \brief A helper function that removes the sensor associated with an /// entity, if an associated sensor exists. This should be called in /// RenderUtil::Update. @@ -897,6 +935,7 @@ void RenderUtil::Update() auto actorTransforms = std::move(this->dataPtr->actorTransforms); auto actorAnimationData = std::move(this->dataPtr->actorAnimationData); auto entityTemp = std::move(this->dataPtr->entityTemp); + auto entityLabel = std::move(this->dataPtr->entityLabel); auto newTransparentVisualLinks = std::move(this->dataPtr->newTransparentVisualLinks); auto newInertiaLinks = std::move(this->dataPtr->newInertiaLinks); @@ -923,6 +962,7 @@ void RenderUtil::Update() this->dataPtr->actorTransforms.clear(); this->dataPtr->actorAnimationData.clear(); this->dataPtr->entityTemp.clear(); + this->dataPtr->entityLabel.clear(); this->dataPtr->newTransparentVisualLinks.clear(); this->dataPtr->newInertiaLinks.clear(); this->dataPtr->newJointModels.clear(); @@ -1178,8 +1218,7 @@ void RenderUtil::Update() if (!node) continue; - auto visual = - std::dynamic_pointer_cast(node); + auto visual = std::dynamic_pointer_cast(node); if (!visual) continue; @@ -1194,6 +1233,8 @@ void RenderUtil::Update() } } + this->dataPtr->UpdateVisualLabels(entityLabel); + // update joint parent visual poses { for (const auto &jointEntity : updateJointParentPoses) @@ -1370,596 +1411,673 @@ void RenderUtilPrivate::CreateRenderingEntities( const EntityComponentManager &_ecm, const UpdateInfo &_info) { IGN_PROFILE("RenderUtilPrivate::CreateRenderingEntities"); - auto addNewSensor = [&_ecm, this](Entity _entity, const sdf::Sensor &_sdfData, - Entity _parent, - const std::string &_topicSuffix) + + // Treat all pre-existent entities as new at startup + // TODO(anyone) Combine the two CreateEntities functions below to reduce + // duplicate code + if (!this->initialized) { - sdf::Sensor sdfDataCopy(_sdfData); - std::string sensorScopedName = - removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); - sdfDataCopy.SetName(sensorScopedName); - // check topic - if (sdfDataCopy.Topic().empty()) - { - sdfDataCopy.SetTopic(scopedName(_entity, _ecm) + _topicSuffix); - } - this->newSensors.push_back( - std::make_tuple(_entity, std::move(sdfDataCopy), _parent)); - this->sensorEntities.insert(_entity); - }; + this->CreateEntitiesFirstUpdate(_ecm, _info); + this->initialized = true; + } + else + { + this->CreateEntitiesRuntime(_ecm, _info); + } +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::AddNewSensor(const EntityComponentManager &_ecm, + Entity _entity, const sdf::Sensor &_sdfData, Entity _parent, + const std::string &_topicSuffix) +{ + sdf::Sensor sdfDataCopy(_sdfData); + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdfDataCopy.SetName(sensorScopedName); + // check topic + if (sdfDataCopy.Topic().empty()) + { + sdfDataCopy.SetTopic(scopedName(_entity, _ecm) + _topicSuffix); + } + this->newSensors.push_back( + std::make_tuple(_entity, std::move(sdfDataCopy), _parent)); + this->sensorEntities.insert(_entity); +} +////////////////////////////////////////////////// +void RenderUtilPrivate::CreateEntitiesFirstUpdate( + const EntityComponentManager &_ecm, const UpdateInfo &_info) +{ const std::string cameraSuffix{"/image"}; const std::string depthCameraSuffix{"/depth_image"}; const std::string rgbdCameraSuffix{""}; const std::string thermalCameraSuffix{"/image"}; const std::string gpuLidarSuffix{"/scan"}; + const std::string segmentationCameraSuffix{"/segmentation"}; - // Treat all pre-existent entities as new at startup - // TODO(anyone) refactor Each and EachNew below to reduce duplicate code - if (!this->initialized) - { - // Get all the new worlds - // TODO(anyone) Only one scene is supported for now - // extend the sensor system to support mutliple scenes in the future - _ecm.Each( - [&](const Entity & _entity, - const components::World *, - const components::Scene *_scene)->bool - { - this->sceneManager.SetWorldId(_entity); - const sdf::Scene &sceneSdf = _scene->Data(); - this->newScenes.push_back(sceneSdf); - return true; - }); + // Get all the new worlds + // TODO(anyone) Only one scene is supported for now + // extend the sensor system to support mutliple scenes in the future + _ecm.Each( + [&](const Entity & _entity, + const components::World *, + const components::Scene *_scene)->bool + { + this->sceneManager.SetWorldId(_entity); + const sdf::Scene &sceneSdf = _scene->Data(); + this->newScenes.push_back(sceneSdf); + return true; + }); - _ecm.Each( - [&](const Entity &_entity, - const components::Model *, - const components::Name *_name, - const components::Pose *_pose, - const components::ParentEntity *_parent)->bool + _ecm.Each( + [&](const Entity &_entity, + const components::Model *, + const components::Name *_name, + const components::Pose *_pose, + const components::ParentEntity *_parent)->bool + { + sdf::Model model; + model.SetName(_name->Data()); + model.SetRawPose(_pose->Data()); + this->newModels.push_back(std::make_tuple(_entity, model, + _parent->Data(), _info.iterations)); + this->modelToModelEntities[_parent->Data()].push_back(_entity); + return true; + }); + + _ecm.Each( + [&](const Entity &_entity, + const components::Link *, + const components::Name *_name, + const components::Pose *_pose, + const components::ParentEntity *_parent)->bool + { + sdf::Link link; + link.SetName(_name->Data()); + link.SetRawPose(_pose->Data()); + this->newLinks.push_back( + std::make_tuple(_entity, link, _parent->Data())); + // used for collsions + this->modelToLinkEntities[_parent->Data()].push_back(_entity); + // used for joints + this->matchLinksWithEntities[_parent->Data()][_name->Data()] = + _entity; + return true; + }); + + // visuals + _ecm.Each( + [&](const Entity &_entity, + const components::Visual *, + const components::Name *_name, + const components::Pose *_pose, + const components::Geometry *_geom, + const components::CastShadows *_castShadows, + const components::Transparency *_transparency, + const components::VisibilityFlags *_visibilityFlags, + const components::ParentEntity *_parent)->bool + { + sdf::Visual visual; + visual.SetName(_name->Data()); + visual.SetRawPose(_pose->Data()); + visual.SetGeom(_geom->Data()); + visual.SetCastShadows(_castShadows->Data()); + visual.SetTransparency(_transparency->Data()); + visual.SetVisibilityFlags(_visibilityFlags->Data()); + + // Optional components + auto material = _ecm.Component(_entity); + if (material != nullptr) { - sdf::Model model; - model.SetName(_name->Data()); - model.SetRawPose(_pose->Data()); - this->newModels.push_back(std::make_tuple(_entity, model, - _parent->Data(), _info.iterations)); - this->modelToModelEntities[_parent->Data()].push_back(_entity); - return true; - }); + visual.SetMaterial(material->Data()); + } - _ecm.Each( - [&](const Entity &_entity, - const components::Link *, - const components::Name *_name, - const components::Pose *_pose, - const components::ParentEntity *_parent)->bool + auto laserRetro = _ecm.Component(_entity); + if (laserRetro != nullptr) { - sdf::Link link; - link.SetName(_name->Data()); - link.SetRawPose(_pose->Data()); - this->newLinks.push_back( - std::make_tuple(_entity, link, _parent->Data())); - // used for collsions - this->modelToLinkEntities[_parent->Data()].push_back(_entity); - // used for joints - this->matchLinksWithEntities[_parent->Data()][_name->Data()] = - _entity; - return true; - }); + visual.SetLaserRetro(laserRetro->Data()); + } - // visuals - _ecm.Each( - [&](const Entity &_entity, - const components::Visual *, - const components::Name *_name, - const components::Pose *_pose, - const components::Geometry *_geom, - const components::CastShadows *_castShadows, - const components::Transparency *_transparency, - const components::VisibilityFlags *_visibilityFlags, - const components::ParentEntity *_parent)->bool + // set label + auto label = _ecm.Component(_entity); + if (label != nullptr) { - sdf::Visual visual; - visual.SetName(_name->Data()); - visual.SetRawPose(_pose->Data()); - visual.SetGeom(_geom->Data()); - visual.SetCastShadows(_castShadows->Data()); - visual.SetTransparency(_transparency->Data()); - visual.SetVisibilityFlags(_visibilityFlags->Data()); - - // Optional components - auto material = _ecm.Component(_entity); - if (material != nullptr) - { - visual.SetMaterial(material->Data()); - } + this->entityLabel[_entity] = label->Data(); + } - auto laserRetro = _ecm.Component(_entity); - if (laserRetro != nullptr) + if (auto temp = _ecm.Component(_entity)) + { + // get the uniform temperature for the entity + this->entityTemp[_entity] = std::make_tuple + (temp->Data().Kelvin(), 0.0, ""); + } + else + { + // entity doesn't have a uniform temperature. Check if it has + // a heat signature with an associated temperature range + auto heatSignature = + _ecm.Component(_entity); + auto tempRange = + _ecm.Component(_entity); + if (heatSignature && tempRange) { - visual.SetLaserRetro(laserRetro->Data()); + this->entityTemp[_entity] = + std::make_tuple( + tempRange->Data().min.Kelvin(), + tempRange->Data().max.Kelvin(), + std::string(heatSignature->Data())); } + } - if (auto temp = _ecm.Component(_entity)) - { - // get the uniform temperature for the entity - this->entityTemp[_entity] = std::make_tuple - (temp->Data().Kelvin(), 0.0, ""); - } - else - { - // entity doesn't have a uniform temperature. Check if it has - // a heat signature with an associated temperature range - auto heatSignature = - _ecm.Component(_entity); - auto tempRange = - _ecm.Component(_entity); - if (heatSignature && tempRange) - { - this->entityTemp[_entity] = - std::make_tuple( - tempRange->Data().min.Kelvin(), - tempRange->Data().max.Kelvin(), - std::string(heatSignature->Data())); - } - } + this->newVisuals.push_back( + std::make_tuple(_entity, visual, _parent->Data())); - this->newVisuals.push_back( - std::make_tuple(_entity, visual, _parent->Data())); + this->linkToVisualEntities[_parent->Data()].push_back(_entity); + return true; + }); - this->linkToVisualEntities[_parent->Data()].push_back(_entity); - return true; - }); + // actors + _ecm.Each( + [&](const Entity &_entity, + const components::Actor *_actor, + const components::Name *_name, + const components::ParentEntity *_parent) -> bool + { + this->newActors.push_back(std::make_tuple(_entity, _actor->Data(), + _name->Data(), _parent->Data())); - // actors - _ecm.Each( - [&](const Entity &_entity, - const components::Actor *_actor, - const components::Name *_name, - const components::ParentEntity *_parent) -> bool + // set label + auto label = _ecm.Component(_entity); + if (label != nullptr) { - this->newActors.push_back(std::make_tuple(_entity, _actor->Data(), + this->entityLabel[_entity] = label->Data(); + } + + return true; + }); + + // lights + _ecm.Each( + [&](const Entity &_entity, + const components::Light *_light, + const components::Name *_name, + const components::ParentEntity *_parent) -> bool + { + this->newLights.push_back(std::make_tuple(_entity, _light->Data(), _name->Data(), _parent->Data())); - return true; - }); + return true; + }); - // lights - _ecm.Each( - [&](const Entity &_entity, - const components::Light *_light, - const components::Name *_name, - const components::ParentEntity *_parent) -> bool + // inertials + _ecm.Each( + [&](const Entity &_entity, + const components::Inertial *_inrElement, + const components::Pose *) -> bool + { + this->entityInertials[_entity] = _inrElement->Data(); + return true; + }); + + // collisions + _ecm.Each( + [&](const Entity &_entity, + const components::Collision *, + const components::Name *, + const components::Pose *, + const components::Geometry *, + const components::CollisionElement *_collElement, + const components::ParentEntity *_parent) -> bool + { + this->entityCollisions[_entity] = _collElement->Data(); + this->linkToCollisionEntities[_parent->Data()].push_back(_entity); + return true; + }); + + // joints + _ecm.Each( + [&](const Entity &_entity, + const components::Joint * /* _joint */, + const components::Name *_name, + const components::JointType *_jointType, + const components::Pose *_pose, + const components::ParentEntity *_parentModel, + const components::ParentLinkName *_parentLinkName, + const components::ChildLinkName *_childLinkName) -> bool + { + sdf::Joint joint; + joint.SetName(_name->Data()); + joint.SetType(_jointType->Data()); + joint.SetRawPose(_pose->Data()); + + joint.SetParentLinkName(_parentLinkName->Data()); + joint.SetChildLinkName(_childLinkName->Data()); + + auto jointAxis = _ecm.Component(_entity); + auto jointAxis2 = _ecm.Component(_entity); + + if (jointAxis) { - this->newLights.push_back(std::make_tuple(_entity, _light->Data(), - _name->Data(), _parent->Data())); - return true; - }); + joint.SetAxis(0, jointAxis->Data()); + } + if (jointAxis2) + { + joint.SetAxis(1, jointAxis2->Data()); + } + + this->entityJoints[_entity] = joint; + this->modelToJointEntities[_parentModel->Data()].push_back(_entity); + return true; + }); - // inertials - _ecm.Each( - [&](const Entity &_entity, - const components::Inertial *_inrElement, - const components::Pose *) -> bool + // particle emitters + _ecm.Each( + [&](const Entity &_entity, + const components::ParticleEmitter *_emitter, + const components::ParentEntity *_parent) -> bool + { + this->newParticleEmitters.push_back( + std::make_tuple(_entity, _emitter->Data(), _parent->Data())); + return true; + }); + + if (this->enableSensors) + { + // Create cameras + _ecm.Each( + [&](const Entity &_entity, + const components::Camera *_camera, + const components::ParentEntity *_parent)->bool { - this->entityInertials[_entity] = _inrElement->Data(); + this->AddNewSensor(_ecm, _entity, _camera->Data(), _parent->Data(), + cameraSuffix); return true; }); - // collisions - _ecm.Each( - [&](const Entity &_entity, - const components::Collision *, - const components::Name *, - const components::Pose *, - const components::Geometry *, - const components::CollisionElement *_collElement, - const components::ParentEntity *_parent) -> bool + // Create depth cameras + _ecm.Each( + [&](const Entity &_entity, + const components::DepthCamera *_depthCamera, + const components::ParentEntity *_parent)->bool { - this->entityCollisions[_entity] = _collElement->Data(); - this->linkToCollisionEntities[_parent->Data()].push_back(_entity); + this->AddNewSensor(_ecm, _entity, _depthCamera->Data(), + _parent->Data(), depthCameraSuffix); return true; }); - // joints - _ecm.Each( - [&](const Entity &_entity, - const components::Joint * /* _joint */, - const components::Name *_name, - const components::JointType *_jointType, - const components::Pose *_pose, - const components::ParentEntity *_parentModel, - const components::ParentLinkName *_parentLinkName, - const components::ChildLinkName *_childLinkName) -> bool + // Create rgbd cameras + _ecm.Each( + [&](const Entity &_entity, + const components::RgbdCamera *_rgbdCamera, + const components::ParentEntity *_parent)->bool { - sdf::Joint joint; - joint.SetName(_name->Data()); - joint.SetType(_jointType->Data()); - joint.SetRawPose(_pose->Data()); - - joint.SetParentLinkName(_parentLinkName->Data()); - joint.SetChildLinkName(_childLinkName->Data()); - - auto jointAxis = _ecm.Component(_entity); - auto jointAxis2 = _ecm.Component(_entity); - - if (jointAxis) - { - joint.SetAxis(0, jointAxis->Data()); - } - if (jointAxis2) - { - joint.SetAxis(1, jointAxis2->Data()); - } - - this->entityJoints[_entity] = joint; - this->modelToJointEntities[_parentModel->Data()].push_back(_entity); + this->AddNewSensor(_ecm, _entity, _rgbdCamera->Data(), + _parent->Data(), rgbdCameraSuffix); return true; }); - // particle emitters - _ecm.Each( - [&](const Entity &_entity, - const components::ParticleEmitter *_emitter, - const components::ParentEntity *_parent) -> bool + // Create gpu lidar + _ecm.Each( + [&](const Entity &_entity, + const components::GpuLidar *_gpuLidar, + const components::ParentEntity *_parent)->bool { - this->newParticleEmitters.push_back( - std::make_tuple(_entity, _emitter->Data(), _parent->Data())); + this->AddNewSensor(_ecm, _entity, _gpuLidar->Data(), _parent->Data(), + gpuLidarSuffix); return true; }); - if (this->enableSensors) - { - // Create cameras - _ecm.Each( - [&](const Entity &_entity, - const components::Camera *_camera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _camera->Data(), _parent->Data(), - cameraSuffix); - return true; - }); - - // Create depth cameras - _ecm.Each( - [&](const Entity &_entity, - const components::DepthCamera *_depthCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), - depthCameraSuffix); - return true; - }); - - // Create rgbd cameras - _ecm.Each( - [&](const Entity &_entity, - const components::RgbdCamera *_rgbdCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), - rgbdCameraSuffix); - return true; - }); - - // Create gpu lidar - _ecm.Each( - [&](const Entity &_entity, - const components::GpuLidar *_gpuLidar, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), - gpuLidarSuffix); - return true; - }); - - // Create thermal camera - _ecm.Each( - [&](const Entity &_entity, - const components::ThermalCamera *_thermalCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), - thermalCameraSuffix); - return true; - }); - } - this->initialized = true; - } - else - { - // Get all the new worlds - // TODO(anyone) Only one scene is supported for now - // extend the sensor system to support mutliple scenes in the future - _ecm.EachNew( - [&](const Entity & _entity, - const components::World *, - const components::Scene *_scene)->bool + // Create thermal camera + _ecm.Each( + [&](const Entity &_entity, + const components::ThermalCamera *_thermalCamera, + const components::ParentEntity *_parent)->bool { - this->sceneManager.SetWorldId(_entity); - const sdf::Scene &sceneSdf = _scene->Data(); - this->newScenes.push_back(sceneSdf); + this->AddNewSensor(_ecm, _entity, _thermalCamera->Data(), + _parent->Data(), thermalCameraSuffix); return true; }); - _ecm.EachNew( - [&](const Entity &_entity, - const components::Model *, - const components::Name *_name, - const components::Pose *_pose, - const components::ParentEntity *_parent)->bool + // Create segmentation cameras + _ecm.Each( + [&](const Entity &_entity, + const components::SegmentationCamera *_segmentationCamera, + const components::ParentEntity *_parent)->bool { - sdf::Model model; - model.SetName(_name->Data()); - model.SetRawPose(_pose->Data()); - this->newModels.push_back(std::make_tuple(_entity, model, - _parent->Data(), _info.iterations)); - this->modelToModelEntities[_parent->Data()].push_back(_entity); + this->AddNewSensor(_ecm, _entity, _segmentationCamera->Data(), + _parent->Data(), segmentationCameraSuffix); return true; }); + } +} - _ecm.EachNew( - [&](const Entity &_entity, - const components::Link *, - const components::Name *_name, - const components::Pose *_pose, - const components::ParentEntity *_parent)->bool +////////////////////////////////////////////////// +void RenderUtilPrivate::CreateEntitiesRuntime( + const EntityComponentManager &_ecm, const UpdateInfo &_info) +{ + const std::string cameraSuffix{"/image"}; + const std::string depthCameraSuffix{"/depth_image"}; + const std::string rgbdCameraSuffix{""}; + const std::string thermalCameraSuffix{"/image"}; + const std::string gpuLidarSuffix{"/scan"}; + const std::string segmentationCameraSuffix{"/segmentation"}; + + // Get all the new worlds + // TODO(anyone) Only one scene is supported for now + // extend the sensor system to support mutliple scenes in the future + _ecm.EachNew( + [&](const Entity & _entity, + const components::World *, + const components::Scene *_scene)->bool + { + this->sceneManager.SetWorldId(_entity); + const sdf::Scene &sceneSdf = _scene->Data(); + this->newScenes.push_back(sceneSdf); + return true; + }); + + _ecm.EachNew( + [&](const Entity &_entity, + const components::Model *, + const components::Name *_name, + const components::Pose *_pose, + const components::ParentEntity *_parent)->bool + { + sdf::Model model; + model.SetName(_name->Data()); + model.SetRawPose(_pose->Data()); + this->newModels.push_back(std::make_tuple(_entity, model, + _parent->Data(), _info.iterations)); + this->modelToModelEntities[_parent->Data()].push_back(_entity); + return true; + }); + + _ecm.EachNew( + [&](const Entity &_entity, + const components::Link *, + const components::Name *_name, + const components::Pose *_pose, + const components::ParentEntity *_parent)->bool + { + sdf::Link link; + link.SetName(_name->Data()); + link.SetRawPose(_pose->Data()); + this->newLinks.push_back( + std::make_tuple(_entity, link, _parent->Data())); + // used for collsions + this->modelToLinkEntities[_parent->Data()].push_back(_entity); + // used for joints + this->matchLinksWithEntities[_parent->Data()][_name->Data()] = + _entity; + return true; + }); + + // visuals + _ecm.EachNew( + [&](const Entity &_entity, + const components::Visual *, + const components::Name *_name, + const components::Pose *_pose, + const components::Geometry *_geom, + const components::CastShadows *_castShadows, + const components::Transparency *_transparency, + const components::VisibilityFlags *_visibilityFlags, + const components::ParentEntity *_parent)->bool + { + sdf::Visual visual; + visual.SetName(_name->Data()); + visual.SetRawPose(_pose->Data()); + visual.SetGeom(_geom->Data()); + visual.SetCastShadows(_castShadows->Data()); + visual.SetTransparency(_transparency->Data()); + visual.SetVisibilityFlags(_visibilityFlags->Data()); + + // Optional components + auto material = _ecm.Component(_entity); + if (material != nullptr) { - sdf::Link link; - link.SetName(_name->Data()); - link.SetRawPose(_pose->Data()); - this->newLinks.push_back( - std::make_tuple(_entity, link, _parent->Data())); - // used for collsions - this->modelToLinkEntities[_parent->Data()].push_back(_entity); - // used for joints - this->matchLinksWithEntities[_parent->Data()][_name->Data()] = - _entity; - return true; - }); + visual.SetMaterial(material->Data()); + } - // visuals - _ecm.EachNew( - [&](const Entity &_entity, - const components::Visual *, - const components::Name *_name, - const components::Pose *_pose, - const components::Geometry *_geom, - const components::CastShadows *_castShadows, - const components::Transparency *_transparency, - const components::VisibilityFlags *_visibilityFlags, - const components::ParentEntity *_parent)->bool + auto laserRetro = _ecm.Component(_entity); + if (laserRetro != nullptr) { - sdf::Visual visual; - visual.SetName(_name->Data()); - visual.SetRawPose(_pose->Data()); - visual.SetGeom(_geom->Data()); - visual.SetCastShadows(_castShadows->Data()); - visual.SetTransparency(_transparency->Data()); - visual.SetVisibilityFlags(_visibilityFlags->Data()); - - // Optional components - auto material = _ecm.Component(_entity); - if (material != nullptr) - { - visual.SetMaterial(material->Data()); - } + visual.SetLaserRetro(laserRetro->Data()); + } - auto laserRetro = _ecm.Component(_entity); - if (laserRetro != nullptr) - { - visual.SetLaserRetro(laserRetro->Data()); - } + // set label + auto label = _ecm.Component(_entity); + if (label != nullptr) + { + this->entityLabel[_entity] = label->Data(); + } - if (auto temp = _ecm.Component(_entity)) + if (auto temp = _ecm.Component(_entity)) + { + // get the uniform temperature for the entity + this->entityTemp[_entity] = std::make_tuple + (temp->Data().Kelvin(), 0.0, ""); + } + else + { + // entity doesn't have a uniform temperature. Check if it has + // a heat signature with an associated temperature range + auto heatSignature = + _ecm.Component(_entity); + auto tempRange = + _ecm.Component(_entity); + if (heatSignature && tempRange) { - // get the uniform temperature for the entity - this->entityTemp[_entity] = std::make_tuple - (temp->Data().Kelvin(), 0.0, ""); - } - else - { - // entity doesn't have a uniform temperature. Check if it has - // a heat signature with an associated temperature range - auto heatSignature = - _ecm.Component(_entity); - auto tempRange = - _ecm.Component(_entity); - if (heatSignature && tempRange) - { - this->entityTemp[_entity] = - std::make_tuple( - tempRange->Data().min.Kelvin(), - tempRange->Data().max.Kelvin(), - std::string(heatSignature->Data())); - } + this->entityTemp[_entity] = + std::make_tuple( + tempRange->Data().min.Kelvin(), + tempRange->Data().max.Kelvin(), + std::string(heatSignature->Data())); } + } - this->newVisuals.push_back( - std::make_tuple(_entity, visual, _parent->Data())); + this->newVisuals.push_back( + std::make_tuple(_entity, visual, _parent->Data())); - this->linkToVisualEntities[_parent->Data()].push_back(_entity); - return true; - }); + this->linkToVisualEntities[_parent->Data()].push_back(_entity); + return true; + }); + + // actors + _ecm.EachNew( + [&](const Entity &_entity, + const components::Actor *_actor, + const components::Name *_name, + const components::ParentEntity *_parent) -> bool + { + this->newActors.push_back( + std::make_tuple(_entity, _actor->Data(), _name->Data(), + _parent->Data())); + + // set label + auto label = _ecm.Component(_entity); + if (label != nullptr) + { + this->entityLabel[_entity] = label->Data(); + } + + return true; + }); + + // lights + _ecm.EachNew( + [&](const Entity &_entity, + const components::Light *_light, + const components::Name *_name, + const components::ParentEntity *_parent) -> bool + { + this->newLights.push_back(std::make_tuple(_entity, _light->Data(), + _name->Data(), _parent->Data())); + return true; + }); + + // inertials + _ecm.EachNew( + [&](const Entity &_entity, + const components::Inertial *_inrElement, + const components::Pose *) -> bool + { + this->entityInertials[_entity] = _inrElement->Data(); + return true; + }); + + // collisions + _ecm.EachNew( + [&](const Entity &_entity, + const components::Collision *, + const components::Name *, + const components::Pose *, + const components::Geometry *, + const components::CollisionElement *_collElement, + const components::ParentEntity *_parent) -> bool + { + this->entityCollisions[_entity] = _collElement->Data(); + this->linkToCollisionEntities[_parent->Data()].push_back(_entity); + return true; + }); - // actors - _ecm.EachNew( - [&](const Entity &_entity, - const components::Actor *_actor, - const components::Name *_name, - const components::ParentEntity *_parent) -> bool + // joints + _ecm.EachNew( + [&](const Entity &_entity, + const components::Joint * /* _joint */, + const components::Name *_name, + const components::JointType *_jointType, + const components::Pose *_pose, + const components::ParentEntity *_parentModel, + const components::ParentLinkName *_parentLinkName, + const components::ChildLinkName *_childLinkName) -> bool + { + sdf::Joint joint; + joint.SetName(_name->Data()); + joint.SetType(_jointType->Data()); + joint.SetRawPose(_pose->Data()); + + joint.SetParentLinkName(_parentLinkName->Data()); + joint.SetChildLinkName(_childLinkName->Data()); + + auto jointAxis = _ecm.Component(_entity); + auto jointAxis2 = _ecm.Component(_entity); + + if (jointAxis) + { + joint.SetAxis(0, jointAxis->Data()); + } + if (jointAxis2) { - this->newActors.push_back( - std::make_tuple(_entity, _actor->Data(), _name->Data(), - _parent->Data())); + joint.SetAxis(1, jointAxis2->Data()); + } + + this->entityJoints[_entity] = joint; + this->modelToJointEntities[_parentModel->Data()].push_back(_entity); + return true; + }); + + // particle emitters + _ecm.EachNew( + [&](const Entity &_entity, + const components::ParticleEmitter *_emitter, + const components::ParentEntity *_parent) -> bool + { + this->newParticleEmitters.push_back( + std::make_tuple(_entity, _emitter->Data(), _parent->Data())); + return true; + }); + + if (this->enableSensors) + { + // Create cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::Camera *_camera, + const components::ParentEntity *_parent)->bool + { + this->AddNewSensor(_ecm, _entity, _camera->Data(), _parent->Data(), + cameraSuffix); return true; }); - // lights - _ecm.EachNew( - [&](const Entity &_entity, - const components::Light *_light, - const components::Name *_name, - const components::ParentEntity *_parent) -> bool + // Create depth cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::DepthCamera *_depthCamera, + const components::ParentEntity *_parent)->bool { - this->newLights.push_back(std::make_tuple(_entity, _light->Data(), - _name->Data(), _parent->Data())); + this->AddNewSensor(_ecm, _entity, _depthCamera->Data(), + _parent->Data(), depthCameraSuffix); return true; }); - // inertials - _ecm.EachNew( - [&](const Entity &_entity, - const components::Inertial *_inrElement, - const components::Pose *) -> bool + // Create RGBD cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::RgbdCamera *_rgbdCamera, + const components::ParentEntity *_parent)->bool { - this->entityInertials[_entity] = _inrElement->Data(); + this->AddNewSensor(_ecm, _entity, _rgbdCamera->Data(), + _parent->Data(), rgbdCameraSuffix); return true; }); - // collisions - _ecm.EachNew( - [&](const Entity &_entity, - const components::Collision *, - const components::Name *, - const components::Pose *, - const components::Geometry *, - const components::CollisionElement *_collElement, - const components::ParentEntity *_parent) -> bool + // Create gpu lidar + _ecm.EachNew( + [&](const Entity &_entity, + const components::GpuLidar *_gpuLidar, + const components::ParentEntity *_parent)->bool { - this->entityCollisions[_entity] = _collElement->Data(); - this->linkToCollisionEntities[_parent->Data()].push_back(_entity); + this->AddNewSensor(_ecm, _entity, _gpuLidar->Data(), _parent->Data(), + gpuLidarSuffix); return true; }); - // joints - _ecm.EachNew( - [&](const Entity &_entity, - const components::Joint * /* _joint */, - const components::Name *_name, - const components::JointType *_jointType, - const components::Pose *_pose, - const components::ParentEntity *_parentModel, - const components::ParentLinkName *_parentLinkName, - const components::ChildLinkName *_childLinkName) -> bool + // Create thermal camera + _ecm.EachNew( + [&](const Entity &_entity, + const components::ThermalCamera *_thermalCamera, + const components::ParentEntity *_parent)->bool { - sdf::Joint joint; - joint.SetName(_name->Data()); - joint.SetType(_jointType->Data()); - joint.SetRawPose(_pose->Data()); - - joint.SetParentLinkName(_parentLinkName->Data()); - joint.SetChildLinkName(_childLinkName->Data()); - - auto jointAxis = _ecm.Component(_entity); - auto jointAxis2 = _ecm.Component(_entity); - - if (jointAxis) - { - joint.SetAxis(0, jointAxis->Data()); - } - if (jointAxis2) - { - joint.SetAxis(1, jointAxis2->Data()); - } - - this->entityJoints[_entity] = joint; - this->modelToJointEntities[_parentModel->Data()].push_back(_entity); + this->AddNewSensor(_ecm, _entity, _thermalCamera->Data(), + _parent->Data(), thermalCameraSuffix); return true; }); - // particle emitters - _ecm.EachNew( - [&](const Entity &_entity, - const components::ParticleEmitter *_emitter, - const components::ParentEntity *_parent) -> bool + // Create segmentation cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::SegmentationCamera *_segmentationCamera, + const components::ParentEntity *_parent)->bool { - this->newParticleEmitters.push_back( - std::make_tuple(_entity, _emitter->Data(), _parent->Data())); + this->AddNewSensor(_ecm, _entity, _segmentationCamera->Data(), + _parent->Data(), segmentationCameraSuffix); return true; }); - - if (this->enableSensors) - { - // Create cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::Camera *_camera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _camera->Data(), _parent->Data(), - cameraSuffix); - return true; - }); - - // Create depth cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::DepthCamera *_depthCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), - depthCameraSuffix); - return true; - }); - - // Create RGBD cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::RgbdCamera *_rgbdCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), - rgbdCameraSuffix); - return true; - }); - - // Create gpu lidar - _ecm.EachNew( - [&](const Entity &_entity, - const components::GpuLidar *_gpuLidar, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), - gpuLidarSuffix); - return true; - }); - - // Create thermal camera - _ecm.EachNew( - [&](const Entity &_entity, - const components::ThermalCamera *_thermalCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), - thermalCameraSuffix); - return true; - }); - } } } @@ -2110,6 +2228,16 @@ void RenderUtilPrivate::UpdateRenderingEntities( this->entityPoses[_entity] = _pose->Data(); return true; }); + + // Update segmentation cameras + _ecm.Each( + [&](const Entity &_entity, + const components::SegmentationCamera *, + const components::Pose *_pose)->bool + { + this->entityPoses[_entity] = _pose->Data(); + return true; + }); } ////////////////////////////////////////////////// @@ -2233,6 +2361,14 @@ void RenderUtilPrivate::RemoveRenderingEntities( return true; }); + // segmentation cameras + _ecm.EachRemoved( + [&](const Entity &_entity, const components::SegmentationCamera *)->bool + { + this->removeEntities[_entity] = _info.iterations; + return true; + }); + // collisions _ecm.EachRemoved( [&](const Entity &_entity, const components::Collision *)->bool @@ -2469,6 +2605,25 @@ void RenderUtil::SetTransformActive(bool _active) this->dataPtr->transformActive = _active; } +//////////////////////////////////////////////// +void RenderUtilPrivate::UpdateVisualLabels( + const std::unordered_map &_entityLabel) +{ + // set visual label + for (const auto &label : _entityLabel) + { + auto node = this->sceneManager.NodeById(label.first); + if (!node) + continue; + + auto visual = std::dynamic_pointer_cast(node); + if (!visual) + continue; + + visual->SetUserData("label", label.second); + } +} + //////////////////////////////////////////////// void RenderUtilPrivate::HighlightNode(const rendering::NodePtr &_node) { diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index e48c1b1f5c..c5e7770470 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -107,6 +107,7 @@ add_subdirectory(joint_position_controller) add_subdirectory(joint_state_publisher) add_subdirectory(joint_traj_control) add_subdirectory(kinetic_energy_monitor) +add_subdirectory(label) add_subdirectory(lift_drag) add_subdirectory(log) add_subdirectory(log_video_recorder) diff --git a/src/systems/label/CMakeLists.txt b/src/systems/label/CMakeLists.txt new file mode 100644 index 0000000000..22b66ff1cd --- /dev/null +++ b/src/systems/label/CMakeLists.txt @@ -0,0 +1,4 @@ +gz_add_system(label + SOURCES + Label.cc +) diff --git a/src/systems/label/Label.cc b/src/systems/label/Label.cc new file mode 100644 index 0000000000..09986de104 --- /dev/null +++ b/src/systems/label/Label.cc @@ -0,0 +1,106 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#include "Label.hh" + +#include +#include + +#include + +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/components/Actor.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/SemanticLabel.hh" +#include "ignition/gazebo/components/Visual.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +////////////////////////////////////////////////// +Label::Label() : System() +{ +} + +////////////////////////////////////////////////// +Label::~Label() = default; + +////////////////////////////////////////////////// +void Label::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + gazebo::EntityComponentManager &_ecm, + gazebo::EventManager & /*_eventMgr*/) +{ + const std::string labelTag = "label"; + + if (!_sdf->HasElement(labelTag)) + { + ignerr << "Failed to load Label system; label tag not found.\n"; + return; + } + + auto label = _sdf->Get(labelTag); + + if (label < 0 || label > 255) + { + ignerr << "Failed to configure Label system; value " << label + << " is not in [0-255] range.\n"; + return; + } + + // Attach a semantic label component to the visual. + // If the plugin is inside the tag, get its visual child. + if (_ecm.EntityHasComponentType(_entity, components::Visual::typeId) || + _ecm.EntityHasComponentType(_entity, components::Actor::typeId)) + { + _ecm.CreateComponent(_entity, components::SemanticLabel(label)); + } + else if (_ecm.EntityHasComponentType(_entity, components::Model::typeId)) + { + // TODO(anyone) add support for nested models. We will need to check for + // child models and their respective links/visuals + // https://github.com/ignitionrobotics/ign-gazebo/issues/1041 + + // Get link childern of parent model + auto links = _ecm.ChildrenByComponents( + _entity, components::Link()); + + for (auto linkEntity : links) + { + // get visual child of parent link + auto visuals = _ecm.ChildrenByComponents( + linkEntity, components::Visual()); + + // Create label component to all visual childern + for (auto visualEntity : visuals) + { + _ecm.CreateComponent(visualEntity, + components::SemanticLabel(label)); + } + } + } + else + { + ignerr << "Entity [" << _entity << "] is not a visual, actor, or model. " + << "Label will be ignored. \n"; + return; + } +} + +IGNITION_ADD_PLUGIN(Label, System, Label::ISystemConfigure) +IGNITION_ADD_PLUGIN_ALIAS(Label, "ignition::gazebo::systems::Label") diff --git a/src/systems/label/Label.hh b/src/systems/label/Label.hh new file mode 100644 index 0000000000..a0d6fffde6 --- /dev/null +++ b/src/systems/label/Label.hh @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_LABEL_HH_ +#define IGNITION_GAZEBO_SYSTEMS_LABEL_HH_ + +#include + +#include "ignition/gazebo/config.hh" +#include "ignition/gazebo/System.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + /// \brief A label plugin that annotates models by setting the label + /// for the parent entity's visuals. The plugin can be attached to models, + /// visuals, or actors. + /// + /// Ex: "" means the visual has a label of 1 + /// Label value must be in [0-255] range + class Label: + public System, + public ISystemConfigure + { + /// \brief Constructor + public: explicit Label(); + + /// \brief Destructor + public: ~Label() override; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + gazebo::EventManager &_eventMgr) final; + }; + } +} +} +} +#endif diff --git a/src/systems/sensors/CMakeLists.txt b/src/systems/sensors/CMakeLists.txt index 9dc80d906e..8b56378885 100644 --- a/src/systems/sensors/CMakeLists.txt +++ b/src/systems/sensors/CMakeLists.txt @@ -11,6 +11,7 @@ gz_add_system(sensors ignition-sensors${IGN_SENSORS_VER}::ignition-sensors${IGN_SENSORS_VER} ignition-sensors${IGN_SENSORS_VER}::lidar ignition-sensors${IGN_SENSORS_VER}::rgbd_camera + ignition-sensors${IGN_SENSORS_VER}::segmentation_camera ignition-sensors${IGN_SENSORS_VER}::thermal_camera ) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index f859f5ba38..4043124df5 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -37,6 +37,7 @@ #include #include #include +#include #include #include "ignition/gazebo/components/Atmosphere.hh" @@ -46,6 +47,7 @@ #include "ignition/gazebo/components/RenderEngineServerHeadless.hh" #include "ignition/gazebo/components/RenderEngineServerPlugin.hh" #include "ignition/gazebo/components/RgbdCamera.hh" +#include "ignition/gazebo/components/SegmentationCamera.hh" #include "ignition/gazebo/components/ThermalCamera.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Events.hh" @@ -465,7 +467,9 @@ void Sensors::PostUpdate(const UpdateInfo &_info, _ecm.HasComponentType(components::DepthCamera::typeId) || _ecm.HasComponentType(components::GpuLidar::typeId) || _ecm.HasComponentType(components::RgbdCamera::typeId) || - _ecm.HasComponentType(components::ThermalCamera::typeId))) + _ecm.HasComponentType(components::ThermalCamera::typeId) || + _ecm.HasComponentType(components::SegmentationCamera::typeId) + )) { igndbg << "Initialization needed" << std::endl; std::unique_lock lock(this->dataPtr->renderMutex); @@ -565,6 +569,11 @@ std::string Sensors::CreateSensor(const Entity &_entity, sensor = this->dataPtr->sensorManager.CreateSensor< sensors::ThermalCameraSensor>(_sdf); } + else if (_sdf.Type() == sdf::SensorType::SEGMENTATION_CAMERA) + { + sensor = this->dataPtr->sensorManager.CreateSensor< + sensors::SegmentationCameraSensor>(_sdf); + } if (nullptr == sensor) {