From b843232791563eabe15fba69cc91a51dc5600870 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Mon, 20 Jun 2022 09:16:50 -0600 Subject: [PATCH] Add bounding boxes into the label system plugin (#1040) Signed-off-by: AmrElsersy Signed-off-by: Ashton Larkin Signed-off-by: Louise Poubel Co-authored-by: AmrElsersy Co-authored-by: Louise Poubel --- CMakeLists.txt | 5 +- examples/worlds/boundingbox_camera.sdf | 415 ++++++++++++++++++ .../gazebo/components/BoundingBoxCamera.hh | 45 ++ src/SdfEntityCreator.cc | 6 + src/rendering/RenderUtil.cc | 43 ++ src/systems/sensors/CMakeLists.txt | 1 + src/systems/sensors/Sensors.cc | 15 +- 7 files changed, 527 insertions(+), 3 deletions(-) create mode 100644 examples/worlds/boundingbox_camera.sdf create mode 100644 include/ignition/gazebo/components/BoundingBoxCamera.hh diff --git a/CMakeLists.txt b/CMakeLists.txt index 4b9ff4329a..ce2bae341c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -119,7 +119,7 @@ set(IGN_PHYSICS_VER ${ignition-physics5_VERSION_MAJOR}) #-------------------------------------- # Find ignition-sensors -ign_find_package(ignition-sensors6 REQUIRED VERSION 6.1 +ign_find_package(ignition-sensors6 REQUIRED VERSION 6.6 # component order is important COMPONENTS # non-rendering @@ -138,6 +138,7 @@ ign_find_package(ignition-sensors6 REQUIRED VERSION 6.1 # cameras camera + boundingbox_camera segmentation_camera depth_camera rgbd_camera @@ -147,7 +148,7 @@ set(IGN_SENSORS_VER ${ignition-sensors6_VERSION_MAJOR}) #-------------------------------------- # Find ignition-rendering -ign_find_package(ignition-rendering6 REQUIRED VERSION 6.3) +ign_find_package(ignition-rendering6 REQUIRED VERSION 6.5) set(IGN_RENDERING_VER ${ignition-rendering6_VERSION_MAJOR}) #-------------------------------------- diff --git a/examples/worlds/boundingbox_camera.sdf b/examples/worlds/boundingbox_camera.sdf new file mode 100644 index 0000000000..a4c16af4c5 --- /dev/null +++ b/examples/worlds/boundingbox_camera.sdf @@ -0,0 +1,415 @@ + + + + + + + + + + + + ogre2 + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 1.0 1.0 1.0 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + boxes_full_2d_image + + Full 2D + docked + 400 + 600 + + + + + boxes_visible_2d_image + + Visible 2D + docked + 400 + 600 + + + + + boxes_3d_image + + 3D + docked + 400 + 600 + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 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 + + + + + + + 0 -1 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 0 0 0.5 1 + 0 0 1 1 + 0 0 0.3 1 + + + + + + + + + + + + + 0 1 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 0.8 0.2 0 1 + 0.8 0 0 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 + + + + + + + + + 4 0 1.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + boxes_full_2d + + full_2d + 1.047 + + 800 + 600 + + + 0.1 + 10 + + + bounding_box_full_2d_data + + + 1 + 30 + true + + + + boxes_visible_2d + + visible_2d + 1.047 + + 800 + 600 + + + 0.1 + 10 + + + bounding_box_visible_2d_data + + + 1 + 30 + true + + + + boxes_3d + + 3d + 1.047 + + 800 + 600 + + + 0.1 + 10 + + + bounding_box_3d_data + + + 1 + 30 + true + + + + + + -1 0 3 0.0 0.0 1.57 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + + + + + + + + + diff --git a/include/ignition/gazebo/components/BoundingBoxCamera.hh b/include/ignition/gazebo/components/BoundingBoxCamera.hh new file mode 100644 index 0000000000..2451fd8f24 --- /dev/null +++ b/include/ignition/gazebo/components/BoundingBoxCamera.hh @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2022 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/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index d54e1f377f..afb49a05cb 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" @@ -868,6 +869,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) 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 23348a4cf9..415908ff9c 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -52,6 +52,7 @@ #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/ChildLinkName.hh" @@ -1644,6 +1645,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const std::string thermalCameraSuffix{"/image"}; const std::string gpuLidarSuffix{"/scan"}; const std::string segmentationCameraSuffix{"/segmentation"}; + const std::string boundingBoxCameraSuffix{"/boundingbox"}; // Get all the new worlds // TODO(anyone) Only one scene is supported for now @@ -1885,6 +1887,17 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( _parent->Data(), segmentationCameraSuffix); return true; }); + + // Create bounding box cameras + _ecm.Each( + [&](const Entity &_entity, + const components::BoundingBoxCamera *_boundingBoxCamera, + const components::ParentEntity *_parent)->bool + { + this->AddNewSensor(_ecm, _entity, _boundingBoxCamera->Data(), + _parent->Data(), boundingBoxCameraSuffix); + return true; + }); } } @@ -1898,6 +1911,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const std::string thermalCameraSuffix{"/image"}; const std::string gpuLidarSuffix{"/scan"}; const std::string segmentationCameraSuffix{"/segmentation"}; + const std::string boundingBoxCameraSuffix{"/boundingbox"}; // Get all the new worlds // TODO(anyone) Only one scene is supported for now @@ -2139,6 +2153,17 @@ void RenderUtilPrivate::CreateEntitiesRuntime( _parent->Data(), segmentationCameraSuffix); return true; }); + + // Create bounding box cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::BoundingBoxCamera *_boundingBoxCamera, + const components::ParentEntity *_parent)->bool + { + this->AddNewSensor(_ecm, _entity, _boundingBoxCamera->Data(), + _parent->Data(), boundingBoxCameraSuffix); + return true; + }); } } @@ -2299,6 +2324,16 @@ void RenderUtilPrivate::UpdateRenderingEntities( 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; + }); } ////////////////////////////////////////////////// @@ -2430,6 +2465,14 @@ void RenderUtilPrivate::RemoveRenderingEntities( 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/sensors/CMakeLists.txt b/src/systems/sensors/CMakeLists.txt index 8b56378885..7f616f6c3d 100644 --- a/src/systems/sensors/CMakeLists.txt +++ b/src/systems/sensors/CMakeLists.txt @@ -5,6 +5,7 @@ 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}::boundingbox_camera ignition-sensors${IGN_SENSORS_VER}::camera ignition-sensors${IGN_SENSORS_VER}::depth_camera ignition-sensors${IGN_SENSORS_VER}::gpu_lidar diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 7ba021d997..3a0573d58a 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -32,6 +32,7 @@ #include #include +#include #include #include #include @@ -43,6 +44,7 @@ #include "ignition/gazebo/components/Atmosphere.hh" #include "ignition/gazebo/components/BatterySoC.hh" +#include "ignition/gazebo/components/BoundingBoxCamera.hh" #include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/DepthCamera.hh" #include "ignition/gazebo/components/GpuLidar.hh" @@ -576,7 +578,8 @@ void Sensors::PostUpdate(const UpdateInfo &_info, _ecm.HasComponentType(components::GpuLidar::typeId) || _ecm.HasComponentType(components::RgbdCamera::typeId) || _ecm.HasComponentType(components::ThermalCamera::typeId) || - _ecm.HasComponentType(components::SegmentationCamera::typeId))) + _ecm.HasComponentType(components::SegmentationCamera::typeId) || + _ecm.HasComponentType(components::BoundingBoxCamera::typeId))) { igndbg << "Initialization needed" << std::endl; this->dataPtr->doInit = true; @@ -740,6 +743,11 @@ std::string Sensors::CreateSensor(const Entity &_entity, sensor = this->dataPtr->sensorManager.CreateSensor< sensors::ThermalCameraSensor>(_sdf); } + 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< @@ -861,6 +869,11 @@ bool SensorsPrivate::HasConnections(sensors::RenderingSensor *_sensor) const if (s) return s->HasConnections(); } + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } { auto s = dynamic_cast(_sensor); if (s)