From b9206e2e0a17b6d2794b8fab4debe8aba8a909bd Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 14 Mar 2022 09:59:56 -0700 Subject: [PATCH] Add support for wide angle camera in sensors system (#1215) Signed-off-by: Ian Chen Signed-off-by: Steve Peters Signed-off-by: William Lew Co-authored-by: Steve Peters Co-authored-by: William Lew Co-authored-by: Louise Poubel --- CMakeLists.txt | 1 + examples/worlds/sensors_demo.sdf | 1 - examples/worlds/wide_angle_camera.sdf | 355 ++++++++++++++++++ .../gazebo/components/WideAngleCamera.hh | 45 +++ src/SdfEntityCreator.cc | 6 + src/rendering/RenderUtil.cc | 43 +++ src/systems/sensors/CMakeLists.txt | 1 + src/systems/sensors/Sensors.cc | 10 +- test/integration/CMakeLists.txt | 1 + test/integration/wide_angle_camera.cc | 96 +++++ test/worlds/wide_angle_camera_sensor.sdf | 129 +++++++ 11 files changed, 686 insertions(+), 2 deletions(-) create mode 100644 examples/worlds/wide_angle_camera.sdf create mode 100644 include/ignition/gazebo/components/WideAngleCamera.hh create mode 100644 test/integration/wide_angle_camera.cc create mode 100644 test/worlds/wide_angle_camera_sensor.sdf diff --git a/CMakeLists.txt b/CMakeLists.txt index 57632347e6..8cb2dc1c20 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -142,6 +142,7 @@ ign_find_package(ignition-sensors7 REQUIRED depth_camera rgbd_camera thermal_camera + wide_angle_camera ) set(IGN_SENSORS_VER ${ignition-sensors7_VERSION_MAJOR}) diff --git a/examples/worlds/sensors_demo.sdf b/examples/worlds/sensors_demo.sdf index 31374a1a3e..7e5d996089 100644 --- a/examples/worlds/sensors_demo.sdf +++ b/examples/worlds/sensors_demo.sdf @@ -518,7 +518,6 @@ - 0 1 3 0.0 0.0 1.57 diff --git a/examples/worlds/wide_angle_camera.sdf b/examples/worlds/wide_angle_camera.sdf new file mode 100644 index 0000000000..3fef95edd4 --- /dev/null +++ b/examples/worlds/wide_angle_camera.sdf @@ -0,0 +1,355 @@ + + + + + + 0.001 + 1.0 + + + + + ogre + + + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 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 + + + + + false + 5 + 5 + floating + 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 + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + docked + + + + + + + docked + + + + + + Wide angle camera + floating + 350 + 315 + + wide_angle_camera + false + + + + + + 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 + + + + + + 20 20 0.1 + + + + + + + + 20 20 0.1 + + + + 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 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + true + 1 1 0.5 0 0.0 3.14 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 3.14 + + 800 + 600 + + + 0.1 + 100 + + + + + custom + + + + 1.05 + + 4 + + 1.0 + + tan + + + + + true + + 3.1415 + + 512 + + + 1 + 30 + wide_angle_camera + + + + + + 0 1 3 0.0 0.0 1.57 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + + + + + diff --git a/include/ignition/gazebo/components/WideAngleCamera.hh b/include/ignition/gazebo/components/WideAngleCamera.hh new file mode 100644 index 0000000000..ebb166a7a7 --- /dev/null +++ b/include/ignition/gazebo/components/WideAngleCamera.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_WIDEANGLECAMERA_HH_ +#define IGNITION_GAZEBO_COMPONENTS_WIDEANGLECAMERA_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 wide angle camera sensor, + /// sdf::Camera, information. + using WideAngleCamera = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.WideAngleCamera", + WideAngleCamera) +} +} +} +} +#endif diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index d54e1f377f..d77dd33c36 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -76,6 +76,7 @@ #include "ignition/gazebo/components/Transparency.hh" #include "ignition/gazebo/components/Visibility.hh" #include "ignition/gazebo/components/Visual.hh" +#include "ignition/gazebo/components/WideAngleCamera.hh" #include "ignition/gazebo/components/WindMode.hh" #include "ignition/gazebo/components/World.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::WIDE_ANGLE_CAMERA) + { + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::WideAngleCamera(*_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 759b5588e3..90f1a48f69 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -86,6 +86,7 @@ #include "ignition/gazebo/components/Visibility.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/VisualCmd.hh" +#include "ignition/gazebo/components/WideAngleCamera.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" @@ -1641,6 +1642,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const std::string thermalCameraSuffix{"/image"}; const std::string gpuLidarSuffix{"/scan"}; const std::string segmentationCameraSuffix{"/segmentation"}; + const std::string wideAngleCameraSuffix{"/image"}; // Get all the new worlds // TODO(anyone) Only one scene is supported for now @@ -1882,6 +1884,17 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( _parent->Data(), segmentationCameraSuffix); return true; }); + + // Create wide angle cameras + _ecm.Each( + [&](const Entity &_entity, + const components::WideAngleCamera *_wideAngleCamera, + const components::ParentEntity *_parent)->bool + { + this->AddNewSensor(_ecm, _entity, _wideAngleCamera->Data(), + _parent->Data(), wideAngleCameraSuffix); + return true; + }); } } @@ -1895,6 +1908,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const std::string thermalCameraSuffix{"/image"}; const std::string gpuLidarSuffix{"/scan"}; const std::string segmentationCameraSuffix{"/segmentation"}; + const std::string wideAngleCameraSuffix{"/image"}; // Get all the new worlds // TODO(anyone) Only one scene is supported for now @@ -2136,6 +2150,17 @@ void RenderUtilPrivate::CreateEntitiesRuntime( _parent->Data(), segmentationCameraSuffix); return true; }); + + // Create wide angle cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::WideAngleCamera *_wideAngleCamera, + const components::ParentEntity *_parent)->bool + { + this->AddNewSensor(_ecm, _entity, _wideAngleCamera->Data(), + _parent->Data(), wideAngleCameraSuffix); + return true; + }); } } @@ -2296,6 +2321,16 @@ void RenderUtilPrivate::UpdateRenderingEntities( this->entityPoses[_entity] = _pose->Data(); return true; }); + + // Update wide angle cameras + _ecm.Each( + [&](const Entity &_entity, + const components::WideAngleCamera *, + const components::Pose *_pose)->bool + { + this->entityPoses[_entity] = _pose->Data(); + return true; + }); } ////////////////////////////////////////////////// @@ -2427,6 +2462,14 @@ void RenderUtilPrivate::RemoveRenderingEntities( return true; }); + // wide angle cameras + _ecm.EachRemoved( + [&](const Entity &_entity, const components::WideAngleCamera *)->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..f198dcd924 100644 --- a/src/systems/sensors/CMakeLists.txt +++ b/src/systems/sensors/CMakeLists.txt @@ -13,5 +13,6 @@ gz_add_system(sensors ignition-sensors${IGN_SENSORS_VER}::rgbd_camera ignition-sensors${IGN_SENSORS_VER}::segmentation_camera ignition-sensors${IGN_SENSORS_VER}::thermal_camera + ignition-sensors${IGN_SENSORS_VER}::wide_angle_camera ) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index d5309a82c7..0d678ecc75 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" @@ -49,6 +50,7 @@ #include "ignition/gazebo/components/RgbdCamera.hh" #include "ignition/gazebo/components/SegmentationCamera.hh" #include "ignition/gazebo/components/ThermalCamera.hh" +#include "ignition/gazebo/components/WideAngleCamera.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Events.hh" #include "ignition/gazebo/EntityComponentManager.hh" @@ -501,7 +503,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::WideAngleCamera::typeId))) { igndbg << "Initialization needed" << std::endl; this->dataPtr->doInit = true; @@ -606,6 +609,11 @@ std::string Sensors::CreateSensor(const Entity &_entity, sensor = this->dataPtr->sensorManager.CreateSensor< sensors::SegmentationCameraSensor>(_sdf); } + else if (_sdf.Type() == sdf::SensorType::WIDE_ANGLE_CAMERA) + { + sensor = this->dataPtr->sensorManager.CreateSensor< + sensors::WideAngleCameraSensor>(_sdf); + } if (nullptr == sensor) { diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 4d38836b90..5b9dc95c13 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -83,6 +83,7 @@ set(tests_needing_display shader_param_system.cc thermal_system.cc thermal_sensor_system.cc + wide_angle_camera.cc ) # Add systems that need a valid display here. diff --git a/test/integration/wide_angle_camera.cc b/test/integration/wide_angle_camera.cc new file mode 100644 index 0000000000..a4e224aa24 --- /dev/null +++ b/test/integration/wide_angle_camera.cc @@ -0,0 +1,96 @@ +/* + * 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. + * + */ + +#include + +#include + +#include +#include +#include +#include +#include + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/test_config.hh" + +#include "plugins/MockSystem.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test WideAndleCameraTest system +class WideAngleCameraTest : public InternalFixture<::testing::Test> +{ +}; + +std::mutex mutex; +msgs::Image imageMsg; +unsigned char *imageBuffer = nullptr; + +///////////////////////////////////////////////// +void imageCb(const msgs::Image &_msg) +{ + ASSERT_EQ(msgs::PixelFormatType::RGB_INT8, + _msg.pixel_format_type()); + + mutex.lock(); + unsigned int imageSamples = _msg.width() * _msg.height() * 3; + + if (!imageBuffer) + imageBuffer = new unsigned char[imageSamples]; + memcpy(imageBuffer, _msg.data().c_str(), imageSamples); + mutex.unlock(); +} + +///////////////////////////////////////////////// +// The test checks the Wide Angle Camera readings +TEST_F(WideAngleCameraTest, IGN_UTILS_TEST_DISABLED_ON_MAC(WideAngleCameraBox)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "wide_angle_camera_sensor.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // subscribe to the image topic + transport::Node node; + node.Subscribe("/wide_angle_camera", &imageCb); + + // Run server and verify that we are receiving a message + // from the wide angle camera + size_t iters100 = 100u; + server.Run(true, iters100, false); + + int sleep{0}; + int maxSleep{30}; + while (imageBuffer == nullptr && sleep < maxSleep) + { + std::this_thread::sleep_for(100ms); + sleep++; + } + EXPECT_LT(sleep, maxSleep); + ASSERT_NE(imageBuffer, nullptr); + + delete[] imageBuffer; +} diff --git a/test/worlds/wide_angle_camera_sensor.sdf b/test/worlds/wide_angle_camera_sensor.sdf new file mode 100644 index 0000000000..4e02add3c5 --- /dev/null +++ b/test/worlds/wide_angle_camera_sensor.sdf @@ -0,0 +1,129 @@ + + + + + 0.001 + 1.0 + + + + + ogre + + + + + + 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 + + + + + + 20 20 0.1 + + + + + + + + 20 20 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + true + 1 1 0.5 0 0.0 3.14 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 3.14 + + 256 + 256 + + + 0.1 + 100 + + + + + custom + + + + 1.05 + + 4 + + 1.0 + + tan + + + + + true + + 3.1415 + + 512 + + + 10 + wide_angle_camera + + + + +