From 902099e50a6e7c8f3eb418f6cff1b4c0709ac3eb Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Wed, 9 Jun 2021 01:27:09 +0200 Subject: [PATCH 01/15] Label Component & System Signed-off-by: AmrElsersy --- include/ignition/gazebo/components/Label.hh | 42 ++++++++++ src/rendering/RenderUtil.cc | 41 +++++++-- src/systems/CMakeLists.txt | 1 + src/systems/label/CMakeLists.txt | 4 + src/systems/label/Label.cc | 93 +++++++++++++++++++++ src/systems/label/Label.hh | 53 ++++++++++++ 6 files changed, 228 insertions(+), 6 deletions(-) create mode 100644 include/ignition/gazebo/components/Label.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/include/ignition/gazebo/components/Label.hh b/include/ignition/gazebo/components/Label.hh new file mode 100644 index 0000000000..f3e54d28de --- /dev/null +++ b/include/ignition/gazebo/components/Label.hh @@ -0,0 +1,42 @@ +/* + * Copyright (C) 2018 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 +#include +#include "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/components/Component.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 the object used by + /// Segmentation & Bounding box sensors to generate datasets annotations + using Label = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Label", + Label) +} +} +} +} +#endif diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 00373c88fa..09586da9a5 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -57,6 +57,7 @@ #include "ignition/gazebo/components/DepthCamera.hh" #include "ignition/gazebo/components/GpuLidar.hh" #include "ignition/gazebo/components/Geometry.hh" +#include "ignition/gazebo/components/Label.hh" #include "ignition/gazebo/components/LaserRetro.hh" #include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/LightCmd.hh" @@ -220,6 +221,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::map entityLabel; + /// \brief A map of entity ids and wire boxes public: std::unordered_map wireBoxes; @@ -637,6 +641,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 newWireframeVisualLinks = std::move(this->dataPtr->newWireframeVisualLinks); auto newCollisionLinks = std::move(this->dataPtr->newCollisionLinks); @@ -657,6 +662,7 @@ void RenderUtil::Update() this->dataPtr->actorTransforms.clear(); this->dataPtr->actorAnimationData.clear(); this->dataPtr->entityTemp.clear(); + this->dataPtr->entityLabel.clear(); this->dataPtr->newWireframeVisualLinks.clear(); this->dataPtr->newCollisionLinks.clear(); this->dataPtr->thermalCameraData.clear(); @@ -680,9 +686,7 @@ void RenderUtil::Update() if (scene.Grid() && !this->dataPtr->enableSensors) this->ShowGrid(); if (scene.Sky()) - { this->dataPtr->scene->SetSkyEnabled(true); - } // only one scene so break break; @@ -971,9 +975,7 @@ void RenderUtil::Update() math::Pose3d globalPose; if (entityPoses.find(tf.first) != entityPoses.end()) - { globalPose = entityPoses[tf.first]; - } math::Pose3d trajPose; // Trajectory from the ECS @@ -1096,8 +1098,7 @@ void RenderUtil::Update() if (!node) continue; - auto visual = - std::dynamic_pointer_cast(node); + auto visual = std::dynamic_pointer_cast(node); if (!visual) continue; @@ -1112,6 +1113,20 @@ void RenderUtil::Update() } } + // set visual label + for (const auto &label : entityLabel) + { + auto node = this->dataPtr->sceneManager.NodeById(label.first); + if (!node) + continue; + + auto visual = std::dynamic_pointer_cast(node); + if (!visual) + continue; + + visual->SetUserData("label", label.second); + } + // create new wireframe visuals { for (const auto &link : newWireframeVisualLinks) @@ -1329,6 +1344,13 @@ void RenderUtilPrivate::CreateRenderingEntities( 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)) { // get the uniform temperature for the entity @@ -1560,6 +1582,13 @@ void RenderUtilPrivate::CreateRenderingEntities( 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)) { // get the uniform temperature for the entity diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 2e2f830236..a35508b08d 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -105,6 +105,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..2691ddec47 --- /dev/null +++ b/src/systems/label/Label.cc @@ -0,0 +1,93 @@ +/* + * 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 +#include + +#include +#include "ignition/gazebo/components/Label.hh" +#include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/components/Visual.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "Label.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"; + + std::string parentName = _sdf->GetParent()->GetName(); + + if (!_sdf->HasElement(labelTag)) + { + ignerr << "Failed to load Label system, label tag not found"; + return; + } + + int label = _sdf->Get(labelTag); + + if (label < 0 || label > 255) + { + ignerr << "Failed to Configure Label system, value " << label + << " is not in [0-255] range"; + return; + } + + /// Set the component to the visual to set its user data, but + /// if the plugin is inside the tag, get its visual child + if (parentName == "visual") + { + _ecm.CreateComponent(_entity, components::Label(label)); + } + else if (parentName == "model") + { + // get link child of parent model + auto links = _ecm.ChildrenByComponents( + _entity, components::Link()); + if (links.size() > 0) + { + Entity linkEntity = links[0]; + + // get visual child of parent link + auto visuals = _ecm.ChildrenByComponents( + linkEntity, components::Visual()); + if (visuals.size() > 0) + { + Entity visualEntity = visuals[0]; + _ecm.CreateComponent(visualEntity, components::Label(label)); + } + } + } +} + +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..e02fe6a570 --- /dev/null +++ b/src/systems/label/Label.hh @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2018 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 +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + /// \brief A label plugin that sets the label for the parent entity + 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 From 472a228b5a18e282c2a9dd1cc8be86aeb4fdb54f Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Sun, 13 Jun 2021 05:25:02 +0200 Subject: [PATCH 02/15] Add Segmentation & BoundingBox Components, Modified the RenderUtil & SdfEntityCreator to recognize the new components Signed-off-by: AmrElsersy --- .../gazebo/components/BoundingBoxCamera.hh | 45 ++++++++++ include/ignition/gazebo/components/Label.hh | 8 +- .../gazebo/components/SegmentationCamera.hh | 45 ++++++++++ src/SdfEntityCreator.cc | 12 +++ src/rendering/RenderUtil.cc | 88 ++++++++++++++++++- src/systems/label/Label.hh | 6 +- src/systems/sensors/Sensors.cc | 7 +- 7 files changed, 201 insertions(+), 10 deletions(-) create mode 100644 include/ignition/gazebo/components/BoundingBoxCamera.hh create mode 100644 include/ignition/gazebo/components/SegmentationCamera.hh diff --git a/include/ignition/gazebo/components/BoundingBoxCamera.hh b/include/ignition/gazebo/components/BoundingBoxCamera.hh new file mode 100644 index 0000000000..17242019a0 --- /dev/null +++ b/include/ignition/gazebo/components/BoundingBoxCamera.hh @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2019 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_BOUNDINGBOXCAMERA_HH_ +#define IGNITION_GAZEBO_COMPONENTS_BOUNDINGBOXCAMERA_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 BoundingBox camera sensor, + /// sdf::Camera, information. + using BoundingBoxCamera = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.BoundingBoxCamera", + BoundingBoxCamera) +} +} +} +} +#endif diff --git a/include/ignition/gazebo/components/Label.hh b/include/ignition/gazebo/components/Label.hh index f3e54d28de..209642ebfd 100644 --- a/include/ignition/gazebo/components/Label.hh +++ b/include/ignition/gazebo/components/Label.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -17,10 +17,10 @@ #ifndef IGNITION_GAZEBO_COMPONENTS_LABEL_HH_ #define IGNITION_GAZEBO_COMPONENTS_LABEL_HH_ -#include -#include -#include "ignition/gazebo/components/Factory.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 { diff --git a/include/ignition/gazebo/components/SegmentationCamera.hh b/include/ignition/gazebo/components/SegmentationCamera.hh new file mode 100644 index 0000000000..ce449922a5 --- /dev/null +++ b/include/ignition/gazebo/components/SegmentationCamera.hh @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2019 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/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index d7c98189c6..b898766143 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -27,6 +27,7 @@ #include "ignition/gazebo/components/Altimeter.hh" #include "ignition/gazebo/components/AngularVelocity.hh" #include "ignition/gazebo/components/Atmosphere.hh" +#include "ignition/gazebo/components/BoundingBoxCamera.hh" #include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/CanonicalLink.hh" #include "ignition/gazebo/components/CastShadows.hh" @@ -62,6 +63,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" @@ -778,6 +780,16 @@ 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::BOUNDINGBOX_CAMERA) + { + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::BoundingBoxCamera(*_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 09586da9a5..bacca3ccee 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -54,6 +54,7 @@ #include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/CastShadows.hh" #include "ignition/gazebo/components/Collision.hh" +#include "ignition/gazebo/components/BoundingBoxCamera.hh" #include "ignition/gazebo/components/DepthCamera.hh" #include "ignition/gazebo/components/GpuLidar.hh" #include "ignition/gazebo/components/Geometry.hh" @@ -70,6 +71,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/SourceFilePath.hh" #include "ignition/gazebo/components/Temperature.hh" #include "ignition/gazebo/components/TemperatureRange.hh" @@ -686,7 +688,9 @@ void RenderUtil::Update() if (scene.Grid() && !this->dataPtr->enableSensors) this->ShowGrid(); if (scene.Sky()) + { this->dataPtr->scene->SetSkyEnabled(true); + } // only one scene so break break; @@ -1066,9 +1070,7 @@ void RenderUtil::Update() // update actor trajectory animation math::Pose3d globalPose; if (entityPoses.find(it.first) != entityPoses.end()) - { globalPose = entityPoses[it.first]; - } math::Pose3d trajPose; // Trajectory from the ECS @@ -1250,6 +1252,8 @@ void RenderUtilPrivate::CreateRenderingEntities( const std::string rgbdCameraSuffix{""}; const std::string thermalCameraSuffix{"/image"}; const std::string gpuLidarSuffix{"/scan"}; + const std::string segmentationCameraSuffix{"/segmentation"}; + const std::string boundingBoxCameraSuffix{"/boundingbox"}; // Treat all pre-existent entities as new at startup // TODO(anyone) refactor Each and EachNew below to reduce duplicate code @@ -1489,6 +1493,28 @@ void RenderUtilPrivate::CreateRenderingEntities( thermalCameraSuffix); return true; }); + + // Create segmentation cameras + _ecm.Each( + [&](const Entity &_entity, + const components::SegmentationCamera *_segmentationCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _segmentationCamera->Data(), + _parent->Data(), segmentationCameraSuffix); + return true; + }); + + // Create bounding box cameras + _ecm.Each( + [&](const Entity &_entity, + const components::BoundingBoxCamera *_boundingBoxCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _boundingBoxCamera->Data(), + _parent->Data(), boundingBoxCameraSuffix); + return true; + }); } this->initialized = true; } @@ -1727,6 +1753,28 @@ void RenderUtilPrivate::CreateRenderingEntities( thermalCameraSuffix); return true; }); + + // Create segmentation cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::SegmentationCamera *_segmentationCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _segmentationCamera->Data(), + _parent->Data(), segmentationCameraSuffix); + return true; + }); + + // Create bounding box cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::BoundingBoxCamera *_boundingBoxCamera, + const components::ParentEntity *_parent)->bool + { + addNewSensor(_entity, _boundingBoxCamera->Data(), + _parent->Data(), boundingBoxCameraSuffix); + return true; + }); } } } @@ -1878,6 +1926,26 @@ 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; + }); + + // Update bounding box cameras + _ecm.Each( + [&](const Entity &_entity, + const components::BoundingBoxCamera *, + const components::Pose *_pose)->bool + { + this->entityPoses[_entity] = _pose->Data(); + return true; + }); } ////////////////////////////////////////////////// @@ -1970,6 +2038,22 @@ void RenderUtilPrivate::RemoveRenderingEntities( return true; }); + // segmentation cameras + _ecm.EachRemoved( + [&](const Entity &_entity, const components::SegmentationCamera *)->bool + { + this->removeEntities[_entity] = _info.iterations; + return true; + }); + + // bounding box cameras + _ecm.EachRemoved( + [&](const Entity &_entity, const components::BoundingBoxCamera *)->bool + { + this->removeEntities[_entity] = _info.iterations; + return true; + }); + // collisions _ecm.EachRemoved( [&](const Entity &_entity, const components::Collision *)->bool diff --git a/src/systems/label/Label.hh b/src/systems/label/Label.hh index e02fe6a570..f38d1cc9e4 100644 --- a/src/systems/label/Label.hh +++ b/src/systems/label/Label.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -18,8 +18,8 @@ #define IGNITION_GAZEBO_SYSTEMS_LABEL_HH_ #include -#include -#include +#include "ignition/gazebo/config.hh" +#include "ignition/gazebo/System.hh" namespace ignition { diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index a20b5c3280..122d5f67fd 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -38,10 +38,12 @@ #include "ignition/gazebo/components/Atmosphere.hh" #include "ignition/gazebo/components/Camera.hh" +#include "ignition/gazebo/components/BoundingBoxCamera.hh" #include "ignition/gazebo/components/DepthCamera.hh" #include "ignition/gazebo/components/GpuLidar.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" @@ -442,7 +444,10 @@ 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) || + _ecm.HasComponentType(components::BoundingBoxCamera::typeId) + )) { igndbg << "Initialization needed" << std::endl; std::unique_lock lock(this->dataPtr->renderMutex); From cc18cb6631736c068be466d06f96d52edb2e90ba Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Thu, 17 Jun 2021 05:47:29 +0200 Subject: [PATCH 03/15] Label all visuals childeren + style + RenderUtil entity labels Signed-off-by: AmrElsersy --- .../gazebo/components/BoundingBoxCamera.hh | 2 +- .../gazebo/components/SegmentationCamera.hh | 2 +- .../components/{Label.hh => SemanticLabel.hh} | 11 ++-- src/rendering/RenderUtil.cc | 53 +++++++++++++------ src/systems/label/Label.cc | 49 +++++++++-------- src/systems/label/Label.hh | 11 +++- src/systems/sensors/Sensors.cc | 2 +- 7 files changed, 81 insertions(+), 49 deletions(-) rename include/ignition/gazebo/components/{Label.hh => SemanticLabel.hh} (75%) diff --git a/include/ignition/gazebo/components/BoundingBoxCamera.hh b/include/ignition/gazebo/components/BoundingBoxCamera.hh index 17242019a0..1e18636e29 100644 --- a/include/ignition/gazebo/components/BoundingBoxCamera.hh +++ b/include/ignition/gazebo/components/BoundingBoxCamera.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/SegmentationCamera.hh b/include/ignition/gazebo/components/SegmentationCamera.hh index ce449922a5..c476fc8ba8 100644 --- a/include/ignition/gazebo/components/SegmentationCamera.hh +++ b/include/ignition/gazebo/components/SegmentationCamera.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Label.hh b/include/ignition/gazebo/components/SemanticLabel.hh similarity index 75% rename from include/ignition/gazebo/components/Label.hh rename to include/ignition/gazebo/components/SemanticLabel.hh index 209642ebfd..3139b75310 100644 --- a/include/ignition/gazebo/components/Label.hh +++ b/include/ignition/gazebo/components/SemanticLabel.hh @@ -30,11 +30,12 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { - /// \brief A component that holds the label of the object used by - /// Segmentation & Bounding box sensors to generate datasets annotations - using Label = Component; - IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Label", - Label) + /// \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) } } } diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index bacca3ccee..933233823d 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -51,14 +51,14 @@ #include #include "ignition/gazebo/components/Actor.hh" +#include "ignition/gazebo/components/BoundingBoxCamera.hh" #include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/CastShadows.hh" #include "ignition/gazebo/components/Collision.hh" -#include "ignition/gazebo/components/BoundingBoxCamera.hh" #include "ignition/gazebo/components/DepthCamera.hh" #include "ignition/gazebo/components/GpuLidar.hh" #include "ignition/gazebo/components/Geometry.hh" -#include "ignition/gazebo/components/Label.hh" +#include "ignition/gazebo/components/SemanticLabel.hh" #include "ignition/gazebo/components/LaserRetro.hh" #include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/LightCmd.hh" @@ -224,7 +224,7 @@ class ignition::gazebo::RenderUtilPrivate public: std::map> entityTemp; /// \brief A map of entity ids and label data for datasets annotations - public: std::map entityLabel; + public: std::unordered_map entityLabel; /// \brief A map of entity ids and wire boxes public: std::unordered_map wireBoxes; @@ -357,8 +357,15 @@ 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); }; + + ////////////////////////////////////////////////// RenderUtil::RenderUtil() : dataPtr(std::make_unique()) { @@ -979,7 +986,9 @@ void RenderUtil::Update() math::Pose3d globalPose; if (entityPoses.find(tf.first) != entityPoses.end()) + { globalPose = entityPoses[tf.first]; + } math::Pose3d trajPose; // Trajectory from the ECS @@ -1070,7 +1079,9 @@ void RenderUtil::Update() // update actor trajectory animation math::Pose3d globalPose; if (entityPoses.find(it.first) != entityPoses.end()) + { globalPose = entityPoses[it.first]; + } math::Pose3d trajPose; // Trajectory from the ECS @@ -1115,19 +1126,8 @@ void RenderUtil::Update() } } - // set visual label - for (const auto &label : entityLabel) - { - auto node = this->dataPtr->sceneManager.NodeById(label.first); - if (!node) - continue; - - auto visual = std::dynamic_pointer_cast(node); - if (!visual) - continue; + this->dataPtr->UpdateVisualLabels(entityLabel); - visual->SetUserData("label", label.second); - } // create new wireframe visuals { @@ -1349,7 +1349,7 @@ void RenderUtilPrivate::CreateRenderingEntities( } // set label - auto label = _ecm.Component(_entity); + auto label = _ecm.Component(_entity); if (label != nullptr) { this->entityLabel[_entity] = label->Data(); @@ -1609,7 +1609,7 @@ void RenderUtilPrivate::CreateRenderingEntities( } // set label - auto label = _ecm.Component(_entity); + auto label = _ecm.Component(_entity); if (label != nullptr) { this->entityLabel[_entity] = label->Data(); @@ -2273,6 +2273,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/label/Label.cc b/src/systems/label/Label.cc index 2691ddec47..86e8c86601 100644 --- a/src/systems/label/Label.cc +++ b/src/systems/label/Label.cc @@ -14,16 +14,14 @@ * limitations under the License. * */ -#include +#include "Label.hh" #include #include -#include "ignition/gazebo/components/Label.hh" -#include "ignition/gazebo/components/World.hh" -#include "ignition/gazebo/components/Visual.hh" -#include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/EntityComponentManager.hh" -#include "Label.hh" +#include "ignition/gazebo/components/SemanticLabel.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Visual.hh" using namespace ignition; using namespace gazebo; @@ -45,48 +43,55 @@ void Label::Configure(const Entity &_entity, { const std::string labelTag = "label"; - std::string parentName = _sdf->GetParent()->GetName(); - if (!_sdf->HasElement(labelTag)) { - ignerr << "Failed to load Label system, label tag not found"; + ignerr << "Failed to load Label system; label tag not found.\n"; return; } - int label = _sdf->Get(labelTag); + auto label = _sdf->Get(labelTag); if (label < 0 || label > 255) { - ignerr << "Failed to Configure Label system, value " << label - << " is not in [0-255] range"; + ignerr << "Failed to configure Label system; value " << label + << " is not in [0-255] range.\n"; return; } - /// Set the component to the visual to set its user data, but - /// if the plugin is inside the tag, get its visual child + const std::string parentName = _sdf->GetParent()->GetName(); + + // Set the component to the visual to set its user data, but + // if the plugin is inside the tag, get its visual child if (parentName == "visual") { - _ecm.CreateComponent(_entity, components::Label(label)); + _ecm.CreateComponent(_entity, components::SemanticLabel(label)); } else if (parentName == "model") { - // get link child of parent model + // Get link childern of parent model auto links = _ecm.ChildrenByComponents( _entity, components::Link()); - if (links.size() > 0) - { - Entity linkEntity = links[0]; + for (auto linkEntity : links) + { // get visual child of parent link auto visuals = _ecm.ChildrenByComponents( linkEntity, components::Visual()); - if (visuals.size() > 0) + + // Create label component to all visual childern + for (auto visualEntity : visuals) { - Entity visualEntity = visuals[0]; - _ecm.CreateComponent(visualEntity, components::Label(label)); + _ecm.CreateComponent(visualEntity, + components::SemanticLabel(label)); } } } + else + { + ignerr << "Label plugin is not a child of model or visual " << + "label will be ignored. \n"; + return; + } } IGNITION_ADD_PLUGIN(Label, System, Label::ISystemConfigure) diff --git a/src/systems/label/Label.hh b/src/systems/label/Label.hh index f38d1cc9e4..a0da0f77f9 100644 --- a/src/systems/label/Label.hh +++ b/src/systems/label/Label.hh @@ -18,6 +18,7 @@ #define IGNITION_GAZEBO_SYSTEMS_LABEL_HH_ #include + #include "ignition/gazebo/config.hh" #include "ignition/gazebo/System.hh" @@ -29,7 +30,13 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { - /// \brief A label plugin that sets the label for the parent entity + /// \brief A label plugin that annotates models by setting the label + /// for the parent entity's visuals + /// + /// Must contain exactly one - + 4 0 1.0 0 0.0 3.14 0.05 0.05 0.05 0 0 0 @@ -289,10 +289,10 @@ - - full_2d + boxes + full_2d 1.047 800 diff --git a/src/systems/sensors/CMakeLists.txt b/src/systems/sensors/CMakeLists.txt index 9dc80d906e..b25e4de454 100644 --- a/src/systems/sensors/CMakeLists.txt +++ b/src/systems/sensors/CMakeLists.txt @@ -6,6 +6,7 @@ gz_add_system(sensors ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} ignition-sensors${IGN_SENSORS_VER}::camera + ignition-sensors${IGN_SENSORS_VER}::boundingbox_camera ignition-sensors${IGN_SENSORS_VER}::depth_camera ignition-sensors${IGN_SENSORS_VER}::gpu_lidar ignition-sensors${IGN_SENSORS_VER}::ignition-sensors${IGN_SENSORS_VER} diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 2006b5f656..c74dc89954 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -38,6 +38,7 @@ #include #include #include +// #include #include #include "ignition/gazebo/components/Atmosphere.hh" @@ -566,6 +567,11 @@ std::string Sensors::CreateSensor(const Entity &_entity, sensor = this->dataPtr->sensorManager.CreateSensor< sensors::BoundingBoxCameraSensor>(_sdf); } + else if (_sdf.Type() == sdf::SensorType::SEGMENTATION_CAMERA) + { + // sensor = this->dataPtr->sensorManager.CreateSensor< + // sensors::SegmentationCameraSensor>(_sdf); + } if (nullptr == sensor) { From 86bf2df5b25e2d3fe233811b2cee25b84aef1502 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Fri, 27 Aug 2021 00:13:55 +0200 Subject: [PATCH 07/15] Update Segmentation & BoundingBox examples Signed-off-by: AmrElsersy --- CMakeLists.txt | 1 + examples/worlds/boundingbox_camera.sdf | 12 +- examples/worlds/segmentation_camera.sdf | 309 +++++++++++++----------- src/systems/label/Label.cc | 4 +- src/systems/sensors/CMakeLists.txt | 1 + 5 files changed, 182 insertions(+), 145 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 994f243d68..e44476b984 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -122,6 +122,7 @@ ign_find_package(ignition-sensors6 REQUIRED # cameras camera boundingbox_camera + segmentation_camera depth_camera rgbd_camera thermal_camera diff --git a/examples/worlds/boundingbox_camera.sdf b/examples/worlds/boundingbox_camera.sdf index fb842d4ce1..1e71a83460 100644 --- a/examples/worlds/boundingbox_camera.sdf +++ b/examples/worlds/boundingbox_camera.sdf @@ -263,7 +263,7 @@ - 4 0 1.0 0 0.0 3.14 + 3 0 1.0 0 0.0 3.14 0.05 0.05 0.05 0 0 0 @@ -292,16 +292,20 @@ boxes - full_2d - 1.047 + 3d + 1.57 800 600 0.1 - 10 + 100 + + 1 30 diff --git a/examples/worlds/segmentation_camera.sdf b/examples/worlds/segmentation_camera.sdf index ad2cd01b43..56c9be5423 100644 --- a/examples/worlds/segmentation_camera.sdf +++ b/examples/worlds/segmentation_camera.sdf @@ -1,22 +1,13 @@ - - - 0.001 - 1.0 - + - - ogre2 - @@ -25,12 +16,11 @@ filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster"> - - - 1.0 1.0 1.0 - 0.8 0.8 0.8 - true - + + ogre2 + @@ -95,34 +85,29 @@ true + - segmentation/colored_map - docked + semantic/colored_map - - - segmentation/labels_map + - docked - 400 - 600 + panoptic/colored_map - - + + semantic/labels_map - true 0 0 10 0 0 0 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 1000 0.9 @@ -159,122 +144,175 @@ - - 0 1 0.5 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 1 1 1 - - - + + + Car1 + -2 -2 0 0 0 0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Hatchback blue + + + + + - - - - 1 1 1 - - - - 1 0 0 1 - 0.8 0.2 0 1 - 0.8 0 0 1 - - - - - - - + + 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 + + + + + - - 0 -1 0.5 0 0 0 - + + 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 + + + + + + + + + Homee + -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 - 1 - 0 - 0 - 1 - 0 - 1 + 0.000166667 + 0.000166667 + 0.000166667 - 1.0 - + - 1 1 1 + 0.1 0.1 0.1 - - + - 1 1 1 + 0.1 0.1 0.1 - - 0 0 0.5 1 - 0 0 1 1 - 0 0 0.3 1 - - - - - - - - true - -1 -2 0.5 0 0 0 - - - - - 0.5 - - - - - - - 0.5 - - - - 0 1 0 1 - 0 1 0 1 - 0 1 0 1 - - false - + + panoptic + + instance + 1.57 + + 800 + 600 + + + 0.1 + 100 + + + + + 1 + 30 + true + - - - - - 4 0 1.0 0 0.0 3.14 + + + 6 0 2.0 0 0.0 3.14 - 0.05 0.05 0.05 0 0 0 + 0 0 0 0 0 0 - 0.1 + 0.5 0.000166667 0.000166667 @@ -295,20 +333,23 @@ - - - instance - segmentation + + semantic - 1.047 + semantic + 1.57 800 600 0.1 - 10 + 100 + + 1 30 @@ -317,15 +358,5 @@ - - -1 0 3 0.0 0.0 1.57 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone - - - - - - diff --git a/src/systems/label/Label.cc b/src/systems/label/Label.cc index 86e8c86601..3942cd1e68 100644 --- a/src/systems/label/Label.cc +++ b/src/systems/label/Label.cc @@ -88,8 +88,8 @@ void Label::Configure(const Entity &_entity, } else { - ignerr << "Label plugin is not a child of model or visual " << - "label will be ignored. \n"; + ignerr << "Label plugin " + parentName + " is not a child of model or visual " + << "label will be ignored. \n"; return; } } diff --git a/src/systems/sensors/CMakeLists.txt b/src/systems/sensors/CMakeLists.txt index b25e4de454..d918788200 100644 --- a/src/systems/sensors/CMakeLists.txt +++ b/src/systems/sensors/CMakeLists.txt @@ -7,6 +7,7 @@ gz_add_system(sensors ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} ignition-sensors${IGN_SENSORS_VER}::camera ignition-sensors${IGN_SENSORS_VER}::boundingbox_camera + ignition-sensors${IGN_SENSORS_VER}::segmentation_camera ignition-sensors${IGN_SENSORS_VER}::depth_camera ignition-sensors${IGN_SENSORS_VER}::gpu_lidar ignition-sensors${IGN_SENSORS_VER}::ignition-sensors${IGN_SENSORS_VER} From f5b786da97c04e741c4af40595bce64ab6d14eaa Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Fri, 17 Sep 2021 23:01:33 +0200 Subject: [PATCH 08/15] solve actor visual problem Signed-off-by: AmrElsersy --- examples/worlds/boundingbox_camera.sdf | 11 +++++------ examples/worlds/segmentation_camera.sdf | 2 +- src/rendering/RenderUtil.cc | 21 ++++++++++++++++++--- src/systems/label/Label.cc | 11 +++++++++-- src/systems/label/Label.hh | 2 -- src/systems/sensors/CMakeLists.txt | 4 ++-- src/systems/sensors/Sensors.cc | 9 +++------ 7 files changed, 38 insertions(+), 22 deletions(-) diff --git a/examples/worlds/boundingbox_camera.sdf b/examples/worlds/boundingbox_camera.sdf index 1e71a83460..f397d7b65b 100644 --- a/examples/worlds/boundingbox_camera.sdf +++ b/examples/worlds/boundingbox_camera.sdf @@ -263,7 +263,7 @@ - 3 0 1.0 0 0.0 3.14 + 4 0 1.0 0 0.0 3.14 0.05 0.05 0.05 0 0 0 @@ -292,19 +292,18 @@ boxes - 3d - 1.57 + full_2d + 1.047 800 600 0.1 - 100 + 10 - 1 diff --git a/examples/worlds/segmentation_camera.sdf b/examples/worlds/segmentation_camera.sdf index 56c9be5423..b7615896fb 100644 --- a/examples/worlds/segmentation_camera.sdf +++ b/examples/worlds/segmentation_camera.sdf @@ -1,6 +1,6 @@ diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index d5474d5cac..2b2fdb44a9 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -58,7 +58,6 @@ #include "ignition/gazebo/components/DepthCamera.hh" #include "ignition/gazebo/components/GpuLidar.hh" #include "ignition/gazebo/components/Geometry.hh" -#include "ignition/gazebo/components/SemanticLabel.hh" #include "ignition/gazebo/components/Inertial.hh" #include "ignition/gazebo/components/LaserRetro.hh" #include "ignition/gazebo/components/Light.hh" @@ -73,6 +72,7 @@ #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" @@ -414,6 +414,7 @@ class ignition::gazebo::RenderUtilPrivate /// \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. @@ -456,8 +457,6 @@ class ignition::gazebo::RenderUtilPrivate const std::unordered_map &_trajectoryPoses); }; - - ////////////////////////////////////////////////// RenderUtil::RenderUtil() : dataPtr(std::make_unique()) { @@ -1405,6 +1404,14 @@ void RenderUtilPrivate::CreateRenderingEntities( { this->newActors.push_back( std::make_tuple(_entity, _actor->Data(), _parent->Data())); + + // set label + auto label = _ecm.Component(_entity); + if (label != nullptr) + { + this->entityLabel[_entity] = label->Data(); + } + return true; }); @@ -1675,6 +1682,14 @@ void RenderUtilPrivate::CreateRenderingEntities( { this->newActors.push_back( std::make_tuple(_entity, _actor->Data(), _parent->Data())); + + // set label + auto label = _ecm.Component(_entity); + if (label != nullptr) + { + this->entityLabel[_entity] = label->Data(); + } + return true; }); diff --git a/src/systems/label/Label.cc b/src/systems/label/Label.cc index 3942cd1e68..4d9e1132b9 100644 --- a/src/systems/label/Label.cc +++ b/src/systems/label/Label.cc @@ -19,8 +19,10 @@ #include #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/SemanticLabel.hh" #include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Visual.hh" using namespace ignition; @@ -62,11 +64,11 @@ void Label::Configure(const Entity &_entity, // Set the component to the visual to set its user data, but // if the plugin is inside the tag, get its visual child - if (parentName == "visual") + if (_ecm.EntityHasComponentType(_entity, components::Visual::typeId)) { _ecm.CreateComponent(_entity, components::SemanticLabel(label)); } - else if (parentName == "model") + else if (_ecm.EntityHasComponentType(_entity, components::Model::typeId)) { // Get link childern of parent model auto links = _ecm.ChildrenByComponents( @@ -86,6 +88,11 @@ void Label::Configure(const Entity &_entity, } } } + else if (_ecm.EntityHasComponentType(_entity, components::Actor::typeId)) + { + std::cout << "actor " << _entity << " .. label " << label << std::endl; + _ecm.CreateComponent(_entity, components::SemanticLabel(label)); + } else { ignerr << "Label plugin " + parentName + " is not a child of model or visual " diff --git a/src/systems/label/Label.hh b/src/systems/label/Label.hh index a0da0f77f9..4a2a9cbaed 100644 --- a/src/systems/label/Label.hh +++ b/src/systems/label/Label.hh @@ -17,8 +17,6 @@ #ifndef IGNITION_GAZEBO_SYSTEMS_LABEL_HH_ #define IGNITION_GAZEBO_SYSTEMS_LABEL_HH_ -#include - #include "ignition/gazebo/config.hh" #include "ignition/gazebo/System.hh" diff --git a/src/systems/sensors/CMakeLists.txt b/src/systems/sensors/CMakeLists.txt index d918788200..7f616f6c3d 100644 --- a/src/systems/sensors/CMakeLists.txt +++ b/src/systems/sensors/CMakeLists.txt @@ -5,14 +5,14 @@ gz_add_system(sensors ${rendering_target} ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} - ignition-sensors${IGN_SENSORS_VER}::camera ignition-sensors${IGN_SENSORS_VER}::boundingbox_camera - ignition-sensors${IGN_SENSORS_VER}::segmentation_camera + ignition-sensors${IGN_SENSORS_VER}::camera ignition-sensors${IGN_SENSORS_VER}::depth_camera ignition-sensors${IGN_SENSORS_VER}::gpu_lidar 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 c74dc89954..b9e7fd0eea 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -32,13 +32,12 @@ #include #include -#include #include #include #include #include #include -// #include +#include #include #include "ignition/gazebo/components/Atmosphere.hh" @@ -564,13 +563,11 @@ std::string Sensors::CreateSensor(const Entity &_entity, } else if (_sdf.Type() == sdf::SensorType::BOUNDINGBOX_CAMERA) { - sensor = this->dataPtr->sensorManager.CreateSensor< - sensors::BoundingBoxCameraSensor>(_sdf); } else if (_sdf.Type() == sdf::SensorType::SEGMENTATION_CAMERA) { - // sensor = this->dataPtr->sensorManager.CreateSensor< - // sensors::SegmentationCameraSensor>(_sdf); + sensor = this->dataPtr->sensorManager.CreateSensor< + sensors::SegmentationCameraSensor>(_sdf); } if (nullptr == sensor) From 545c83c6c455c7387734a9df9f6a52b57f0d4a53 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Fri, 17 Sep 2021 23:41:36 +0200 Subject: [PATCH 09/15] clean code for actor Signed-off-by: AmrElsersy --- src/systems/label/Label.cc | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/systems/label/Label.cc b/src/systems/label/Label.cc index 4d9e1132b9..6223ccf5a3 100644 --- a/src/systems/label/Label.cc +++ b/src/systems/label/Label.cc @@ -63,8 +63,9 @@ void Label::Configure(const Entity &_entity, const std::string parentName = _sdf->GetParent()->GetName(); // Set the component to the visual to set its user data, but - // if the plugin is inside the tag, get its visual child - if (_ecm.EntityHasComponentType(_entity, components::Visual::typeId)) + // 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)); } @@ -88,11 +89,6 @@ void Label::Configure(const Entity &_entity, } } } - else if (_ecm.EntityHasComponentType(_entity, components::Actor::typeId)) - { - std::cout << "actor " << _entity << " .. label " << label << std::endl; - _ecm.CreateComponent(_entity, components::SemanticLabel(label)); - } else { ignerr << "Label plugin " + parentName + " is not a child of model or visual " From ecc178393238219a812fdcac4aa6f5009b74f89b Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Tue, 21 Sep 2021 23:24:09 -0400 Subject: [PATCH 10/15] cleanup Signed-off-by: Ashton Larkin --- examples/worlds/boundingbox_camera.sdf | 50 +++++++++---------- examples/worlds/segmentation_camera.sdf | 64 ++++++++++++------------- src/rendering/RenderUtil.cc | 7 +-- src/systems/label/Label.cc | 14 +++--- src/systems/label/Label.hh | 7 ++- src/systems/sensors/Sensors.cc | 3 ++ 6 files changed, 74 insertions(+), 71 deletions(-) diff --git a/examples/worlds/boundingbox_camera.sdf b/examples/worlds/boundingbox_camera.sdf index f397d7b65b..c704eff8fd 100644 --- a/examples/worlds/boundingbox_camera.sdf +++ b/examples/worlds/boundingbox_camera.sdf @@ -152,12 +152,9 @@ - - - - - 0 1 0.5 0 0 0 - + + 0 -1 0.5 0 0 0 + 1 @@ -169,7 +166,7 @@ 1.0 - + 1 1 1 @@ -177,24 +174,30 @@ - + 1 1 1 - 1 0 0 1 - 0.8 0.2 0 1 - 0.8 0 0 1 + 0 0 0.5 1 + 0 0 1 1 + 0 0 0.3 1 + + + - - 0 -1 0.5 0 0 0 - + + + + + 0 1 0.5 0 0 0 + 1 @@ -206,7 +209,7 @@ 1.0 - + 1 1 1 @@ -214,20 +217,17 @@ - + 1 1 1 - 0 0 0.5 1 - 0 0 1 1 - 0 0 0.3 1 + 1 0 0 1 + 0.8 0.2 0 1 + 0.8 0 0 1 - - - @@ -302,9 +302,9 @@ 0.1 10 - + + boundingBox_data + 1 30 diff --git a/examples/worlds/segmentation_camera.sdf b/examples/worlds/segmentation_camera.sdf index b7615896fb..095c05fb4f 100644 --- a/examples/worlds/segmentation_camera.sdf +++ b/examples/worlds/segmentation_camera.sdf @@ -148,9 +148,9 @@ Car1 -2 -2 0 0 0 0 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Hatchback blue - + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Hatchback blue + @@ -159,9 +159,9 @@ Car2 -3 -5 0 0 0 0 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pickup - + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pickup + @@ -170,9 +170,9 @@ Car3 -4 3 0 0 0 -1.57 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/SUV - + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/SUV + @@ -182,9 +182,9 @@ tree1 -2 5 0 0 0 0 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pine Tree - + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pine Tree + @@ -193,9 +193,9 @@ tree2 -7 2 0 0 0 0 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pine Tree - + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pine Tree + @@ -204,17 +204,17 @@ tree3 -7 -4 0 0 0 0 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pine Tree - + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pine Tree + - + - Homee + home -15 0 0 0 0 1.57 https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Collapsed House @@ -226,27 +226,29 @@ cone1 0 1 0 0 0 1.570796 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + 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 + 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 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone @@ -254,7 +256,7 @@ - + 6 0 2.0 0 0.0 3.14 0 0 0 0 0 0 @@ -294,10 +296,9 @@ 0.1 100 - - + + segmentation_data/instance_camera + 1 30 @@ -346,10 +347,9 @@ 0.1 100 - - + + segmentation_data/semantic_camera + 1 30 diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 97a04b05f7..aacdd0f661 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -234,7 +234,7 @@ class ignition::gazebo::RenderUtilPrivate public: std::map> entityTemp; /// \brief A map of entity ids and label data for datasets annotations - public: std::unordered_map entityLabel; + public: std::unordered_map entityLabel; /// \brief A map of entity ids and wire boxes public: std::unordered_map wireBoxes; @@ -452,7 +452,7 @@ class ignition::gazebo::RenderUtilPrivate /// \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); + 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 @@ -1204,6 +1204,7 @@ void RenderUtil::Update() } this->dataPtr->UpdateVisualLabels(entityLabel); + // update joint parent visual poses { for (const auto &jointEntity : updateJointParentPoses) @@ -2592,7 +2593,7 @@ void RenderUtil::SetTransformActive(bool _active) //////////////////////////////////////////////// void RenderUtilPrivate::UpdateVisualLabels( - const std::unordered_map &_entityLabel) + const std::unordered_map &_entityLabel) { // set visual label for (const auto &label : _entityLabel) diff --git a/src/systems/label/Label.cc b/src/systems/label/Label.cc index 6223ccf5a3..43a08e3099 100644 --- a/src/systems/label/Label.cc +++ b/src/systems/label/Label.cc @@ -15,14 +15,16 @@ * */ #include "Label.hh" + #include #include + #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/components/Actor.hh" -#include "ignition/gazebo/components/SemanticLabel.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; @@ -60,10 +62,8 @@ void Label::Configure(const Entity &_entity, return; } - const std::string parentName = _sdf->GetParent()->GetName(); - - // Set the component to the visual to set its user data, but - // if the plugin is inside the tag, get its visual child. + // 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)) { @@ -91,8 +91,8 @@ void Label::Configure(const Entity &_entity, } else { - ignerr << "Label plugin " + parentName + " is not a child of model or visual " - << "label will be ignored. \n"; + ignerr << "Entity [" << _entity << "] is not a visual, actor, or model. " + << "Label will be ignored. \n"; return; } } diff --git a/src/systems/label/Label.hh b/src/systems/label/Label.hh index 4a2a9cbaed..4c7555610a 100644 --- a/src/systems/label/Label.hh +++ b/src/systems/label/Label.hh @@ -29,11 +29,10 @@ 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 + /// for the parent entity's visuals. The plugin can be attached to models, + /// visuals, or actors. /// - /// Must contain exactly one