From 36f5d84e62d06f4b69f9e6638485d0055a4e90fc Mon Sep 17 00:00:00 2001 From: methylDragon Date: Thu, 1 Dec 2022 05:29:52 +0800 Subject: [PATCH 01/26] ign -> gz Migrate Ignition Headers : gz-sensors (#260) * Migrate headers Signed-off-by: methylDragon * Add redirection headers Signed-off-by: methylDragon * Migrate include statements Signed-off-by: methylDragon * Leave ignition as primary in headers to fix ABI Signed-off-by: methylDragon * Update ignition names Signed-off-by: Nate Koenig * tweaks Signed-off-by: Nate Koenig Signed-off-by: methylDragon Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 6 +- CONTRIBUTING.md | 2 +- Changelog.md | 178 +++++------ Migration.md | 12 +- NEWS | 2 +- api.md.in | 4 +- examples/imu_noise/CMakeLists.txt | 2 +- examples/imu_noise/main.cc | 8 +- examples/save_image/CMakeLists.txt | 2 +- examples/save_image/main.cc | 64 ++-- include/CMakeLists.txt | 3 +- include/{ignition => gz}/CMakeLists.txt | 0 include/gz/sensors/AirPressureSensor.hh | 91 ++++++ include/gz/sensors/AltimeterSensor.hh | 108 +++++++ .../{ignition => gz}/sensors/CMakeLists.txt | 0 include/gz/sensors/CameraSensor.hh | 181 +++++++++++ include/gz/sensors/DepthCameraSensor.hh | 181 +++++++++++ include/gz/sensors/GaussianNoiseModel.hh | 77 +++++ include/gz/sensors/GpuLidarSensor.hh | 140 +++++++++ include/gz/sensors/ImageGaussianNoiseModel.hh | 76 +++++ include/gz/sensors/ImageNoise.hh | 65 ++++ include/gz/sensors/ImuSensor.hh | 135 ++++++++ include/gz/sensors/Lidar.hh | 270 ++++++++++++++++ include/gz/sensors/LogicalCameraSensor.hh | 125 ++++++++ include/gz/sensors/MagnetometerSensor.hh | 104 +++++++ include/gz/sensors/Manager.hh | 218 +++++++++++++ include/gz/sensors/Noise.hh | 125 ++++++++ include/gz/sensors/RenderingEvents.hh | 68 +++++ include/gz/sensors/RenderingSensor.hh | 99 ++++++ include/gz/sensors/RgbdCameraSensor.hh | 102 +++++++ include/gz/sensors/Sensor.hh | 216 +++++++++++++ include/gz/sensors/SensorFactory.hh | 195 ++++++++++++ include/gz/sensors/SensorTypes.hh | 181 +++++++++++ include/gz/sensors/ThermalCameraSensor.hh | 185 +++++++++++ include/{ignition => gz}/sensors/config.hh.in | 11 +- .../{ignition => gz}/sensors/sensors.hh.in | 2 +- include/ignition/sensors.hh | 19 ++ include/ignition/sensors/AirPressureSensor.hh | 78 +---- include/ignition/sensors/AltimeterSensor.hh | 95 +----- include/ignition/sensors/CameraSensor.hh | 170 +---------- include/ignition/sensors/DepthCameraSensor.hh | 170 +---------- include/ignition/sensors/Export.hh | 19 ++ .../ignition/sensors/GaussianNoiseModel.hh | 66 +--- include/ignition/sensors/GpuLidarSensor.hh | 129 +------- .../sensors/ImageGaussianNoiseModel.hh | 65 +--- include/ignition/sensors/ImageNoise.hh | 54 +--- include/ignition/sensors/ImuSensor.hh | 122 +------- include/ignition/sensors/Lidar.hh | 259 +--------------- .../ignition/sensors/LogicalCameraSensor.hh | 114 +------ .../ignition/sensors/MagnetometerSensor.hh | 91 +----- include/ignition/sensors/Manager.hh | 205 +------------ include/ignition/sensors/Noise.hh | 112 +------ include/ignition/sensors/RenderingEvents.hh | 55 +--- include/ignition/sensors/RenderingSensor.hh | 88 +----- include/ignition/sensors/RgbdCameraSensor.hh | 91 +----- include/ignition/sensors/Sensor.hh | 203 +----------- include/ignition/sensors/SensorFactory.hh | 182 +---------- include/ignition/sensors/SensorTypes.hh | 168 +--------- .../ignition/sensors/ThermalCameraSensor.hh | 174 +---------- .../ignition/sensors/air_pressure/Export.hh | 19 ++ include/ignition/sensors/altimeter/Export.hh | 19 ++ include/ignition/sensors/camera/Export.hh | 19 ++ include/ignition/sensors/config.hh | 42 +++ .../ignition/sensors/depth_camera/Export.hh | 19 ++ include/ignition/sensors/gpu_lidar/Export.hh | 19 ++ include/ignition/sensors/imu/Export.hh | 19 ++ include/ignition/sensors/lidar/Export.hh | 19 ++ .../ignition/sensors/logical_camera/Export.hh | 19 ++ .../ignition/sensors/magnetometer/Export.hh | 19 ++ include/ignition/sensors/rendering/Export.hh | 19 ++ .../ignition/sensors/rgbd_camera/Export.hh | 19 ++ .../ignition/sensors/thermal_camera/Export.hh | 19 ++ src/AirPressureSensor.cc | 20 +- src/AltimeterSensor.cc | 18 +- src/CameraSensor.cc | 40 +-- src/Camera_TEST.cc | 32 +- src/DepthCameraSensor.cc | 48 +-- src/GaussianNoiseModel.cc | 12 +- src/GpuLidarSensor.cc | 38 +-- src/ImageGaussianNoiseModel.cc | 16 +- src/ImageNoise.cc | 8 +- src/ImuSensor.cc | 18 +- src/ImuSensor_TEST.cc | 13 +- src/Lidar.cc | 50 +-- src/Lidar_TEST.cc | 38 +-- src/LogicalCameraSensor.cc | 18 +- src/MagnetometerSensor.cc | 18 +- src/Manager.cc | 22 +- src/Manager_TEST.cc | 18 +- src/Noise.cc | 10 +- src/Noise_TEST.cc | 6 +- src/PointCloudUtil.cc | 2 +- src/PointCloudUtil.hh | 12 +- src/RenderingEvents.cc | 10 +- src/RenderingSensor.cc | 12 +- src/RgbdCameraSensor.cc | 40 +-- src/Sensor.cc | 36 +-- src/SensorFactory.cc | 18 +- src/SensorTypes.cc | 4 +- src/Sensor_TEST.cc | 14 +- src/ThermalCameraSensor.cc | 46 +-- test/integration/TransportTestTools.hh | 4 +- test/integration/air_pressure_plugin.cc | 64 ++-- test/integration/altimeter_plugin.cc | 64 ++-- test/integration/camera_plugin.cc | 48 +-- test/integration/deprecated_TEST.cc | 26 ++ test/integration/depth_camera_plugin.cc | 102 +++---- test/integration/gpu_lidar_sensor_plugin.cc | 288 +++++++++--------- test/integration/imu_plugin.cc | 108 +++---- test/integration/logical_camera_plugin.cc | 98 +++--- test/integration/magnetometer_plugin.cc | 104 +++---- test/integration/rgbd_camera_plugin.cc | 26 +- test/integration/thermal_camera_plugin.cc | 78 ++--- test/test_config.h.in | 8 +- tutorials.md.in | 6 +- tutorials/01_intro.md | 8 +- tutorials/02_install.md | 30 +- tutorials/thermal_camera.md | 30 +- 118 files changed, 4356 insertions(+), 3610 deletions(-) rename include/{ignition => gz}/CMakeLists.txt (100%) create mode 100644 include/gz/sensors/AirPressureSensor.hh create mode 100644 include/gz/sensors/AltimeterSensor.hh rename include/{ignition => gz}/sensors/CMakeLists.txt (100%) create mode 100644 include/gz/sensors/CameraSensor.hh create mode 100644 include/gz/sensors/DepthCameraSensor.hh create mode 100644 include/gz/sensors/GaussianNoiseModel.hh create mode 100644 include/gz/sensors/GpuLidarSensor.hh create mode 100644 include/gz/sensors/ImageGaussianNoiseModel.hh create mode 100644 include/gz/sensors/ImageNoise.hh create mode 100644 include/gz/sensors/ImuSensor.hh create mode 100644 include/gz/sensors/Lidar.hh create mode 100644 include/gz/sensors/LogicalCameraSensor.hh create mode 100644 include/gz/sensors/MagnetometerSensor.hh create mode 100644 include/gz/sensors/Manager.hh create mode 100644 include/gz/sensors/Noise.hh create mode 100644 include/gz/sensors/RenderingEvents.hh create mode 100644 include/gz/sensors/RenderingSensor.hh create mode 100644 include/gz/sensors/RgbdCameraSensor.hh create mode 100644 include/gz/sensors/Sensor.hh create mode 100644 include/gz/sensors/SensorFactory.hh create mode 100644 include/gz/sensors/SensorTypes.hh create mode 100644 include/gz/sensors/ThermalCameraSensor.hh rename include/{ignition => gz}/sensors/config.hh.in (72%) rename include/{ignition => gz}/sensors/sensors.hh.in (52%) create mode 100644 include/ignition/sensors.hh create mode 100644 include/ignition/sensors/Export.hh create mode 100644 include/ignition/sensors/air_pressure/Export.hh create mode 100644 include/ignition/sensors/altimeter/Export.hh create mode 100644 include/ignition/sensors/camera/Export.hh create mode 100644 include/ignition/sensors/config.hh create mode 100644 include/ignition/sensors/depth_camera/Export.hh create mode 100644 include/ignition/sensors/gpu_lidar/Export.hh create mode 100644 include/ignition/sensors/imu/Export.hh create mode 100644 include/ignition/sensors/lidar/Export.hh create mode 100644 include/ignition/sensors/logical_camera/Export.hh create mode 100644 include/ignition/sensors/magnetometer/Export.hh create mode 100644 include/ignition/sensors/rendering/Export.hh create mode 100644 include/ignition/sensors/rgbd_camera/Export.hh create mode 100644 include/ignition/sensors/thermal_camera/Export.hh create mode 100644 test/integration/deprecated_TEST.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 8423159a..932cbdef 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,14 +13,16 @@ find_package(ignition-cmake2 2.13 REQUIRED) #============================================================================ # Configure the project #============================================================================ -ign_configure_project() +ign_configure_project( + REPLACE_IGNITION_INCLUDE_PATH gz/sensors +) #============================================================================ # Set project-specific options #============================================================================ set (DRI_TESTS TRUE CACHE BOOL "True to enable DRI tests") -option(ENABLE_PROFILER "Enable Ignition Profiler" FALSE) +option(ENABLE_PROFILER "Enable Gazebo Profiler" FALSE) if(ENABLE_PROFILER) add_definitions("-DIGN_PROFILER_ENABLE=1") diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 147239ce..a1c121ea 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1 +1 @@ -See the [Ignition Robotics contributing guide](https://ignitionrobotics.org/docs/all/contributing). +See the [Gazebo contributing guide](https://gazebosim.org/docs/all/contributing). diff --git a/Changelog.md b/Changelog.md index 90df3dd1..071c3ab8 100644 --- a/Changelog.md +++ b/Changelog.md @@ -5,7 +5,7 @@ 1. Remove redundant namespace references * [Pull request #258](https://github.com/gazebosim/gz-sensors/pull/258) -1. Ignition -> Gazebo +1. Gazebo -> Gazebo * [Pull request #245](https://github.com/gazebosim/gz-sensors/pull/245) 1. Conform to ros format for header field `frame_id` of sensor msgs @@ -17,241 +17,241 @@ 1. Require ign-transport >= 8.2 * [Pull request #167](https://github.com/gazebosim/gz-sensors/pull/167) -### Ignition Sensors 3.3.0 (2021-08-26) +### Gazebo Sensors 3.3.0 (2021-08-26) 1. 👩‍🌾 Print debug messages when sensors advertise topics - * [Pull request #151](https://github.com/ignitionrobotics/ign-sensors/pull/151) + * [Pull request #151](https://github.com/gazebosim/gz-sensors/pull/151) 1. Publish performance sensor metrics. - * [Pull request #146](https://github.com/ignitionrobotics/ign-sensors/pull/146) + * [Pull request #146](https://github.com/gazebosim/gz-sensors/pull/146) 1. CI and infrastructure - * [Pull request #130](https://github.com/ignitionrobotics/ign-sensors/pull/130) - * [Pull request #150](https://github.com/ignitionrobotics/ign-sensors/pull/150) - * [Pull request #106](https://github.com/ignitionrobotics/ign-sensors/pull/106) + * [Pull request #130](https://github.com/gazebosim/gz-sensors/pull/130) + * [Pull request #150](https://github.com/gazebosim/gz-sensors/pull/150) + * [Pull request #106](https://github.com/gazebosim/gz-sensors/pull/106) 1. 👩‍🌾 Disable tests that consistently fail on macOS - * [Pull request #121](https://github.com/ignitionrobotics/ign-sensors/pull/121) + * [Pull request #121](https://github.com/gazebosim/gz-sensors/pull/121) 1. 👩‍🌾 Clear Windows warnings (backport #58) - * [Pull request #58](https://github.com/ignitionrobotics/ign-sensors/pull/58) + * [Pull request #58](https://github.com/gazebosim/gz-sensors/pull/58) 1. Fix macOS/windows tests that failed to load library (backport #60) - * [Pull request #60](https://github.com/ignitionrobotics/ign-sensors/pull/60) + * [Pull request #60](https://github.com/gazebosim/gz-sensors/pull/60) -### Ignition Sensors 3.2.0 (2021-02-08) +### Gazebo Sensors 3.2.0 (2021-02-08) 1. Apply noise to lidar point cloud. - * [Pull request 86](https://github.com/ignitionrobotics/ign-sensors/pull/86) + * [Pull request 86](https://github.com/gazebosim/gz-sensors/pull/86) 1. Add Windows Installation. - * [Pull request 82](https://github.com/ignitionrobotics/ign-sensors/pull/82) + * [Pull request 82](https://github.com/gazebosim/gz-sensors/pull/82) 1. Added thermal camera tutorial. - * [Pull request 61](https://github.com/ignitionrobotics/ign-sensors/pull/61) + * [Pull request 61](https://github.com/gazebosim/gz-sensors/pull/61) 1. Prevent segfaults on test failures, make tests verbose. - * [Pull request 56](https://github.com/ignitionrobotics/ign-sensors/pull/56) + * [Pull request 56](https://github.com/gazebosim/gz-sensors/pull/56) 1. Resolve updated codecheck issues. - * [Pull request 57](https://github.com/ignitionrobotics/ign-sensors/pull/57) + * [Pull request 57](https://github.com/gazebosim/gz-sensors/pull/57) 1. Improve fork experience. - * [Pull request 54](https://github.com/ignitionrobotics/ign-sensors/pull/54) + * [Pull request 54](https://github.com/gazebosim/gz-sensors/pull/54) -### Ignition Sensors 3.1.0 (2020-09-03) +### Gazebo Sensors 3.1.0 (2020-09-03) 1. Update camera sensor only when needed - * [Pull request 37](https://github.com/ignitionrobotics/ign-sensors/pull/37) + * [Pull request 37](https://github.com/gazebosim/gz-sensors/pull/37) 1. Add noise to RGBD camera. - * [Pull Request 35](https://github.com/ignitionrobotics/ign-sensors/pull/35) + * [Pull Request 35](https://github.com/gazebosim/gz-sensors/pull/35) 1. Fix version numbers in config.hh - * [Pull Request 42](https://github.com/ignitionrobotics/ign-sensors/pull/42) + * [Pull Request 42](https://github.com/gazebosim/gz-sensors/pull/42) 1. Make sure all sensors have a default topic. When invalid topics are passed in, convert them to valid topics if possible. If not possible to convert into valid topic, fail gracefully. - * [Pull Request 33](https://github.com/ignitionrobotics/ign-sensors/pull/33) + * [Pull Request 33](https://github.com/gazebosim/gz-sensors/pull/33) 1. GitHub migration - * [Pull request 11](https://github.com/ignitionrobotics/ign-sensors/pull/11) - * [Pull request 21](https://github.com/ignitionrobotics/ign-sensors/pull/21) + * [Pull request 11](https://github.com/gazebosim/gz-sensors/pull/11) + * [Pull request 21](https://github.com/gazebosim/gz-sensors/pull/21) -### Ignition Sensors 3.0.0 (2019-12-10) +### Gazebo Sensors 3.0.0 (2019-12-10) 1. Add support for sdformat frame semantics - * [BitBucket pull request 104](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/104) + * [BitBucket pull request 104](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/104) 1. Remove deprecations in ign-sensors3 - * [BitBucket pull request 103](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/103) + * [BitBucket pull request 103](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/103) 1. Break out image noise classes - * [BitBucket pull request 102](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/102) + * [BitBucket pull request 102](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/102) 1. Depend on ign-transport8, ign-msgs5, sdformat9 - * [BitBucket pull request 101](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/101) - * [BitBucket pull request 105](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/105) + * [BitBucket pull request 101](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/101) + * [BitBucket pull request 105](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/105) 1. Add Thermal Camera Sensor - * [BitBucket pull request 100](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/100) + * [BitBucket pull request 100](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/100) 1. Updating exports and includes - * [BitBucket pull request 98](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/98) + * [BitBucket pull request 98](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/98) 1. Removed deprecations from Manager. - * [BitBucket pull request 99](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/99) + * [BitBucket pull request 99](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/99) 1. Depend on ign-rendering3 - * [BitBucket pull request 88](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/88) + * [BitBucket pull request 88](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/88) -## Ignition Sensors 2 +## Gazebo Sensors 2 -### Ignition Sensors 2.9.1 (2020-12-23) +### Gazebo Sensors 2.9.1 (2020-12-23) 1. Fix version numbers in config.hh - * [Pull Request 42](https://github.com/ignitionrobotics/ign-sensors/pull/42) + * [Pull Request 42](https://github.com/gazebosim/gz-sensors/pull/42) 1. Resolve codecheck issues - * [Pull Request 57](https://github.com/ignitionrobotics/ign-sensors/pull/57) + * [Pull Request 57](https://github.com/gazebosim/gz-sensors/pull/57) -### Ignition Sensors 2.9.0 (2020-08-07) +### Gazebo Sensors 2.9.0 (2020-08-07) 1. Add noise to RGBD camera. - * [Pull Request 35](https://github.com/ignitionrobotics/ign-sensors/pull/35) + * [Pull Request 35](https://github.com/gazebosim/gz-sensors/pull/35) 1. Make sure all sensors have a default topic.When invalid topics are passed in, convert them to valid topics if possible. If not possible to convert into valid topic, fail gracefully. - * [Pull Request 33](https://github.com/ignitionrobotics/ign-sensors/pull/33) + * [Pull Request 33](https://github.com/gazebosim/gz-sensors/pull/33) -### Ignition Sensors 2.8.0 (2020-03-04) +### Gazebo Sensors 2.8.0 (2020-03-04) 1. Added sequence numbers to sensor data messages. - * [BitBucket pull request 112](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/112) + * [BitBucket pull request 112](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/112) -### Ignition Sensors 2.7.0 (2019-12-16) +### Gazebo Sensors 2.7.0 (2019-12-16) 1. Add clipping for depth camera on rgbd camera sensor (requires sdformat 8.7.0) - * [BitBucket pull request 107](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/107) + * [BitBucket pull request 107](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/107) -### Ignition Sensors 2.6.1 (2019-09-13) +### Gazebo Sensors 2.6.1 (2019-09-13) 1. Fix IMU noise model dt - * [BitBucket pull request 94](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/94) + * [BitBucket pull request 94](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/94) -### Ignition Sensors 2.6.0 (2019-08-27) +### Gazebo Sensors 2.6.0 (2019-08-27) 1. Update depth and rgbd camera sensor to output point cloud data generated by ign-rendering DepthCamera - * [BitBucket pull request 91](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/91) + * [BitBucket pull request 91](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/91) -### Ignition Sensors 2.5.1 (2019-08-12) +### Gazebo Sensors 2.5.1 (2019-08-12) 1. Add intensity and ring fields to GpuLidarSensor point cloud msg - * [BitBucket pull request 89](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/89) + * [BitBucket pull request 89](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/89) -### Ignition Sensors 2.5.0 +### Gazebo Sensors 2.5.0 1. Add `IGN_PROFILER_ENABLE` cmake option for enabling the ign-common profiler. - * [BitBucket pull request 82](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/82) + * [BitBucket pull request 82](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/82) 1. Deduplicate `frame_ids` from sensor message headers - * [BitBucket pull request 83](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/83) + * [BitBucket pull request 83](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/83) 1. Baseline for stereo cameras - * [BitBucket pull request 84](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/84) + * [BitBucket pull request 84](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/84) -### Ignition Sensors 2.4.0 (2019-07-17) +### Gazebo Sensors 2.4.0 (2019-07-17) 1. Support manual scene updates for rendering sensors - * [BitBucket pull request 81](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/81) + * [BitBucket pull request 81](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/81) -### Ignition Sensors 2.3.0 (2019-07-16) +### Gazebo Sensors 2.3.0 (2019-07-16) 1. The GpuLidar and Rgbd sensors publish point cloud data using `msgs::PointCloudPacked`. - * [BitBucket pull request 78](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/78) + * [BitBucket pull request 78](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/78) -### Ignition Sensors 2.2.0 (2019-06-27) +### Gazebo Sensors 2.2.0 (2019-06-27) 1. Update the GPU Lidar to use the sensor's name as the `frame_id`. - * [BitBucket pull request 74](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/74) + * [BitBucket pull request 74](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/74) 1. Fix camera_info topic to be on the same level as image and depth_image for RGBD Camera. - * [BitBucket pull request 73](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/73) + * [BitBucket pull request 73](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/73) -### Ignition Sensors 2.1.0 (2019-06-18) +### Gazebo Sensors 2.1.0 (2019-06-18) 1. Adds an RGBD camera sensor that combines a CameraSensor and DepthCameraSensor, and also outputs a pointcloud. - * [BitBucket pull request 70](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/70) + * [BitBucket pull request 70](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/70) 1. Create and publish on `camera_info` topics for the Camera and DepthCamera sensors. - * [BitBucket pull request 67](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/67) + * [BitBucket pull request 67](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/67) -### Ignition Sensors 2.0.0 (2019-05-21) +### Gazebo Sensors 2.0.0 (2019-05-21) 1. Zero update rate, virtual SetParent and fix gpu_lidar - * [BitBucket pull request 66](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/66) + * [BitBucket pull request 66](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/66) 1. Add `frame_id` to sensor messages - * [BitBucket pull request 63](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/63) + * [BitBucket pull request 63](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/63) 1. Restore `pixel_format` in message and add deprecation comment. - * [BitBucket pull request 62](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/62) - * [BitBucket pull request 65](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/65) + * [BitBucket pull request 62](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/62) + * [BitBucket pull request 65](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/65) 1. Added noise to camera and lidar sensors. - * [BitBucket pull request 60](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/60) - * [BitBucket pull request 61](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/61) + * [BitBucket pull request 60](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/60) + * [BitBucket pull request 61](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/61) 1. Add support for loading a Lidar sensor from an SDF Sensor DOM object. - * [BitBucket pull request 59](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/59) + * [BitBucket pull request 59](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/59) 1. Add support for loading an IMU sensor from an SDF Sensor DOM object. - * [BitBucket pull request 58](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/58) + * [BitBucket pull request 58](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/58) 1. Add support for loading a camera and depth camera sensor from an SDF Sensor DOM object. - * [BitBucket pull request 57](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/57) + * [BitBucket pull request 57](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/57) 1. Add support for loading an air pressure sensor from an SDF Sensor DOM object. - * [BitBucket pull request 56](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/56) + * [BitBucket pull request 56](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/56) 1. Add support for loading an altimeter sensor from an SDF Sensor DOM object. - * [BitBucket pull request 55](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/55) + * [BitBucket pull request 55](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/55) 1. Noise factory uses `sdf::Noise` objects, Magnetometer sensor utilizes noise parameters. - * [BitBucket pull request 54](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/54) + * [BitBucket pull request 54](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/54) 1. Add support for loading a magnetometer sensor from an SDF Sensor DOM object. - * [BitBucket pull request 53](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/53) + * [BitBucket pull request 53](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/53) 1. Add magnetometer - * [BitBucket pull request 47](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/47) + * [BitBucket pull request 47](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/47) 1. Add IMU - * [BitBucket pull request 44](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/44) + * [BitBucket pull request 44](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/44) 1. Add altimeter - * [BitBucket pull request 43](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/43) + * [BitBucket pull request 43](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/43) 1. Create component for rendering sensor classes - * [BitBucket pull request 42](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/42) + * [BitBucket pull request 42](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/42) -1. Upgrade to ignition-rendering2 - * [BitBucket pull request 45](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/45) +1. Upgrade to gz-rendering2 + * [BitBucket pull request 45](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/45) -1. Upgrade to ignition-msgs4 and ignition-transport7 - * [BitBucket pull request 51](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/51) +1. Upgrade to gz-msgs4 and gz-transport7 + * [BitBucket pull request 51](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/51) -### Ignition Sensors 1.X.X (2019-XX-XX) +### Gazebo Sensors 1.X.X (2019-XX-XX) 1. Fix windows linking * [BitBucket pull request 49](https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-requests/49) * [Issue 6](https://github.com/osrf/gazebo/issues/6) -### Ignition Sensors 1.0.0 (2019-03-01) +### Gazebo Sensors 1.0.0 (2019-03-01) diff --git a/Migration.md b/Migration.md index 9286cd69..8ffd6ff3 100644 --- a/Migration.md +++ b/Migration.md @@ -6,7 +6,7 @@ notification to users that their code should be upgraded. The next major release will remove the deprecated code. -## Ignition Sensors 2.X to 3.X +## Gazebo Sensors 2.X to 3.X ### Additions @@ -24,7 +24,7 @@ and header files. Similarly, noise models for rendering sensors need be created using the new ImageNoiseFactory class instead of NoiseFactory by including ImageNoise.hh. -## Ignition Sensors 1.X to 2.X +## Gazebo Sensors 1.X to 2.X ### Additions @@ -43,16 +43,16 @@ ImageNoise.hh. + ***Replacement*** virtual void Load(const sdf::Noise &_sdf) 1. **include/sensors/Events.hh** - + ***Deprecation:*** public: static ignition::common::ConnectionPtr ConnectSceneChangeCallback(std::function) + + ***Deprecation:*** public: static gz::common::ConnectionPtr ConnectSceneChangeCallback(std::function) + ***Replacement:*** RenderingEvents::ConnectSceneChangeCallback 1. **include/sensors/Manager.hh** - + ***Deprecation:*** public: bool Init(ignition::rendering::ScenePtr); + + ***Deprecation:*** public: bool Init(gz::rendering::ScenePtr); + ***Replacement:*** RenderingSensor::SetScene - + ***Deprecation:*** public: void SetRenderingScene(ignition::rendering::ScenePtr + + ***Deprecation:*** public: void SetRenderingScene(gz::rendering::ScenePtr + ***Replacement:*** RenderingSensor::SetScene - + ***Deprecation:*** public: ignition::rendering::ScenePtr RenderingScene() const + + ***Deprecation:*** public: gz::rendering::ScenePtr RenderingScene() const + ***Replacement:*** RenderingSensor::Scene() 1. **include/sensors/Noise.hh** diff --git a/NEWS b/NEWS index 604eba66..a73d9b7c 100644 --- a/NEWS +++ b/NEWS @@ -1 +1 @@ -http://ignitionrobotics.org +http://gazebosim.org diff --git a/api.md.in b/api.md.in index 31af8657..afd386f2 100644 --- a/api.md.in +++ b/api.md.in @@ -1,6 +1,6 @@ -## Ignition @GZ_DESIGNATION_CAP@ +## Gazebo @GZ_DESIGNATION_CAP@ -Ignition @GZ_DESIGNATION_CAP@ is a component in Ignition Robotics, a set of libraries +Gazebo @GZ_DESIGNATION_CAP@ is a component in Gazebo Robotics, a set of libraries designed to rapidly develop robot and simulation applications. ## License diff --git a/examples/imu_noise/CMakeLists.txt b/examples/imu_noise/CMakeLists.txt index 6a30fe88..f84a656a 100644 --- a/examples/imu_noise/CMakeLists.txt +++ b/examples/imu_noise/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.5 FATAL_ERROR) project(ignition-sensors-noise-demo) -# Find the Ignition Libraries used directly by the example +# Find the Gazebo Libraries used directly by the example find_package(ignition-sensors3 REQUIRED) add_executable(sensor_noise main.cc) diff --git a/examples/imu_noise/main.cc b/examples/imu_noise/main.cc index 128f4aca..cfd25934 100644 --- a/examples/imu_noise/main.cc +++ b/examples/imu_noise/main.cc @@ -20,9 +20,9 @@ #include -#include -#include -#include +#include +#include +#include static constexpr double kSampleFrequency = 100.0; // 16-bit ADC @@ -115,7 +115,7 @@ generateSamples(size_t _nSamples, const NoiseParameters& _params) noise.SetDynamicBiasCorrelationTime(_params.dynamicBiasCorrelationTime); const double dt = 1.0 / _params.sampleFreq; - auto model = ignition::sensors::NoiseFactory::NewNoiseModel(noise); + auto model = gz::sensors::NoiseFactory::NewNoiseModel(noise); for (size_t ii = 0; ii < _nSamples ; ++ii) { samples.push_back(model->Apply(0.0, dt)); } diff --git a/examples/save_image/CMakeLists.txt b/examples/save_image/CMakeLists.txt index efd2efdf..f59ad003 100644 --- a/examples/save_image/CMakeLists.txt +++ b/examples/save_image/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.5 FATAL_ERROR) project(ignition-sensors-camera-demo) -# Find the Ignition Libraries used directly by the example +# Find the Gazebo Libraries used directly by the example find_package(ignition-rendering3 REQUIRED OPTIONAL_COMPONENTS ogre ogre2) find_package(ignition-sensors3 REQUIRED COMPONENTS rendering camera) diff --git a/examples/save_image/main.cc b/examples/save_image/main.cc index ee63b631..94c6443e 100644 --- a/examples/save_image/main.cc +++ b/examples/save_image/main.cc @@ -21,9 +21,9 @@ #include #include -#include -#include -#include +#include +#include +#include // TODO(louise) Remove these pragmas once ign-rendering is disabling the // warnings @@ -31,39 +31,39 @@ #pragma warning(push) #pragma warning(disable: 4251) #endif -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #ifdef _WIN32 #pragma warning(pop) #endif -#include +#include -void OnImageFrame(const ignition::msgs::Image &_image) +void OnImageFrame(const gz::msgs::Image &_image) { const unsigned char *data = reinterpret_cast( _image.data().c_str()); - auto format = static_cast( + auto format = static_cast( _image.pixel_format_type()); - ignition::common::Image image; + gz::common::Image image; image.SetFromData(data, _image.width(), _image.height(), format); std::cout << "Saving image.png\n"; image.SavePNG("image.png"); } -void BuildScene(ignition::rendering::ScenePtr _scene); +void BuildScene(gz::rendering::ScenePtr _scene); int main() { // Setup ign-rendering with a scene #ifdef WITH_OGRE - auto engine = ignition::rendering::engine("ogre"); + auto engine = gz::rendering::engine("ogre"); #else #ifdef WITH_OGRE2 - auto engine = ignition::rendering::engine("ogre2"); + auto engine = gz::rendering::engine("ogre2"); #endif #endif @@ -72,13 +72,13 @@ int main() std::cerr << "Failed to load ogre\n"; return 1; } - ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); // Add stuff to take a picture of BuildScene(scene); // Create a sensor manager - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; // Create SDF describing a camera sensor const std::string name = "ExampleCamera"; @@ -113,7 +113,7 @@ int main() sensorSdf.SetCameraSensor(cameraSdf); // Create a CameraSensor - auto cameraSensor = mgr.CreateSensor( + auto cameraSensor = mgr.CreateSensor( sensorSdf); if (!cameraSensor) @@ -124,24 +124,24 @@ int main() cameraSensor->SetScene(scene); // Set a callback on the camera sensor to get a camera frame - ignition::common::ConnectionPtr connection = + gz::common::ConnectionPtr connection = cameraSensor->ConnectImageCallback(&OnImageFrame); // Force the camera to generate an image - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(gz::common::Time::Zero, true); return 0; } // Copy/paste from an ignition-rendering example -void BuildScene(ignition::rendering::ScenePtr _scene) +void BuildScene(gz::rendering::ScenePtr _scene) { // initialize _scene _scene->SetAmbientLight(0.3, 0.3, 0.3); - ignition::rendering::VisualPtr root = _scene->RootVisual(); + gz::rendering::VisualPtr root = _scene->RootVisual(); // create directional light - ignition::rendering::DirectionalLightPtr light0 = + gz::rendering::DirectionalLightPtr light0 = _scene->CreateDirectionalLight(); light0->SetDirection(-0.5, 0.5, -1); light0->SetDiffuseColor(0.5, 0.5, 0.5); @@ -149,14 +149,14 @@ void BuildScene(ignition::rendering::ScenePtr _scene) root->AddChild(light0); // create point light - ignition::rendering::PointLightPtr light2 = _scene->CreatePointLight(); + gz::rendering::PointLightPtr light2 = _scene->CreatePointLight(); light2->SetDiffuseColor(0.5, 0.5, 0.5); light2->SetSpecularColor(0.5, 0.5, 0.5); light2->SetLocalPosition(3, 5, 5); root->AddChild(light2); // create green material - ignition::rendering::MaterialPtr green = _scene->CreateMaterial(); + gz::rendering::MaterialPtr green = _scene->CreateMaterial(); green->SetAmbient(0.0, 0.5, 0.0); green->SetDiffuse(0.0, 0.7, 0.0); green->SetSpecular(0.5, 0.5, 0.5); @@ -164,7 +164,7 @@ void BuildScene(ignition::rendering::ScenePtr _scene) green->SetReflectivity(0); // create center visual - ignition::rendering::VisualPtr center = _scene->CreateVisual(); + gz::rendering::VisualPtr center = _scene->CreateVisual(); center->AddGeometry(_scene->CreateSphere()); center->SetLocalPosition(3, 0, 0); center->SetLocalScale(0.1, 0.1, 0.1); @@ -172,7 +172,7 @@ void BuildScene(ignition::rendering::ScenePtr _scene) root->AddChild(center); // create red material - ignition::rendering::MaterialPtr red = _scene->CreateMaterial(); + gz::rendering::MaterialPtr red = _scene->CreateMaterial(); red->SetAmbient(0.5, 0.0, 0.0); red->SetDiffuse(1.0, 0.0, 0.0); red->SetSpecular(0.5, 0.5, 0.5); @@ -180,7 +180,7 @@ void BuildScene(ignition::rendering::ScenePtr _scene) red->SetReflectivity(0); // create sphere visual - ignition::rendering::VisualPtr sphere = _scene->CreateVisual(); + gz::rendering::VisualPtr sphere = _scene->CreateVisual(); sphere->AddGeometry(_scene->CreateSphere()); sphere->SetOrigin(0.0, -0.5, 0.0); sphere->SetLocalPosition(3, 0, 0); @@ -190,7 +190,7 @@ void BuildScene(ignition::rendering::ScenePtr _scene) root->AddChild(sphere); // create blue material - ignition::rendering::MaterialPtr blue = _scene->CreateMaterial(); + gz::rendering::MaterialPtr blue = _scene->CreateMaterial(); blue->SetAmbient(0.0, 0.0, 0.3); blue->SetDiffuse(0.0, 0.0, 0.8); blue->SetSpecular(0.5, 0.5, 0.5); @@ -198,7 +198,7 @@ void BuildScene(ignition::rendering::ScenePtr _scene) blue->SetReflectivity(0); // create box visual - ignition::rendering::VisualPtr box = _scene->CreateVisual(); + gz::rendering::VisualPtr box = _scene->CreateVisual(); box->AddGeometry(_scene->CreateBox()); box->SetOrigin(0.0, 0.5, 0.0); box->SetLocalPosition(3, 0, 0); @@ -208,14 +208,14 @@ void BuildScene(ignition::rendering::ScenePtr _scene) root->AddChild(box); // create white material - ignition::rendering::MaterialPtr white = _scene->CreateMaterial(); + gz::rendering::MaterialPtr white = _scene->CreateMaterial(); white->SetAmbient(0.5, 0.5, 0.5); white->SetDiffuse(0.8, 0.8, 0.8); white->SetReceiveShadows(true); white->SetReflectivity(0); // create sphere visual - ignition::rendering::VisualPtr plane = _scene->CreateVisual(); + gz::rendering::VisualPtr plane = _scene->CreateVisual(); plane->AddGeometry(_scene->CreatePlane()); plane->SetLocalScale(5, 8, 1); plane->SetLocalPosition(3, 0, -0.5); diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt index 25ec8976..4b2bdd7b 100644 --- a/include/CMakeLists.txt +++ b/include/CMakeLists.txt @@ -1 +1,2 @@ -add_subdirectory(ignition) +add_subdirectory(gz) +install(DIRECTORY ignition DESTINATION ${IGN_INCLUDE_INSTALL_DIR_FULL}) diff --git a/include/ignition/CMakeLists.txt b/include/gz/CMakeLists.txt similarity index 100% rename from include/ignition/CMakeLists.txt rename to include/gz/CMakeLists.txt diff --git a/include/gz/sensors/AirPressureSensor.hh b/include/gz/sensors/AirPressureSensor.hh new file mode 100644 index 00000000..121028ca --- /dev/null +++ b/include/gz/sensors/AirPressureSensor.hh @@ -0,0 +1,91 @@ +/* + * 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 GZ_SENSORS_AIRPRESSURESENSOR_HH_ +#define GZ_SENSORS_AIRPRESSURESENSOR_HH_ + +#include + +#include + +#include +#include + +#include +#include + +#include "gz/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class AirPressureSensorPrivate; + + /// \brief AirPressure Sensor Class + /// + /// A sensor that reports air pressure readings. + class IGNITION_SENSORS_AIR_PRESSURE_VISIBLE AirPressureSensor : + public Sensor + { + /// \brief constructor + public: AirPressureSensor(); + + /// \brief destructor + public: virtual ~AirPressureSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update(const common::Time &_now) override; + + /// \brief Set the reference altitude. + /// \param[in] _ref Verical reference position in meters + public: void SetReferenceAltitude(double _reference); + + /// \brief Get the vertical reference altitude. + /// \return Verical reference position in meters + public: double ReferenceAltitude() const; + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/AltimeterSensor.hh b/include/gz/sensors/AltimeterSensor.hh new file mode 100644 index 00000000..c89affe1 --- /dev/null +++ b/include/gz/sensors/AltimeterSensor.hh @@ -0,0 +1,108 @@ +/* + * 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 GZ_SENSORS_ALTIMETERSENSOR_HH_ +#define GZ_SENSORS_ALTIMETERSENSOR_HH_ + +#include + +#include + +#include +#include + +#include +#include + +#include "gz/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class AltimeterSensorPrivate; + + /// \brief Altimeter Sensor Class + /// + /// An altimeter sensor that reports vertical position and velocity + /// readings over ign transport + class IGNITION_SENSORS_ALTIMETER_VISIBLE AltimeterSensor : public Sensor + { + /// \brief constructor + public: AltimeterSensor(); + + /// \brief destructor + public: virtual ~AltimeterSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update(const common::Time &_now) override; + + /// \brief Set the vertical reference position of the altimeter + /// \param[in] _ref Verical reference position in meters + public: void SetVerticalReference(double _reference); + + /// \brief Get the vertical reference position of the altimeter + /// \return Verical reference position in meters + public: double VerticalReference() const; + + /// \brief Set the current z position of the altimeter + /// \param[in] _pos Z position in meters + public: void SetPosition(double _pos); + + /// \brief Get the vertical position of the altimeter relative to the + /// reference position + /// \return Vertical position relative to referene position + public: double VerticalPosition() const; + + /// \brief Set the vertical velocity of the altimeter + /// \param[in] _vel Vertical velocity in meters per second + public: void SetVerticalVelocity(double _vel); + + /// \brief Get the vertical velocity of the altimeter + /// \return Vertical velocity in meters per second + public: double VerticalVelocity() const; + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/ignition/sensors/CMakeLists.txt b/include/gz/sensors/CMakeLists.txt similarity index 100% rename from include/ignition/sensors/CMakeLists.txt rename to include/gz/sensors/CMakeLists.txt diff --git a/include/gz/sensors/CameraSensor.hh b/include/gz/sensors/CameraSensor.hh new file mode 100644 index 00000000..3f8647fc --- /dev/null +++ b/include/gz/sensors/CameraSensor.hh @@ -0,0 +1,181 @@ +/* + * Copyright (C) 2017 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 GZ_SENSORS_CAMERASENSOR_HH_ +#define GZ_SENSORS_CAMERASENSOR_HH_ + +#include +#include +#include + +#include + +#include +#include +#include + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4005) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +// TODO(louise) Remove these pragmas once ign-rendering is disabling the +// warnings +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include "gz/sensors/camera/Export.hh" +#include "gz/sensors/config.hh" +#include "gz/sensors/Export.hh" +#include "gz/sensors/RenderingSensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class CameraSensorPrivate; + + /// \brief Camera Sensor Class + /// + /// This class creates images from an ignition rendering scene. The scene + /// must be created in advance and given to Manager::Init(). + /// It offers both an ignition-transport interface and a direct C++ API + /// to access the image data. The API works by setting a callback to be + /// called with image data. + class IGNITION_SENSORS_CAMERA_VISIBLE CameraSensor : public RenderingSensor + { + /// \brief constructor + public: CameraSensor(); + + /// \brief destructor + public: virtual ~CameraSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update(const common::Time &_now) override; + + /// \brief Set a callback to be called when image frame data is + /// generated. + /// \param[in] _callback This callback will be called every time the + /// camera produces image data. The Update function will be blocked + /// while the callbacks are executed. + /// \remark Do not block inside of the callback. + /// \return A connection pointer that must remain in scope. When the + /// connection pointer falls out of scope, the connection is broken. + public: gz::common::ConnectionPtr ConnectImageCallback( + std::function< + void(const gz::msgs::Image &)> _callback); + + /// \brief Set the rendering scene. + /// \param[in] _scene Pointer to the scene + public: virtual void SetScene( + gz::rendering::ScenePtr _scene) override; + + /// \brief Get image width. + /// \return width of the image + public: virtual unsigned int ImageWidth() const; + + /// \brief Get image height. + /// \return height of the image + public: virtual unsigned int ImageHeight() const; + + /// \brief Get pointer to rendering camera object. + /// \return Camera in Gazebo Rendering. + public: rendering::CameraPtr RenderingCamera() const; + + /// \brief Topic where camera info is published. + /// \return Camera info topic. + public: std::string InfoTopic() const; + + /// \brief Set baseline for stereo cameras. This is used to populate the + /// projection matrix in the camera info message. + /// \param[in] _baseline The distance from the 1st camera, in meters. + public: void SetBaseline(double _baseline); + + /// \brief Get baseline for stereo cameras. + /// \return The distance from the 1st camera, in meters. + public: double Baseline() const; + + /// \brief Advertise camera info topic. + /// \return True if successful. + protected: bool AdvertiseInfo(); + + /// \brief Advertise camera info topic. + /// This version takes a string that allows one to override the + /// camera_info topic. + /// \param[in] _topic The topic on which camera info is to be published. + /// \return True if successful. + protected: bool AdvertiseInfo(const std::string &_topic); + + /// \brief Populate camera info message. + /// \param[in] _cameraSdf Pointer to SDF object containing camera + /// information. + protected: void PopulateInfo(const sdf::Camera *_cameraSdf); + + /// \brief Publish camera info message. + /// \param[in] _now The current time + protected: void PublishInfo(const gz::common::Time &_now); + + /// \brief Create a camera in a scene + /// \return True on success. + private: bool CreateCamera(); + + /// \brief Callback that is triggered when the scene changes on + /// the Manager. + /// \param[in] _scene Pointer to the new scene. + private: void OnSceneChange(gz::rendering::ScenePtr /*_scene*/); + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/DepthCameraSensor.hh b/include/gz/sensors/DepthCameraSensor.hh new file mode 100644 index 00000000..efc07109 --- /dev/null +++ b/include/gz/sensors/DepthCameraSensor.hh @@ -0,0 +1,181 @@ +/* + * 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 GZ_SENSORS_DEPTHCAMERASENSOR_HH_ +#define GZ_SENSORS_DEPTHCAMERASENSOR_HH_ + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4005) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +// TODO(louise) Remove these pragmas once ign-rendering is disabling the +// warnings +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include "gz/sensors/depth_camera/Export.hh" +#include "gz/sensors/CameraSensor.hh" +#include "gz/sensors/Export.hh" +#include "gz/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // forward declarations + class DepthCameraSensorPrivate; + + /// \brief Depth camera sensor class. + /// + /// This class creates depth image from an ignition rendering scene. + /// The scene must be created in advance and given to Manager::Init(). + /// It offers both an ignition-transport interface and a direct C++ API + /// to access the image data. The API works by setting a callback to be + /// called with image data. + class IGNITION_SENSORS_DEPTH_CAMERA_VISIBLE DepthCameraSensor + : public CameraSensor + { + /// \brief constructor + public: DepthCameraSensor(); + + /// \brief destructor + public: virtual ~DepthCameraSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update(const common::Time &_now) override; + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual rendering::DepthCameraPtr DepthCamera(); + + /// \brief Depth data callback used to get the data from the sensor + /// \param[in] _scan pointer to the data from the sensor + /// \param[in] _width width of the depth image + /// \param[in] _height height of the depth image + /// \param[in] _channel bytes used for the depth data + /// \param[in] _format string with the format + public: void OnNewDepthFrame(const float *_scan, + unsigned int _width, unsigned int _height, + unsigned int /*_channels*/, + const std::string &/*_format*/); + + /// \brief Point cloud data callback used to get the data from the sensor + /// \param[in] _data pointer to the data from the sensor + /// \param[in] _width width of the point cloud image + /// \param[in] _height height of the point cloud image + /// \param[in] _channel bytes used for the point cloud data + /// \param[in] _format string with the format + public: void OnNewRgbPointCloud(const float *_data, + unsigned int _width, unsigned int _height, + unsigned int /*_channels*/, + const std::string &/*_format*/); + + /// \brief Set a callback to be called when image frame data is + /// generated. + /// \param[in] _callback This callback will be called every time the + /// camera produces image data. The Update function will be blocked + /// while the callbacks are executed. + /// \remark Do not block inside of the callback. + /// \return A connection pointer that must remain in scope. When the + /// connection pointer falls out of scope, the connection is broken. + public: gz::common::ConnectionPtr ConnectImageCallback( + std::function< + void(const gz::msgs::Image &)> _callback); + + /// \brief Set the rendering scene. + /// \param[in] _scene Pointer to the scene + public: virtual void SetScene( + gz::rendering::ScenePtr _scene) override; + + /// \brief Get image width. + /// \return width of the image + public: virtual unsigned int ImageWidth() const override; + + /// \brief Get image height. + /// \return height of the image + public: virtual unsigned int ImageHeight() const override; + + /// \brief Get image width. + /// \return width of the image + public: virtual double FarClip() const; + + /// \brief Get image height. + /// \return height of the image + public: virtual double NearClip() const; + + /// \brief Create a camera in a scene + /// \return True on success. + private: bool CreateCamera(); + + /// \brief Callback that is triggered when the scene changes on + /// the Manager. + /// \param[in] _scene Pointer to the new scene. + private: void OnSceneChange(gz::rendering::ScenePtr /*_scene*/) + { } + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/GaussianNoiseModel.hh b/include/gz/sensors/GaussianNoiseModel.hh new file mode 100644 index 00000000..7e8173ce --- /dev/null +++ b/include/gz/sensors/GaussianNoiseModel.hh @@ -0,0 +1,77 @@ +/* + * 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 GZ_SENSORS_GAUSSIANNOISEMODEL_HH_ +#define GZ_SENSORS_GAUSSIANNOISEMODEL_HH_ + +#include + +#include "gz/sensors/config.hh" +#include "gz/sensors/Export.hh" +#include "gz/sensors/Noise.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + // Forward declarations + class GaussianNoiseModelPrivate; + + /** \class GaussianNoiseModel GaussianNoiseModel.hh \ + ignition/sensors/GaussianNoiseModel.hh + **/ + /// \brief Gaussian noise class + class IGNITION_SENSORS_VISIBLE GaussianNoiseModel : public Noise + { + /// \brief Constructor. + public: GaussianNoiseModel(); + + /// \brief Destructor. + public: virtual ~GaussianNoiseModel(); + + // Documentation inherited. + public: virtual void Load(const sdf::Noise &_sdf) override; + + // Documentation inherited. + public: double ApplyImpl(double _in, double _dt) override; + + /// \brief Accessor for mean. + /// \return Mean of Gaussian noise. + public: double Mean() const; + + /// \brief Accessor for stddev. + /// \return Standard deviation of Gaussian noise. + public: double StdDev() const; + + /// \brief Accessor for bias. + /// \return Bias on output. + public: double Bias() const; + + /// Documentation inherited + public: virtual void Print(std::ostream &_out) const override; + + /// \brief Private data pointer. + private: GaussianNoiseModelPrivate *dataPtr = nullptr; + }; + } + } +} + +#endif diff --git a/include/gz/sensors/GpuLidarSensor.hh b/include/gz/sensors/GpuLidarSensor.hh new file mode 100644 index 00000000..140d3f42 --- /dev/null +++ b/include/gz/sensors/GpuLidarSensor.hh @@ -0,0 +1,140 @@ +/* + * 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 GZ_SENSORS_GPULIDARSENSOR_HH_ +#define GZ_SENSORS_GPULIDARSENSOR_HH_ + +#include +#include + +#include + +#include + +// TODO(louise) Remove these pragmas once ign-rendering is disabling the +// warnings +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include "gz/sensors/gpu_lidar/Export.hh" +#include "gz/sensors/RenderingEvents.hh" +#include "gz/sensors/Lidar.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class GpuLidarSensorPrivate; + + /// \brief GpuLidar Sensor Class + /// + /// This class creates laser scans using the GPU. It's measures the range + /// from the origin of the center to points on the visual geometry in the + /// scene. + /// + /// It offers both an ignition-transport interface and a direct C++ API + /// to access the image data. The API works by setting a callback to be + /// called with image data. + class IGNITION_SENSORS_GPU_LIDAR_VISIBLE GpuLidarSensor : public Lidar + { + /// \brief constructor + public: GpuLidarSensor(); + + /// \brief destructor + public: virtual ~GpuLidarSensor(); + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update(const common::Time &_now) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load sensor sata from SDF + /// \param[in] _sdf SDF used + /// \return True on success + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Create Lidar sensor + public: virtual bool CreateLidar() override; + + /// \brief Gets if sensor is horizontal + /// \return True if horizontal, false if not + public: bool IsHorizontal() const; + + /// \brief Makes possible to change sensor scene + /// \param[in] _scene used with the sensor + public: void SetScene(gz::rendering::ScenePtr _scene) override; + + /// \brief Remove sensor from scene + /// \param[in] _scene used with the sensor + public: void RemoveGpuRays(gz::rendering::ScenePtr _scene); + + /// \brief Get Gpu Rays object used in the sensor + /// \return Pointer to gz::rendering::GpuRays + public: gz::rendering::GpuRaysPtr GpuRays() const; + + /// \brief Return the ratio of horizontal ray count to vertical ray + /// count. + /// + /// A ray count is the number of simulated rays. Whereas a range count + /// is the total number of data points returned. When range count + /// != ray count, then values are interpolated between rays. + public: double RayCountRatio() const; + + /// \brief Get the horizontal field of view of the laser sensor. + /// \return The horizontal field of view of the laser sensor. + public: gz::math::Angle HFOV() const; + + /// \brief Get the vertical field-of-view. + /// \return Vertical field of view. + public: gz::math::Angle VFOV() const; + + /// \brief Connect function pointer to internal GpuRays callback + /// \return gz::common::Connection pointer + public: virtual gz::common::ConnectionPtr ConnectNewLidarFrame( + std::function _subscriber) override; + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/ImageGaussianNoiseModel.hh b/include/gz/sensors/ImageGaussianNoiseModel.hh new file mode 100644 index 00000000..0fca3015 --- /dev/null +++ b/include/gz/sensors/ImageGaussianNoiseModel.hh @@ -0,0 +1,76 @@ +/* + * 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 GZ_SENSORS_IMAGEGAUSSIANNOISEMODEL_HH_ +#define GZ_SENSORS_IMAGEGAUSSIANNOISEMODEL_HH_ + +#include + +// TODO(louise) Remove these pragmas once ign-rendering is disabling the +// warnings +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include "gz/sensors/config.hh" +#include "gz/sensors/GaussianNoiseModel.hh" +#include "gz/sensors/rendering/Export.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // Forward declarations + class ImageGaussianNoiseModelPrivate; + + /** \class ImageGaussianNoiseModel GaussianNoiseModel.hh \ + ignition/sensors/GaussianNoiseModel.hh + **/ + /// \brief Gaussian noise class for image sensors + class IGNITION_SENSORS_RENDERING_VISIBLE ImageGaussianNoiseModel : + public GaussianNoiseModel + { + /// \brief Constructor. + public: ImageGaussianNoiseModel(); + + /// \brief Destructor. + public: virtual ~ImageGaussianNoiseModel(); + + // Documentation inherited. + public: virtual void Load(const sdf::Noise &_sdf) override; + + // Documentation inherited. + public: virtual void SetCamera(rendering::CameraPtr _camera); + + /// Documentation inherited + public: virtual void Print(std::ostream &_out) const override; + + /// \brief Private data pointer. + private: ImageGaussianNoiseModelPrivate *dataPtr = nullptr; + }; + } + } +} + +#endif diff --git a/include/gz/sensors/ImageNoise.hh b/include/gz/sensors/ImageNoise.hh new file mode 100644 index 00000000..87ac6834 --- /dev/null +++ b/include/gz/sensors/ImageNoise.hh @@ -0,0 +1,65 @@ +/* + * 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 GZ_SENSORS_IMAGENOISE_HH_ +#define GZ_SENSORS_IMAGENOISE_HH_ + +#include + +#include + +#include "gz/sensors/config.hh" +#include "gz/sensors/SensorTypes.hh" +#include "gz/sensors/rendering/Export.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // Forward declarations + class NoisePrivate; + + /// \class NoiseFactory Noise.hh ignition/sensors/Noise.hh + /// \brief Use this noise manager for creating and loading noise models. + class IGNITION_SENSORS_RENDERING_VISIBLE ImageNoiseFactory + { + /// \brief Load a noise model based on the input sdf parameters and + /// sensor type. + /// \param[in] _sdf Noise sdf parameters. + /// \param[in] _sensorType Type of sensor. This is currently used to + /// distinguish between image and non image sensors in order to create + /// the appropriate noise model. + /// \return Pointer to the noise model created. + public: static NoisePtr NewNoiseModel(sdf::ElementPtr _sdf, + const std::string &_sensorType = ""); + + /// \brief Load a noise model based on the input sdf parameters and + /// sensor type. + /// \param[in] _sdf Noise sdf parameters. + /// \param[in] _sensorType Type of sensor. This is currently used to + /// distinguish between image and non image sensors in order to create + /// the appropriate noise model. + /// \return Pointer to the noise model created. + public: static NoisePtr NewNoiseModel(const sdf::Noise &_sdf, + const std::string &_sensorType = ""); + }; + } + } +} +#endif diff --git a/include/gz/sensors/ImuSensor.hh b/include/gz/sensors/ImuSensor.hh new file mode 100644 index 00000000..08bc381d --- /dev/null +++ b/include/gz/sensors/ImuSensor.hh @@ -0,0 +1,135 @@ +/* + * 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 GZ_SENSORS_IMUSENSOR_HH_ +#define GZ_SENSORS_IMUSENSOR_HH_ + +#include + +#include + +#include +#include +#include + +#include +#include + +#include "gz/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class ImuSensorPrivate; + + /// \brief Imu Sensor Class + /// + /// An imu sensor that reports linear acceleration, angular velocity, and + /// orientation + class IGNITION_SENSORS_IMU_VISIBLE ImuSensor : public Sensor + { + /// \brief constructor + public: ImuSensor(); + + /// \brief destructor + public: virtual ~ImuSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update(const common::Time &_now) override; + + /// \brief Set the angular velocity of the imu + /// \param[in] _angularVel Angular velocity of the imu in body frame + /// expressed in radians per second + public: void SetAngularVelocity(const math::Vector3d &_angularVel); + + /// \brief Get the angular velocity of the imu + /// \return Angular velocity of the imu in body frame, expressed in + /// radians per second. + public: math::Vector3d AngularVelocity() const; + + /// \brief Set the linear acceleration of the imu + /// \param[in] _linearAcc Linear accceleration of the imu in body frame + /// expressed in meters per second squared. + public: void SetLinearAcceleration(const math::Vector3d &_linearAcc); + + /// \brief Get the linear acceleration of the imu + /// \return Linear acceleration of the imu in local frame, expressed in + /// meters per second squared. + public: math::Vector3d LinearAcceleration() const; + + /// \brief Set the world pose of the imu + /// \param[in] _pose Pose in world frame + public: void SetWorldPose(const math::Pose3d _pose); + + /// \brief Get the world pose of the imu + /// \return Pose in world frame. + public: math::Pose3d WorldPose() const; + + /// \brief Set the orientation reference, i.e. initial imu + /// orientation. Imu orientation data generated will be relative to this + /// reference frame. + /// \param[in] _orientation Reference orientation + public: void SetOrientationReference( + const math::Quaterniond &_orient); + + /// \brief Get the world orienation reference of the imu + /// \return Orientation reference in world frame + public: math::Quaterniond OrientationReference() const; + + /// \brief Get the orienation of the imu with respect to reference frame + /// \return Orientation in reference frame + public: math::Quaterniond Orientation() const; + + /// \brief Set the gravity vector + /// \param[in] _gravity gravity vector in meters per second squared. + public: void SetGravity(const math::Vector3d &_gravity); + + /// \brief Get the gravity vector + /// \return Gravity vectory in meters per second squared. + public: math::Vector3d Gravity() const; + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/Lidar.hh b/include/gz/sensors/Lidar.hh new file mode 100644 index 00000000..9e7cf93f --- /dev/null +++ b/include/gz/sensors/Lidar.hh @@ -0,0 +1,270 @@ +/* + * 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 GZ_SENSORS_LIDAR_HH_ +#define GZ_SENSORS_LIDAR_HH_ + +#include +#include +#include + +#include +#include + +#include "gz/sensors/lidar/Export.hh" +#include "gz/sensors/RenderingSensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class LidarPrivate; + + /// \brief Lidar Sensor Class + /// + /// This class creates laser scans using. It's measures the range + /// from the origin of the center to points on the visual geometry in the + /// scene. + /// + /// It offers both an ignition-transport interface and a direct C++ API + /// to access the image data. The API works by setting a callback to be + /// called with image data. + class IGNITION_SENSORS_LIDAR_VISIBLE Lidar : public RenderingSensor + { + /// \brief constructor + public: Lidar(); + + /// \brief destructor + public: virtual ~Lidar(); + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update(const common::Time &_now) override; + + /// \brief Apply noise to the laser buffer, if noise has been + /// configured. This should be called before PublishLidarScan if you + /// want the scan data to contain noise. + public: void ApplyNoise(); + + /// \brief Publish LaserScan message + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool PublishLidarScan(const common::Time &_now); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: void SetParent(const std::string &_parent) override; + + /// \brief Create Lidar sensor + public: virtual bool CreateLidar(); + + /// \brief Finalize the ray + protected: virtual void Fini(); + + /// \brief Get the minimum angle + /// \return The minimum angle + public: gz::math::Angle AngleMin() const; + + /// \brief Set the scan minimum angle + /// \param[in] _angle The minimum angle + public: void SetAngleMin(const double _angle); + + /// \brief Get the maximum angle + /// \return the maximum angle + public: gz::math::Angle AngleMax() const; + + /// \brief Set the scan maximum angle + /// \param[in] _angle The maximum angle + public: void SetAngleMax(const double _angle); + + /// \brief Get radians between each range + /// \return Return angle resolution + public: double AngleResolution() const; + + /// \brief Get the minimum range + /// \return The minimum range + public: double RangeMin() const; + + /// \brief Get the maximum range + /// \return The maximum range + public: double RangeMax() const; + + /// \brief Get the range resolution + /// If RangeResolution is 1, the number of simulated rays is equal + /// to the number of returned range readings. If it's less than 1, + /// fewer simulated rays than actual returned range readings are + /// used, the results are interpolated from two nearest neighbors, + /// and vice versa. + /// \return The Range Resolution + public: double RangeResolution() const; + + /// \brief Get the ray count + /// \return The number of rays + public: unsigned int RayCount() const; + + /// \brief Get the range count + /// \return The number of ranges + public: unsigned int RangeCount() const; + + /// \brief Get the vertical scan line count + /// \return The number of scan lines vertically + public: unsigned int VerticalRayCount() const; + + /// \brief Get the vertical scan line count + /// \return The number of scan lines vertically + public: unsigned int VerticalRangeCount() const; + + /// \brief Get the vertical scan bottom angle + /// \return The minimum angle of the scan block + public: gz::math::Angle VerticalAngleMin() const; + + /// \brief Set the vertical scan bottom angle + /// \param[in] _angle The minimum angle of the scan block + public: void SetVerticalAngleMin(const double _angle); + + /// \brief Get the vertical scan line top angle + /// \return The Maximum angle of the scan block + public: gz::math::Angle VerticalAngleMax() const; + + /// \brief Set the vertical scan line top angle + /// \param[in] _angle The Maximum angle of the scan block + public: void SetVerticalAngleMax(const double _angle); + + /// \brief Get the vertical angle in radians between each range + /// \return Resolution of the angle + public: double VerticalAngleResolution() const; + + /// \brief Get detected range for a ray. + /// Warning: If you are accessing all the ray data in a loop + /// it's possible that the Ray will update in the middle of + /// your access loop. This means some data will come from one + /// scan, and some from another scan. You can solve this + /// problem by using SetActive(false) + /// SetActive(true). + /// \param[in] _index Index of specific ray + /// \return Returns RangeMax for no detection. + public: double Range(const int _index) const; + + /// \brief Get all the ranges + /// \param[out] _range A vector that will contain all the range data + public: void Ranges(std::vector &_ranges) const; + + /// \brief Get detected retro (intensity) value for a ray. + /// Warning: If you are accessing all the ray data in a loop + /// it's possible that the Ray will update in the middle of + /// your access loop. This means some data will come from one + /// scan, and some from another scan. You can solve this + /// problem by using SetActive(false) + /// SetActive(true). + /// \param[in] _index Index of specific ray + /// \return Intensity value of ray + public: double Retro(const int _index) const; + + /// \brief Get detected fiducial value for a ray. + /// Warning: If you are accessing all the ray data in a loop + /// it's possible that the Ray will update in the middle of + /// your access loop. This means some data will come from one + /// scan, and some from another scan. You can solve this + /// problem by using SetActive(false) + /// SetActive(true). + /// \param[in] _index Index of specific ray + /// \return Fiducial value of ray + public: int Fiducial(const unsigned int _index) const; + + /// \brief Gets if sensor is horizontal + /// \return True if horizontal, false if not + public: bool IsHorizontal() const; + + /// \brief Return the ratio of horizontal ray count to vertical ray + /// count. + /// + /// A ray count is the number of simulated rays. Whereas a range count + /// is the total number of data points returned. When range count + /// != ray count, then values are interpolated between rays. + public: double RayCountRatio() const; + + /// \brief Return the ratio of horizontal range count to vertical + /// range count. + /// + /// A ray count is the number of simulated rays. Whereas a range count + /// is the total number of data points returned. When range count + /// != ray count, then values are interpolated between rays. + public: double RangeCountRatio() const; + + /// \brief Get the horizontal field of view of the laser sensor. + /// \return The horizontal field of view of the laser sensor. + public: double HorzFOV() const; + + /// \brief Get the vertical field-of-view. + /// \return Vertical field of view. + public: double VertFOV() const; + + // Documentation inherited + public: virtual bool IsActive() const; + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Just a mutex for thread safety + public: mutable std::mutex lidarMutex; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + + /// \brief Raw buffer of laser data. + public: float *laserBuffer = nullptr; + + /// \brief true if Load() has been called and was successful + public: bool initialized = false; + + /// \brief Set a callback to be called when data is generated. + /// \param[in] _callback This callback will be called every time the + /// sensor generates data. The Update function will be blocked while the + /// callbacks are executed. + /// \remark Do not block inside of the callback. + /// \return A connection pointer that must remain in scope. When the + /// connection pointer falls out of scope, the connection is broken. + public: virtual gz::common::ConnectionPtr ConnectNewLidarFrame( + std::function _subscriber); + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/LogicalCameraSensor.hh b/include/gz/sensors/LogicalCameraSensor.hh new file mode 100644 index 00000000..c68fe74a --- /dev/null +++ b/include/gz/sensors/LogicalCameraSensor.hh @@ -0,0 +1,125 @@ +/* + * 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 GZ_SENSORS_LOGICALCAMERASENSOR_HH_ +#define GZ_SENSORS_LOGICALCAMERASENSOR_HH_ + +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4005) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include "gz/sensors/config.hh" +#include "gz/sensors/Export.hh" +#include "gz/sensors/logical_camera/Export.hh" +#include "gz/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class LogicalCameraSensorPrivate; + + /// \brief Logical Camera Sensor Class + /// + /// A logical camera reports locations of objects. This camera finds models + /// within the sensor's frustum and publishes information about the models + /// on the sensor's topic. + class IGNITION_SENSORS_LOGICAL_CAMERA_VISIBLE LogicalCameraSensor + : public Sensor + { + /// \brief constructor + public: LogicalCameraSensor(); + + /// \brief destructor + public: virtual ~LogicalCameraSensor(); + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update(const common::Time &_now) override; + + /// \brief Get the near distance. This is the distance from the + /// frustum's vertex to the closest plane. + /// \return Near distance. + public: double Near() const; + + /// \brief Get the far distance. This is the distance from the + /// frustum's vertex to the farthest plane. + /// \return Far distance. + public: double Far() const; + + /// \brief Set the models currently in the world + /// \param[in] _models A map of model names to their world pose. + public: void SetModelPoses(std::map &&_models); + + /// \brief Get the horizontal field of view. The field of view is the + /// angle between the frustum's vertex and the edges of the near or far + /// plane. This value represents the horizontal angle. + /// \return The field of view. + public: gz::math::Angle HorizontalFOV() const; + + /// \brief Get the aspect ratio, which is the width divided by height + /// of the near or far planes. + /// \return The frustum's aspect ratio. + public: double AspectRatio() const; + + /// \brief Get the latest image. An image is an instance of + /// msgs::LogicalCameraImage, which contains a list of detected models. + /// \return List of detected models. + public: msgs::LogicalCameraImage Image() const; + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/MagnetometerSensor.hh b/include/gz/sensors/MagnetometerSensor.hh new file mode 100644 index 00000000..c1a72b04 --- /dev/null +++ b/include/gz/sensors/MagnetometerSensor.hh @@ -0,0 +1,104 @@ +/* + * 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 GZ_SENSORS_MAGNETOMETERSENSOR_HH_ +#define GZ_SENSORS_MAGNETOMETERSENSOR_HH_ + +#include + +#include + +#include +#include +#include + +#include +#include + +#include "gz/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class MagnetometerSensorPrivate; + + /// \brief Magnetometer Sensor Class + /// + /// A magnetometer reports the magnetic field vector + class IGNITION_SENSORS_MAGNETOMETER_VISIBLE MagnetometerSensor + : public Sensor + { + /// \brief constructor + public: MagnetometerSensor(); + + /// \brief destructor + public: virtual ~MagnetometerSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update(const common::Time &_now) override; + + /// \brief Set the world pose of the sensor + /// \param[in] _pose Pose in world frame + public: void SetWorldPose(const math::Pose3d _pose); + + /// \brief Get the world pose of the sensor + /// \return Pose in world frame. + public: math::Pose3d WorldPose() const; + + /// \brief Set the magnetic field vector in world frame + /// \param[in] _field Magnetic field vector in world frame. + public: void SetWorldMagneticField(const math::Vector3d &_field); + + /// \brief Get the magnetic field vector in world frame + /// \return Magnetic field vector in world frame + public: math::Vector3d WorldMagneticField() const; + + /// \brief Get the magnetic field vector in body frame + /// \return Magnetic field vector in body frame + public: math::Vector3d MagneticField() const; + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/Manager.hh b/include/gz/sensors/Manager.hh new file mode 100644 index 00000000..915d9869 --- /dev/null +++ b/include/gz/sensors/Manager.hh @@ -0,0 +1,218 @@ +/* + * Copyright (C) 2017 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 GZ_SENSORS_MANAGER_HH_ +#define GZ_SENSORS_MANAGER_HH_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // Forward declarations + class ManagerPrivate; + + /// \brief Loads and runs sensors + /// + /// This class is responsible for loading and running sensors, and + /// providing sensors with common environments to generat data from. + /// + /// The primary interface through which to load a sensor is LoadSensor(). + /// This takes an sdf element pointer that should be configured with + /// everything the sensor will need. Custom sensors configuration must + /// be in the tag of the sdf::Element. The manager will + /// dynamically load the sensor library and update it. + /// \remarks This class is not thread safe. + class IGNITION_SENSORS_VISIBLE Manager + { + /// \brief constructor + public: Manager(); + + /// \brief destructor + public: virtual ~Manager(); + + /// \brief Initialize the sensor library without rendering or physics. + /// \return True if successfully initialized, false if not + public: bool Init(); + + /// \brief Create a sensor from SDF with a known sensor type. + /// + /// This creates sensors by looking at the given sdf element. + /// Sensors created with this API offer an ignition-transport interface. + /// If you need a direct C++ interface to the data, you must get the + /// sensor pointer and cast to the correct type. + /// + /// A tag may have multiple tags. A SensorId will be + /// returned for each plugin that is described in SDF. + /// If there are no tags then one of the plugins shipped with + /// this library will be loaded. For example, a tag with + /// but no will load a CameraSensor from + /// ignition-sensors-camera. + /// \sa Sensor() + /// \param[in] _sdf pointer to the sdf element + /// \return A pointer to the created sensor. nullptr returned on + /// error. + public: template + T *CreateSensor(sdf::Sensor _sdf) + { + gz::sensors::SensorId id = this->CreateSensor(_sdf); + + if (id != NO_SENSOR) + { + T *result = dynamic_cast(this->Sensor(id)); + + if (!result) + ignerr << "SDF sensor type does not match template type\n"; + + return result; + } + + ignerr << "Failed to create sensor of type[" + << _sdf.TypeStr() << "]\n"; + return nullptr; + } + + /// \brief Create a sensor from SDF with a known sensor type. + /// + /// This creates sensors by looking at the given sdf element. + /// Sensors created with this API offer an ignition-transport interface. + /// If you need a direct C++ interface to the data, you must get the + /// sensor pointer and cast to the correct type. + /// + /// A tag may have multiple tags. A SensorId will be + /// returned for each plugin that is described in SDF. + /// If there are no tags then one of the plugins shipped with + /// this library will be loaded. For example, a tag with + /// but no will load a CameraSensor from + /// ignition-sensors-camera. + /// \sa Sensor() + /// \param[in] _sdf pointer to the sdf element + /// \return A pointer to the created sensor. nullptr returned on + /// error. + public: template + T *CreateSensor(sdf::ElementPtr _sdf) + { + gz::sensors::SensorId id = this->CreateSensor(_sdf); + + if (id != NO_SENSOR) + { + T *result = dynamic_cast(this->Sensor(id)); + + if (nullptr == result) + { + ignerr << "Failed to create sensor [" << id << "] of type [" + << _sdf->Get("type") + << "]. SDF sensor type does not match template type." + << std::endl; + } + + return result; + } + + ignerr << "Failed to create sensor of type [" + << _sdf->Get("type") << "]\n"; + return nullptr; + } + + /// \brief Create a sensor from SDF without a known sensor type. + /// + /// This creates sensors by looking at the given sdf element. + /// Sensors created with this API offer an ignition-transport interface. + /// If you need a direct C++ interface to the data, you must get the + /// sensor pointer and cast to the correct type. + /// + /// A tag may have multiple tags. A SensorId will be + /// returned for each plugin that is described in SDF. + /// If there are no tags then one of the plugins shipped with + /// this library will be loaded. For example, a tag with + /// but no will load a CameraSensor from + /// ignition-sensors-camera. + /// \sa Sensor() + /// \param[in] _sdf pointer to the sdf element + /// \return A sensor id that refers to the created sensor. NO_SENSOR + /// is returned on erro. + public: gz::sensors::SensorId CreateSensor(sdf::ElementPtr _sdf); + + /// \brief Create a sensor from SDF without a known sensor type. + /// + /// This creates sensors by looking at the given sdf element. + /// Sensors created with this API offer an ignition-transport interface. + /// If you need a direct C++ interface to the data, you must get the + /// sensor pointer and cast to the correct type. + /// + /// A tag may have multiple tags. A SensorId will be + /// returned for each plugin that is described in SDF. + /// If there are no tags then one of the plugins shipped with + /// this library will be loaded. For example, a tag with + /// but no will load a CameraSensor from + /// ignition-sensors-camera. + /// \sa Sensor() + /// \param[in] _sdf SDF sensor DOM object + /// \return A sensor id that refers to the created sensor. NO_SENSOR + /// is returned on erro. + public: gz::sensors::SensorId CreateSensor(const sdf::Sensor &_sdf); + + + /// \brief Get an instance of a loaded sensor by sensor id + /// \param[in] _id Idenitifier of the sensor. + /// \return Pointer to the sensor, nullptr on error. + public: gz::sensors::Sensor *Sensor( + gz::sensors::SensorId _id); + + /// \brief Remove a sensor by ID + /// \param[in] _sensorId ID of the sensor to remove + /// \return True if the sensor exists and removed. + public: bool Remove(const gz::sensors::SensorId _id); + + /// \brief Run the sensor generation one step. + /// \param _time: The current simulated time + /// \param _force: If true, all sensors are forced to update. Otherwise + /// a sensor will update based on it's Hz rate. + public: void RunOnce(const gz::common::Time &_time, + bool _force = false); + + /// \brief Adds colon delimited paths sensor plugins may be + public: void AddPluginPaths(const std::string &_path); + + /// \brief load a plugin and return a shared_ptr + /// \param[in] _filename Sensor plugin file to load. + /// \return Pointer to the new sensor, nullptr on error. + private: gz::sensors::SensorId LoadSensorPlugin( + const std::string &_filename, sdf::ElementPtr _sdf); + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief private data pointer + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/Noise.hh b/include/gz/sensors/Noise.hh new file mode 100644 index 00000000..3d66d11d --- /dev/null +++ b/include/gz/sensors/Noise.hh @@ -0,0 +1,125 @@ +/* + * 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 GZ_SENSORS_NOISE_HH_ +#define GZ_SENSORS_NOISE_HH_ + +#include +#include +#include + +#include +#include +#include + +#include + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // Forward declarations + class NoisePrivate; + + /// \class NoiseFactory Noise.hh ignition/sensors/Noise.hh + /// \brief Use this noise manager for creating and loading noise models. + class IGNITION_SENSORS_VISIBLE NoiseFactory + { + /// \brief Load a noise model based on the input sdf parameters and + /// sensor type. + /// \param[in] _sdf Noise sdf parameters. + /// \param[in] _sensorType Type of sensor. This is currently used to + /// distinguish between image and non image sensors in order to create + /// the appropriate noise model. + /// \return Pointer to the noise model created. + public: static NoisePtr NewNoiseModel(sdf::ElementPtr _sdf, + const std::string &_sensorType = ""); + + /// \brief Load a noise model based on the input sdf parameters and + /// sensor type. + /// \param[in] _sdf Noise sdf parameters. + /// \param[in] _sensorType Type of sensor. This is currently used to + /// distinguish between image and non image sensors in order to create + /// the appropriate noise model. + /// \return Pointer to the noise model created. + public: static NoisePtr NewNoiseModel(const sdf::Noise &_sdf, + const std::string &_sensorType = ""); + }; + + /// \brief Which noise types we support + enum class IGNITION_SENSORS_VISIBLE NoiseType : int + { + NONE = 0, + CUSTOM = 1, + GAUSSIAN = 2 + }; + + /// \class Noise Noise.hh ignition/sensors/Noise.hh + /// \brief Noise models for sensor output signals. + class IGNITION_SENSORS_VISIBLE Noise + { + /// \brief Constructor. This should not be called directly unless creating + /// an empty noise model. Use NoiseFactory::NewNoiseModel to instantiate + /// a new noise model. + /// \param[in] _type Type of noise model. + /// \sa NoiseFactory::NewNoiseModel + public: explicit Noise(NoiseType _type); + + /// \brief Destructor. + public: virtual ~Noise(); + + /// \brief Load noise parameters from sdf. + /// \param[in] _sdf SDF Noise DOM object. + public: virtual void Load(const sdf::Noise &_sdf); + + /// \brief Apply noise to input data value. + /// \param[in] _in Input data value. + /// \param[in] _dt Input data time step. + /// \return Data with noise applied. + public: double Apply(double _in, double _dt = 0.0); + + /// \brief Apply noise to input data value. This gets overriden by + /// derived classes, and called by Apply. + /// \param[in] _in Input data value. + /// \param[in] _dt Input data time step. + /// \return Data with noise applied. + public: virtual double ApplyImpl(double _in, double _dt); + + /// \brief Accessor for NoiseType. + /// \return Type of noise currently in use. + public: NoiseType Type() const; + + /// \brief Register a custom noise callback. + /// \param[in] _cb Callback function for applying a custom noise model. + /// This is useful if users want to use their own noise model from a + /// sensor plugin. + public: virtual void SetCustomNoiseCallback( + std::function _cb); + + /// \brief Output information about the noise model. + /// \param[in] _out Output stream + public: virtual void Print(std::ostream &_out) const; + + /// \brief Private data pointer + private: NoisePrivate *dataPtr = nullptr; + }; + } + } +} +#endif diff --git a/include/gz/sensors/RenderingEvents.hh b/include/gz/sensors/RenderingEvents.hh new file mode 100644 index 00000000..0a8872dd --- /dev/null +++ b/include/gz/sensors/RenderingEvents.hh @@ -0,0 +1,68 @@ +/* + * 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 GZ_SENSORS_RENDERINGEVENTS_HH_ +#define GZ_SENSORS_RENDERINGEVENTS_HH_ + +#include +#include + +// TODO(louise) Remove these pragmas once ign-rendering is disabling the +// warnings +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include +#include + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + class IGNITION_SENSORS_RENDERING_VISIBLE RenderingEvents + { + /// \brief Set a callback to be called when the scene is changed. + /// + /// \param[in] _callback This callback will be called every time the + /// scene is changed. + /// \remark Do not block inside of the callback. + /// \return A connection pointer that must remain in scope. When the + /// connection pointer falls out of scope, the connection is broken. + public: static gz::common::ConnectionPtr ConnectSceneChangeCallback( + std::function + _callback); + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Event that is used to trigger callbacks when the scene + /// is changed + public: static gz::common::EventT< + void(const gz::rendering::ScenePtr &)> sceneEvent; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} +#endif diff --git a/include/gz/sensors/RenderingSensor.hh b/include/gz/sensors/RenderingSensor.hh new file mode 100644 index 00000000..a0ce5ff7 --- /dev/null +++ b/include/gz/sensors/RenderingSensor.hh @@ -0,0 +1,99 @@ +/* + * 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 GZ_SENSORS_RENDERINGSENSOR_HH_ +#define GZ_SENSORS_RENDERINGSENSOR_HH_ + +#include + +#include + +// TODO(louise) Remove these pragmas once ign-rendering is disabling the +// warnings +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include "gz/sensors/rendering/Export.hh" +#include "gz/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class RenderingSensorPrivate; + + /// \brief a rendering sensor class + /// + /// This class is a base for all rendering sensor classes. It provides + /// interface to ignition rendering objects + class IGNITION_SENSORS_RENDERING_VISIBLE RenderingSensor + : public Sensor + { + /// \brief constructor + protected: RenderingSensor(); + + /// \brief destructor + public: virtual ~RenderingSensor(); + + /// \brief Set the rendering scene. + /// + /// \param[in] _scene Pointer to the scene + public: virtual void SetScene(rendering::ScenePtr _scene); + + /// \brief Get the rendering scene. + public: rendering::ScenePtr Scene() const; + + /// \brief Render update. This performs the actual render operation. + public: void Render(); + + /// \brief Set whether to update the scene graph manually. If set to true, + /// it is expected that rendering::Scene::PreRender is called manually + /// before calling Render() + /// \param[in] _manual True to enable manual scene graph update + public: void SetManualSceneUpdate(bool _manual); + + /// \brief Get whether the scene graph is updated manually. Defaults to + /// false. + /// \return True if manual scene graph update is enabled, false otherwise + /// \sa SetManualSceneUpdate + public: bool ManualSceneUpdate() const; + + /// \brief Add a rendering::Sensor. Its render updates will be handled + /// by this base class. + /// \param[in] _sensor Sensor to add. + protected: void AddSensor(rendering::SensorPtr _sensor); + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \internal + /// \brief Data pointer for private data + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/RgbdCameraSensor.hh b/include/gz/sensors/RgbdCameraSensor.hh new file mode 100644 index 00000000..f57c89a7 --- /dev/null +++ b/include/gz/sensors/RgbdCameraSensor.hh @@ -0,0 +1,102 @@ +/* + * 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 GZ_SENSORS_RGBDCAMERASENSOR_HH_ +#define GZ_SENSORS_RGBDCAMERASENSOR_HH_ + +#include + +#include + +#include +#include + +#include "gz/sensors/CameraSensor.hh" +#include "gz/sensors/config.hh" +#include "gz/sensors/rgbd_camera/Export.hh" +#include "gz/sensors/Export.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // forward declarations + class RgbdCameraSensorPrivate; + + /// \brief RGBD camera sensor class. + /// + /// This class creates a few types of sensor data from an ignition + /// rendering scene: + /// * RGB image (same as CameraSensor) + /// * Depth image (same as DepthCamera) + /// * (future / todo) Color point cloud + /// The scene must be created in advance and given to Manager::Init(). + /// It offers both an ignition-transport interface and a direct C++ API + /// to access the image data. The API works by setting a callback to be + /// called with image data. + class IGNITION_SENSORS_RGBD_CAMERA_VISIBLE RgbdCameraSensor + : public CameraSensor + { + /// \brief constructor + public: RgbdCameraSensor(); + + /// \brief destructor + public: virtual ~RgbdCameraSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successful + public: virtual bool Update(const common::Time &_now) override; + + /// \brief Set the rendering scene. + /// \param[in] _scene Pointer to the scene + public: virtual void SetScene( + gz::rendering::ScenePtr _scene) override; + + /// \brief Get image width. + /// \return width of the image + public: virtual unsigned int ImageWidth() const override; + + /// \brief Get image height. + /// \return height of the image + public: virtual unsigned int ImageHeight() const override; + + /// \brief Create an RGB camera and a depth camera. + /// \return True on success. + private: bool CreateCameras(); + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/Sensor.hh b/include/gz/sensors/Sensor.hh new file mode 100644 index 00000000..50b00cf0 --- /dev/null +++ b/include/gz/sensors/Sensor.hh @@ -0,0 +1,216 @@ +/* + * Copyright (C) 2017 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 GZ_SENSORS_SENSOR_HH_ +#define GZ_SENSORS_SENSOR_HH_ + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4005) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief A string used to identify a sensor + using SensorId = std::size_t; + const SensorId NO_SENSOR = 0; + + /// \brief forward declarations + class SensorPrivate; + + /// \brief a base sensor class + /// + /// This class is a base for all sensor classes. It parses some common + /// SDF elements in the tag and is responsible for making sure + /// sensors update at the right time. + class IGNITION_SENSORS_VISIBLE Sensor + { + /// \brief constructor + protected: Sensor(); + + /// \brief destructor + public: virtual ~Sensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF or inside of + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf); + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF or inside of + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf); + + /// \brief Initialize values in the sensor + public: virtual bool Init(); + + /// \brief Force the sensor to generate data + /// + /// This method must be overridden by sensors. Subclasses should not + /// not make a decision about whether or not they need to update. The + /// Sensor class will make sure Update() is called at the correct time. + /// + /// If a subclass wants to have a variable update rate it should call + /// SetUpdateRate(). + /// + /// A subclass should return false if there was an error while updating + /// \param[in] _now The current time + /// \return true if the update was successfull + /// \sa SetUpdateRate() + public: virtual bool Update(const common::Time &_now) = 0; + + /// \brief Return the next time the sensor will generate data + public: common::Time NextUpdateTime() const; + + /// \brief Update the sensor. + /// + /// This is called by the manager, and is responsible for determining + /// if this sensor needs to generate data at this time. If so, the + /// subclasses' Update() method will be called. + /// \param[in] _now The current time + /// \param[in] _force Force the update to happen even if it's not time + /// \return True if the update was triggered (_force was true or _now + /// >= next_update_time) and the sensor's + /// bool Sensor::Update(const common::Time &_now) function returned true. + /// False otherwise. + /// \remarks If forced the NextUpdateTime() will be unchanged. + /// \sa virtual bool Update(const common::Time &_name) = 0 + public: bool Update(const common::Time &_now, const bool _force); + + /// \brief Get the update rate of the sensor. + /// + /// The update rate is the number of times per second a sensor should + /// generate and output data. + /// \return _hz update rate of sensor. + public: double UpdateRate() const; + + /// \brief Set the update rate of the sensor. An update rate of zero means + /// that the sensor is updated every cycle. It's zero by default. + /// \detail Negative rates become zero. + /// \param[in] _hz Update rate of sensor in Hertz. + public: void SetUpdateRate(const double _hz); + + /// \brief Get the current pose. + /// \return Current pose of the sensor. + public: gz::math::Pose3d Pose() const; + + /// \brief Update the pose of the sensor + public: void SetPose(const gz::math::Pose3d &_pose); + + /// \brief Set the parent of the sensor + public: virtual void SetParent(const std::string &_parent); + + /// \brief Get name. + /// \return Name of sensor. + public: std::string Name() const; + + /// \brief FrameId. + /// \return FrameId of sensor. + public: std::string FrameId() const; + + /// \brief Set Frame ID of the sensor + /// \param[in] _frameId Frame ID of the sensor + public: void SetFrameId(const std::string &_frameId); + + /// \brief Get topic where sensor data is published. + /// \return Topic sensor publishes data to + public: std::string Topic() const; + + /// \brief Set topic where sensor data is published. + /// \param[in] _topic Topic sensor publishes data to. + /// \return True if a valid topic was set. + public: bool SetTopic(const std::string &_topic); + + /// \brief Get flag state for enabling performance metrics publication. + /// \return True if performance metrics are enabled, false otherwise. + public: bool EnableMetrics() const; + + /// \brief Set flag to enable publishing performance metrics + /// \param[in] _enableMetrics True to enable. + public: void SetEnableMetrics(bool _enableMetrics); + + /// \brief Get parent link of the sensor. + /// \return Parent link of sensor. + public: std::string Parent() const; + + /// \brief Get the sensor's ID. + /// \return The sensor's ID. + public: SensorId Id() const; + + /// \brief Get the SDF used to load this sensor. + /// \return Pointer to an SDF element that contains initialization + /// information for this sensor. + public: sdf::ElementPtr SDF() const; + + /// \brief Add a sequence number to an gz::msgs::Header. This + /// function can be called by a sensor that wants to add a sequence + /// number to a sensor message in order to have improved + /// accountability for generated sensor data. + /// + /// This function will add the following key-value pair to the `data` + /// field in the provided gz::msgs::Header msg. + /// + /// * key: "seq" + /// * value: `sequence_number` + /// + /// If the "seq" key already exists, then the value will be set + /// without adding another key-value pair. + /// + /// The `sequence_number` starts at zero, when a sensor is created, + /// and is incremented by one each time this function is called. + /// \param[in,out] _msg The header which will receive the sequence. + /// \param[in] _seqKey Name of the sequence to use. + public: void AddSequence(gz::msgs::Header *_msg, + const std::string &_seqKey = "default"); + + /// \brief Publishes information about the performance of the sensor. + /// This method is called by Update(). + /// \param[in] _now Current time. + public: void PublishMetrics( + const std::chrono::duration &_now); + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \internal + /// \brief Data pointer for private data + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/SensorFactory.hh b/include/gz/sensors/SensorFactory.hh new file mode 100644 index 00000000..ea3d11b7 --- /dev/null +++ b/include/gz/sensors/SensorFactory.hh @@ -0,0 +1,195 @@ +/* + * 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 GZ_SENSORS_SENSORFACTORY_HH_ +#define GZ_SENSORS_SENSORFACTORY_HH_ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include "gz/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // forward declaration + class SensorFactoryPrivate; + + /// \brief Base sensor plugin interface + class IGNITION_SENSORS_VISIBLE SensorPlugin + { + /// \brief Allows using shorter APIS in common::PluginLoader + public: IGN_COMMON_SPECIALIZE_INTERFACE(gz::sensors::SensorPlugin) + + /// \brief Instantiate new sensor + /// \return New sensor + public: virtual Sensor *New() = 0; + }; + + /// \brief Templated class for instantiating sensors of the specified type + /// \tparam Type of sensor being instantiated. + template + class SensorTypePlugin : public SensorPlugin + { + // Documentation inherited + public: SensorType *New() override + { + return new SensorType(); + }; + }; + + /// \brief A factory class for creating sensors + /// This class wll load a sensor plugin based on the given sensor type and + /// instantiates a sensor object + /// + class IGNITION_SENSORS_VISIBLE SensorFactory + { + /// \brief Constructor + public: SensorFactory(); + + /// \brief Destructor + public: ~SensorFactory(); + + /// \brief Create a sensor from a SDF DOM object with a known sensor type. + /// + /// This creates sensors by looking at the given SDF DOM object. + /// Sensors created with this API offer an ignition-transport interface. + /// If you need a direct C++ interface to the data, you must get the + /// sensor pointer and cast to the correct type. + /// + /// \sa Sensor() + /// \param[in] _sdf SDF Sensor DOM object. + /// \return A pointer to the created sensor. nullptr returned on + /// error. + public: template + std::unique_ptr CreateSensor(const sdf::Sensor &_sdf) + { + auto sensor = SensorFactory::CreateSensor(_sdf); + + if (sensor) + { + std::unique_ptr result( + dynamic_cast(sensor.release())); + + if (!result) + ignerr << "SDF sensor type does not match template type\n"; + + return result; + } + + ignerr << "Failed to create sensor of type[" + << _sdf.TypeStr() << "]\n"; + return nullptr; + } + + /// \brief Create a sensor from SDF with a known sensor type. + /// + /// This creates sensors by looking at the given sdf element. + /// Sensors created with this API offer an ignition-transport interface. + /// If you need a direct C++ interface to the data, you must get the + /// sensor pointer and cast to the correct type. + /// + /// \sa Sensor() + /// \param[in] _sdf pointer to the sdf element + /// \return A pointer to the created sensor. nullptr returned on + /// error. + public: template + std::unique_ptr CreateSensor(sdf::ElementPtr _sdf) + { + auto sensor = SensorFactory::CreateSensor(_sdf); + + if (sensor) + { + std::unique_ptr result( + dynamic_cast(sensor.release())); + + if (!result) + ignerr << "SDF sensor type does not match template type\n"; + + return result; + } + + ignerr << "Failed to create sensor of type[" + << _sdf->Get("type") << "]\n"; + return nullptr; + } + + /// \brief Create a sensor from SDF without a known sensor type. + /// + /// This creates sensors by looking at the given sdf element. + /// Sensors created with this API offer an ignition-transport interface. + /// If you need a direct C++ interface to the data, you must get the + /// sensor pointer and cast to the correct type. + /// + /// \sa Sensor() + /// \param[in] _sdf pointer to the sdf element + /// \return A sensor id that refers to the created sensor. Null is + /// is returned on error. + public: std::unique_ptr CreateSensor(sdf::ElementPtr _sdf); + + /// \brief Create a sensor from an SDF Sensor DOM object without a known + /// sensor type. + /// + /// This creates sensors by looking at the given SDF Sensor DOM + /// object. + /// Sensors created with this API offer an ignition-transport interface. + /// If you need a direct C++ interface to the data, you must get the + /// sensor pointer and cast to the correct type. + /// + /// \sa Sensor() + /// \param[in] _sdf SDF Sensor DOM object. + /// \return A sensor id that refers to the created sensor. Null is + /// is returned on error. + public: std::unique_ptr CreateSensor(const sdf::Sensor &_sdf); + + /// \brief Add additional path to search for sensor plugins + /// \param[in] _path Search path + public: void AddPluginPaths(const std::string &_path); + + /// \brief load a plugin and return a pointer + /// \param[in] _filename Sensor plugin file to load. + /// \return Pointer to the new sensor, nullptr on error. + private: std::shared_ptr LoadSensorPlugin( + const std::string &_filename); + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief private data pointer + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + + /// \brief Sensor registration macro + #define IGN_SENSORS_REGISTER_SENSOR(classname) \ + IGN_COMMON_REGISTER_SINGLE_PLUGIN(\ + gz::sensors::SensorTypePlugin, \ + gz::sensors::SensorPlugin) + } + } +} + +#endif diff --git a/include/gz/sensors/SensorTypes.hh b/include/gz/sensors/SensorTypes.hh new file mode 100644 index 00000000..b75d0897 --- /dev/null +++ b/include/gz/sensors/SensorTypes.hh @@ -0,0 +1,181 @@ +/* + * 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 GZ_SENSORS_SENSORTYPES_HH_ +#define GZ_SENSORS_SENSORTYPES_HH_ + +#include +#include + +#include +#include +#include + +/// \file +/// \ingroup ignition_sensors +/// \brief Forward declarations and typedefs for sensors +namespace ignition +{ + /// \ingroup ignition_sensors + /// \brief Sensors namespace + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // Forward declarations. + class AltimeterSensor; + class CameraSensor; + class GpuLidarSensor; + class GaussianNoiseModel; + class ImageGaussianNoiseModel; + class Noise; + class Sensor; + + /// \def SensorPtr + /// \brief Shared pointer to Sensor + typedef std::shared_ptr SensorPtr; + + /// \def CameraSensorPtr + /// \brief Shared pointer to CameraSensor + typedef std::shared_ptr CameraSensorPtr; + + /// \def GpuLidarSensorPtr + /// \brief Shared pointer to GpuLidarSensor + typedef std::shared_ptr GpuLidarSensorPtr; + + /// \def NoisePtr + /// \brief Shared pointer to Noise + typedef std::shared_ptr NoisePtr; + + /// \def GaussianNoisePtr + /// \brief Shared pointer to Noise + typedef std::shared_ptr GaussianNoiseModelPtr; + + /// \brief Shared pointer to Noise + typedef std::shared_ptr + ImageGaussianNoiseModelPtr; + + /// \def Sensor_V + /// \brief Vector of Sensor shared pointers + typedef std::vector Sensor_V; + + /// \def CameraSensor_V + /// \brief Vector of CameraSensor shared pointers + typedef std::vector CameraSensor_V; + + /// \def GpuLidarSensor_V + /// \brief Vector of GpuLidarSensor shared pointers + typedef std::vector GpuLidarSensor_V; + + /// \def SensorNoiseType + /// \brief Eumeration of all sensor noise types + enum SensorNoiseType + { + /// \internal + /// \brief Indicator used to create an iterator over the enum. Do not + /// use this. + SENSOR_NOISE_TYPE_BEGIN = 0, + + /// \brief Noise streams for the Camera sensor + /// \sa CameraSensor + NO_NOISE = SENSOR_NOISE_TYPE_BEGIN, + + /// \brief Noise streams for the Camera sensor + /// \sa CameraSensor + CAMERA_NOISE = 1, + + /// \brief Magnetometer body-frame X axis noise in Tesla + /// \sa MagnetometerSensor + MAGNETOMETER_X_NOISE_TESLA = 2, + + /// \brief Magnetometer body-frame Y axis noise in Tesla + /// \sa MagnetometerSensor + MAGNETOMETER_Y_NOISE_TESLA = 3, + + /// \brief Magnetometer body-frame Z axis noise in Tesla + /// \sa MagnetometerSensor + MAGNETOMETER_Z_NOISE_TESLA = 4, + + /// \brief Vertical noise stream for the altimeter sensor + /// \sa AltimeterSensor + ALTIMETER_VERTICAL_POSITION_NOISE_METERS = 5, + + /// \brief Velocity noise streams for the altimeter sensor + /// \sa AltimeterSensor + ALTIMETER_VERTICAL_VELOCITY_NOISE_METERS_PER_S = 6, + + /// \brief Air Pressure noise streams for the air pressure sensor + /// \sa AirPressureSensor + AIR_PRESSURE_NOISE_PASCALS = 7, + + /// \brief Accelerometer body-frame X axis noise in m/s^2 + /// \sa ImuSensor + ACCELEROMETER_X_NOISE_M_S_S = 8, + + /// \brief Accelerometer body-frame Y axis noise in m/s^2 + /// \sa ImuSensor + ACCELEROMETER_Y_NOISE_M_S_S = 9, + + /// \brief Accelerometer body-frame Z axis noise in m/s^2 + /// \sa ImuSensor + ACCELEROMETER_Z_NOISE_M_S_S = 10, + + /// \brief Gyroscope body-frame X axis noise in m/s^2 + /// \sa ImuSensor + GYROSCOPE_X_NOISE_RAD_S = 11, + + /// \brief Gyroscope body-frame X axis noise in m/s^2 + /// \sa ImuSensor + GYROSCOPE_Y_NOISE_RAD_S = 12, + + /// \brief Gyroscope body-frame X axis noise in m/s^2 + /// \sa ImuSensor + GYROSCOPE_Z_NOISE_RAD_S = 13, + + /// \brief Noise streams for the Lidar sensor + /// \sa Lidar + LIDAR_NOISE = 14, + + /// \internal + /// \brief Indicator used to create an iterator over the enum. Do not + /// use this. + SENSOR_NOISE_TYPE_END + }; + /// \} + + /// \brief SensorCategory is used to categorize sensors. This is used to + /// put sensors into different threads. + enum SensorCategory + { + // IMAGE must be the first element, and it must start with 0. Do not + // change this! See SensorManager::sensorContainers for reference. + /// \brief Image based sensor class. This type requires the rendering + /// engine. + IMAGE = 0, + + /// \brief Ray based sensor class. + RAY = 1, + + /// \brief A type of sensor is not a RAY or IMAGE sensor. + OTHER = 2, + + /// \brief Number of Sensor Categories + CATEGORY_COUNT = 3 + }; + } + } +} +#endif diff --git a/include/gz/sensors/ThermalCameraSensor.hh b/include/gz/sensors/ThermalCameraSensor.hh new file mode 100644 index 00000000..7b1d550d --- /dev/null +++ b/include/gz/sensors/ThermalCameraSensor.hh @@ -0,0 +1,185 @@ +/* + * 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 GZ_SENSORS_THERMALCAMERASENSOR_HH_ +#define GZ_SENSORS_THERMALCAMERASENSOR_HH_ + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4005) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +// TODO(louise) Remove these pragmas once ign-rendering is disabling the +// warnings +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include "gz/sensors/thermal_camera/Export.hh" +#include "gz/sensors/CameraSensor.hh" +#include "gz/sensors/Export.hh" +#include "gz/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // forward declarations + class ThermalCameraSensorPrivate; + + /// \brief Thermal camera sensor class. + /// + /// This class creates thermal image from an ignition rendering scene. + /// The scene must be created in advance and given to Manager::Init(). + /// It offers both an ignition-transport interface and a direct C++ API + /// to access the image data. The API works by setting a callback to be + /// called with image data. + class IGNITION_SENSORS_THERMAL_CAMERA_VISIBLE ThermalCameraSensor + : public CameraSensor + { + /// \brief constructor + public: ThermalCameraSensor(); + + /// \brief destructor + public: virtual ~ThermalCameraSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update(const common::Time &_now) override; + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual rendering::ThermalCameraPtr ThermalCamera(); + + /// \brief Thermal data callback used to get the data from the sensor + /// \param[in] _scan pointer to the data from the sensor + /// \param[in] _width width of the thermal image + /// \param[in] _height height of the thermal image + /// \param[in] _channel bytes used for the thermal data + /// \param[in] _format string with the format + public: void OnNewThermalFrame(const uint16_t *_scan, + unsigned int _width, unsigned int _height, + unsigned int _channels, + const std::string &_format); + + /// \brief Set a callback to be called when image frame data is + /// generated. + /// \param[in] _callback This callback will be called every time the + /// camera produces image data. The Update function will be blocked + /// while the callbacks are executed. + /// \remark Do not block inside of the callback. + /// \return A connection pointer that must remain in scope. When the + /// connection pointer falls out of scope, the connection is broken. + public: common::ConnectionPtr ConnectImageCallback( + std::function _callback); + + /// \brief Set the rendering scene. + /// \param[in] _scene Pointer to the scene + public: virtual void SetScene( + gz::rendering::ScenePtr _scene) override; + + /// \brief Get image width. + /// \return width of the image + public: virtual unsigned int ImageWidth() const override; + + /// \brief Get image height. + /// \return height of the image + public: virtual unsigned int ImageHeight() const override; + + /// \brief Set the ambient temperature of the environment + /// \param[in] _ambient Ambient temperature in kelvin + public: virtual void SetAmbientTemperature(float _ambient); + + /// \brief Set the range of ambient temperature + /// \param[in] _range The ambient temperature ranges from + /// (ambient - range/2) to (ambient + range/2). + public: virtual void SetAmbientTemperatureRange(float _range); + + /// \brief Set the minimum temperature the sensor can detect + /// \param[in] _min Min temperature in kelvin + public: virtual void SetMinTemperature(float _min); + + /// \brief Set the maximum temperature the sensor can detect + /// \param[in] _max Max temperature in kelvin + public: virtual void SetMaxTemperature(float _max); + + /// \brief Set the temperature linear resolution. The thermal image data + /// returned will be temperature in kelvin / resolution. + /// Typical values are 0.01 (10mK), 0.1 (100mK), or 0.04 to simulate + /// 14 bit format. + /// \param[in] resolution Temperature linear resolution + public: virtual void SetLinearResolution(float _resolution); + + /// \brief Create a camera in a scene + /// \return True on success. + private: bool CreateCamera(); + + /// \brief Callback that is triggered when the scene changes on + /// the Manager. + /// \param[in] _scene Pointer to the new scene. + private: void OnSceneChange(gz::rendering::ScenePtr /*_scene*/) + { } + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/ignition/sensors/config.hh.in b/include/gz/sensors/config.hh.in similarity index 72% rename from include/ignition/sensors/config.hh.in rename to include/gz/sensors/config.hh.in index 8e756279..9f68325a 100644 --- a/include/ignition/sensors/config.hh.in +++ b/include/gz/sensors/config.hh.in @@ -10,7 +10,7 @@ #define IGNITION_SENSORS_VERSION_NAMESPACE v${PROJECT_VERSION_MAJOR} -#define IGNITION_SENSORS_VERSION_HEADER "Ignition ${GZ_DESIGNATION_CAP}, version ${PROJECT_VERSION_FULL}\nCopyright (C) 2014 Open Source Robotics Foundation.\nReleased under the Apache 2.0 License.\n\n" +#define IGNITION_SENSORS_VERSION_HEADER "Gazebo ${GZ_DESIGNATION_CAP}, version ${PROJECT_VERSION_FULL}\nCopyright (C) 2014 Open Source Robotics Foundation.\nReleased under the Apache 2.0 License.\n\n" #cmakedefine BUILD_TYPE_PROFILE 1 #cmakedefine BUILD_TYPE_DEBUG 1 @@ -18,3 +18,12 @@ #define IGN_SENSORS_PLUGIN_PATH "@IGN_SENSORS_PLUGIN_PATH@" #define IGN_SENSORS_PLUGIN_NAME(name) std::string("@PROJECT_LIBRARY_TARGET_NAME@-")+name + +namespace ignition +{ +} + +namespace gz +{ + using namespace ignition; +} diff --git a/include/ignition/sensors/sensors.hh.in b/include/gz/sensors/sensors.hh.in similarity index 52% rename from include/ignition/sensors/sensors.hh.in rename to include/gz/sensors/sensors.hh.in index 1c79666b..61b338de 100644 --- a/include/ignition/sensors/sensors.hh.in +++ b/include/gz/sensors/sensors.hh.in @@ -1,3 +1,3 @@ // Automatically generated -#include +#include ${ign_headers} diff --git a/include/ignition/sensors.hh b/include/ignition/sensors.hh new file mode 100644 index 00000000..c71c526e --- /dev/null +++ b/include/ignition/sensors.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/AirPressureSensor.hh b/include/ignition/sensors/AirPressureSensor.hh index 149c8935..f8ee87f7 100644 --- a/include/ignition/sensors/AirPressureSensor.hh +++ b/include/ignition/sensors/AirPressureSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -13,79 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_AIRPRESSURESENSOR_HH_ -#define IGNITION_SENSORS_AIRPRESSURESENSOR_HH_ - -#include - -#include - -#include -#include + */ +#include #include -#include - -#include "ignition/sensors/Sensor.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - /// \brief forward declarations - class AirPressureSensorPrivate; - - /// \brief AirPressure Sensor Class - /// - /// A sensor that reports air pressure readings. - class IGNITION_SENSORS_AIR_PRESSURE_VISIBLE AirPressureSensor : - public Sensor - { - /// \brief constructor - public: AirPressureSensor(); - - /// \brief destructor - public: virtual ~AirPressureSensor(); - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(sdf::ElementPtr _sdf) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Update the sensor and generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; - - /// \brief Set the reference altitude. - /// \param[in] _ref Verical reference position in meters - public: void SetReferenceAltitude(double _reference); - - /// \brief Get the vertical reference altitude. - /// \return Verical reference position in meters - public: double ReferenceAltitude() const; - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif diff --git a/include/ignition/sensors/AltimeterSensor.hh b/include/ignition/sensors/AltimeterSensor.hh index ee604278..bc14c876 100644 --- a/include/ignition/sensors/AltimeterSensor.hh +++ b/include/ignition/sensors/AltimeterSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -13,96 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_ALTIMETERSENSOR_HH_ -#define IGNITION_SENSORS_ALTIMETERSENSOR_HH_ - -#include - -#include - -#include -#include + */ +#include #include -#include - -#include "ignition/sensors/Sensor.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - /// \brief forward declarations - class AltimeterSensorPrivate; - - /// \brief Altimeter Sensor Class - /// - /// An altimeter sensor that reports vertical position and velocity - /// readings over ign transport - class IGNITION_SENSORS_ALTIMETER_VISIBLE AltimeterSensor : public Sensor - { - /// \brief constructor - public: AltimeterSensor(); - - /// \brief destructor - public: virtual ~AltimeterSensor(); - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(sdf::ElementPtr _sdf) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Update the sensor and generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; - - /// \brief Set the vertical reference position of the altimeter - /// \param[in] _ref Verical reference position in meters - public: void SetVerticalReference(double _reference); - - /// \brief Get the vertical reference position of the altimeter - /// \return Verical reference position in meters - public: double VerticalReference() const; - - /// \brief Set the current z position of the altimeter - /// \param[in] _pos Z position in meters - public: void SetPosition(double _pos); - - /// \brief Get the vertical position of the altimeter relative to the - /// reference position - /// \return Vertical position relative to referene position - public: double VerticalPosition() const; - - /// \brief Set the vertical velocity of the altimeter - /// \param[in] _vel Vertical velocity in meters per second - public: void SetVerticalVelocity(double _vel); - - /// \brief Get the vertical velocity of the altimeter - /// \return Vertical velocity in meters per second - public: double VerticalVelocity() const; - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif diff --git a/include/ignition/sensors/CameraSensor.hh b/include/ignition/sensors/CameraSensor.hh index 399d1f01..bb538944 100644 --- a/include/ignition/sensors/CameraSensor.hh +++ b/include/ignition/sensors/CameraSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * 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. @@ -13,169 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_CAMERASENSOR_HH_ -#define IGNITION_SENSORS_CAMERASENSOR_HH_ + */ -#include -#include -#include - -#include - -#include -#include -#include - -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif - -// TODO(louise) Remove these pragmas once ign-rendering is disabling the -// warnings -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif - -#include "ignition/sensors/camera/Export.hh" -#include "ignition/sensors/config.hh" -#include "ignition/sensors/Export.hh" -#include "ignition/sensors/RenderingSensor.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - /// \brief forward declarations - class CameraSensorPrivate; - - /// \brief Camera Sensor Class - /// - /// This class creates images from an ignition rendering scene. The scene - /// must be created in advance and given to Manager::Init(). - /// It offers both an ignition-transport interface and a direct C++ API - /// to access the image data. The API works by setting a callback to be - /// called with image data. - class IGNITION_SENSORS_CAMERA_VISIBLE CameraSensor : public RenderingSensor - { - /// \brief constructor - public: CameraSensor(); - - /// \brief destructor - public: virtual ~CameraSensor(); - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(sdf::ElementPtr _sdf) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Force the sensor to generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; - - /// \brief Set a callback to be called when image frame data is - /// generated. - /// \param[in] _callback This callback will be called every time the - /// camera produces image data. The Update function will be blocked - /// while the callbacks are executed. - /// \remark Do not block inside of the callback. - /// \return A connection pointer that must remain in scope. When the - /// connection pointer falls out of scope, the connection is broken. - public: ignition::common::ConnectionPtr ConnectImageCallback( - std::function< - void(const ignition::msgs::Image &)> _callback); - - /// \brief Set the rendering scene. - /// \param[in] _scene Pointer to the scene - public: virtual void SetScene( - ignition::rendering::ScenePtr _scene) override; - - /// \brief Get image width. - /// \return width of the image - public: virtual unsigned int ImageWidth() const; - - /// \brief Get image height. - /// \return height of the image - public: virtual unsigned int ImageHeight() const; - - /// \brief Get pointer to rendering camera object. - /// \return Camera in Ignition Rendering. - public: rendering::CameraPtr RenderingCamera() const; - - /// \brief Topic where camera info is published. - /// \return Camera info topic. - public: std::string InfoTopic() const; - - /// \brief Set baseline for stereo cameras. This is used to populate the - /// projection matrix in the camera info message. - /// \param[in] _baseline The distance from the 1st camera, in meters. - public: void SetBaseline(double _baseline); - - /// \brief Get baseline for stereo cameras. - /// \return The distance from the 1st camera, in meters. - public: double Baseline() const; - - /// \brief Advertise camera info topic. - /// \return True if successful. - protected: bool AdvertiseInfo(); - - /// \brief Advertise camera info topic. - /// This version takes a string that allows one to override the - /// camera_info topic. - /// \param[in] _topic The topic on which camera info is to be published. - /// \return True if successful. - protected: bool AdvertiseInfo(const std::string &_topic); - - /// \brief Populate camera info message. - /// \param[in] _cameraSdf Pointer to SDF object containing camera - /// information. - protected: void PopulateInfo(const sdf::Camera *_cameraSdf); - - /// \brief Publish camera info message. - /// \param[in] _now The current time - protected: void PublishInfo(const ignition::common::Time &_now); - - /// \brief Create a camera in a scene - /// \return True on success. - private: bool CreateCamera(); - - /// \brief Callback that is triggered when the scene changes on - /// the Manager. - /// \param[in] _scene Pointer to the new scene. - private: void OnSceneChange(ignition::rendering::ScenePtr /*_scene*/); - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/DepthCameraSensor.hh b/include/ignition/sensors/DepthCameraSensor.hh index 5ee926d3..59a62f41 100644 --- a/include/ignition/sensors/DepthCameraSensor.hh +++ b/include/ignition/sensors/DepthCameraSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -13,169 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_DEPTHCAMERASENSOR_HH_ -#define IGNITION_SENSORS_DEPTHCAMERASENSOR_HH_ + */ -#include -#include -#include - -#include - -#include -#include -#include -#include - -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif - -// TODO(louise) Remove these pragmas once ign-rendering is disabling the -// warnings -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif - -#include "ignition/sensors/depth_camera/Export.hh" -#include "ignition/sensors/CameraSensor.hh" -#include "ignition/sensors/Export.hh" -#include "ignition/sensors/Sensor.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // forward declarations - class DepthCameraSensorPrivate; - - /// \brief Depth camera sensor class. - /// - /// This class creates depth image from an ignition rendering scene. - /// The scene must be created in advance and given to Manager::Init(). - /// It offers both an ignition-transport interface and a direct C++ API - /// to access the image data. The API works by setting a callback to be - /// called with image data. - class IGNITION_SENSORS_DEPTH_CAMERA_VISIBLE DepthCameraSensor - : public CameraSensor - { - /// \brief constructor - public: DepthCameraSensor(); - - /// \brief destructor - public: virtual ~DepthCameraSensor(); - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(sdf::ElementPtr _sdf) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Force the sensor to generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; - - /// \brief Force the sensor to generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual rendering::DepthCameraPtr DepthCamera(); - - /// \brief Depth data callback used to get the data from the sensor - /// \param[in] _scan pointer to the data from the sensor - /// \param[in] _width width of the depth image - /// \param[in] _height height of the depth image - /// \param[in] _channel bytes used for the depth data - /// \param[in] _format string with the format - public: void OnNewDepthFrame(const float *_scan, - unsigned int _width, unsigned int _height, - unsigned int /*_channels*/, - const std::string &/*_format*/); - - /// \brief Point cloud data callback used to get the data from the sensor - /// \param[in] _data pointer to the data from the sensor - /// \param[in] _width width of the point cloud image - /// \param[in] _height height of the point cloud image - /// \param[in] _channel bytes used for the point cloud data - /// \param[in] _format string with the format - public: void OnNewRgbPointCloud(const float *_data, - unsigned int _width, unsigned int _height, - unsigned int /*_channels*/, - const std::string &/*_format*/); - - /// \brief Set a callback to be called when image frame data is - /// generated. - /// \param[in] _callback This callback will be called every time the - /// camera produces image data. The Update function will be blocked - /// while the callbacks are executed. - /// \remark Do not block inside of the callback. - /// \return A connection pointer that must remain in scope. When the - /// connection pointer falls out of scope, the connection is broken. - public: ignition::common::ConnectionPtr ConnectImageCallback( - std::function< - void(const ignition::msgs::Image &)> _callback); - - /// \brief Set the rendering scene. - /// \param[in] _scene Pointer to the scene - public: virtual void SetScene( - ignition::rendering::ScenePtr _scene) override; - - /// \brief Get image width. - /// \return width of the image - public: virtual unsigned int ImageWidth() const override; - - /// \brief Get image height. - /// \return height of the image - public: virtual unsigned int ImageHeight() const override; - - /// \brief Get image width. - /// \return width of the image - public: virtual double FarClip() const; - - /// \brief Get image height. - /// \return height of the image - public: virtual double NearClip() const; - - /// \brief Create a camera in a scene - /// \return True on success. - private: bool CreateCamera(); - - /// \brief Callback that is triggered when the scene changes on - /// the Manager. - /// \param[in] _scene Pointer to the new scene. - private: void OnSceneChange(ignition::rendering::ScenePtr /*_scene*/) - { } - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/Export.hh b/include/ignition/sensors/Export.hh new file mode 100644 index 00000000..db719f28 --- /dev/null +++ b/include/ignition/sensors/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/GaussianNoiseModel.hh b/include/ignition/sensors/GaussianNoiseModel.hh index 70eaec6a..0c2b8f15 100644 --- a/include/ignition/sensors/GaussianNoiseModel.hh +++ b/include/ignition/sensors/GaussianNoiseModel.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -13,65 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_SENSORS_GAUSSIANNOISEMODEL_HH_ -#define IGNITION_SENSORS_GAUSSIANNOISEMODEL_HH_ - -#include - -#include "ignition/sensors/config.hh" -#include "ignition/sensors/Export.hh" -#include "ignition/sensors/Noise.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - // Forward declarations - class GaussianNoiseModelPrivate; - - /** \class GaussianNoiseModel GaussianNoiseModel.hh \ - ignition/sensors/GaussianNoiseModel.hh - **/ - /// \brief Gaussian noise class - class IGNITION_SENSORS_VISIBLE GaussianNoiseModel : public Noise - { - /// \brief Constructor. - public: GaussianNoiseModel(); - - /// \brief Destructor. - public: virtual ~GaussianNoiseModel(); - - // Documentation inherited. - public: virtual void Load(const sdf::Noise &_sdf) override; - - // Documentation inherited. - public: double ApplyImpl(double _in, double _dt) override; - - /// \brief Accessor for mean. - /// \return Mean of Gaussian noise. - public: double Mean() const; - - /// \brief Accessor for stddev. - /// \return Standard deviation of Gaussian noise. - public: double StdDev() const; - - /// \brief Accessor for bias. - /// \return Bias on output. - public: double Bias() const; - - /// Documentation inherited - public: virtual void Print(std::ostream &_out) const override; - - /// \brief Private data pointer. - private: GaussianNoiseModelPrivate *dataPtr = nullptr; - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/GpuLidarSensor.hh b/include/ignition/sensors/GpuLidarSensor.hh index a2fd38f6..46a87866 100644 --- a/include/ignition/sensors/GpuLidarSensor.hh +++ b/include/ignition/sensors/GpuLidarSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -13,128 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_GPULIDARSENSOR_HH_ -#define IGNITION_SENSORS_GPULIDARSENSOR_HH_ + */ -#include -#include - -#include - -#include - -// TODO(louise) Remove these pragmas once ign-rendering is disabling the -// warnings -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif - -#include "ignition/sensors/gpu_lidar/Export.hh" -#include "ignition/sensors/RenderingEvents.hh" -#include "ignition/sensors/Lidar.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - /// \brief forward declarations - class GpuLidarSensorPrivate; - - /// \brief GpuLidar Sensor Class - /// - /// This class creates laser scans using the GPU. It's measures the range - /// from the origin of the center to points on the visual geometry in the - /// scene. - /// - /// It offers both an ignition-transport interface and a direct C++ API - /// to access the image data. The API works by setting a callback to be - /// called with image data. - class IGNITION_SENSORS_GPU_LIDAR_VISIBLE GpuLidarSensor : public Lidar - { - /// \brief constructor - public: GpuLidarSensor(); - - /// \brief destructor - public: virtual ~GpuLidarSensor(); - - /// \brief Force the sensor to generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Load sensor sata from SDF - /// \param[in] _sdf SDF used - /// \return True on success - public: virtual bool Load(sdf::ElementPtr _sdf) override; - - /// \brief Create Lidar sensor - public: virtual bool CreateLidar() override; - - /// \brief Gets if sensor is horizontal - /// \return True if horizontal, false if not - public: bool IsHorizontal() const; - - /// \brief Makes possible to change sensor scene - /// \param[in] _scene used with the sensor - public: void SetScene(ignition::rendering::ScenePtr _scene) override; - - /// \brief Remove sensor from scene - /// \param[in] _scene used with the sensor - public: void RemoveGpuRays(ignition::rendering::ScenePtr _scene); - - /// \brief Get Gpu Rays object used in the sensor - /// \return Pointer to ignition::rendering::GpuRays - public: ignition::rendering::GpuRaysPtr GpuRays() const; - - /// \brief Return the ratio of horizontal ray count to vertical ray - /// count. - /// - /// A ray count is the number of simulated rays. Whereas a range count - /// is the total number of data points returned. When range count - /// != ray count, then values are interpolated between rays. - public: double RayCountRatio() const; - - /// \brief Get the horizontal field of view of the laser sensor. - /// \return The horizontal field of view of the laser sensor. - public: ignition::math::Angle HFOV() const; - - /// \brief Get the vertical field-of-view. - /// \return Vertical field of view. - public: ignition::math::Angle VFOV() const; - - /// \brief Connect function pointer to internal GpuRays callback - /// \return ignition::common::Connection pointer - public: virtual ignition::common::ConnectionPtr ConnectNewLidarFrame( - std::function _subscriber) override; - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/ImageGaussianNoiseModel.hh b/include/ignition/sensors/ImageGaussianNoiseModel.hh index f20ca0bc..37574ca4 100644 --- a/include/ignition/sensors/ImageGaussianNoiseModel.hh +++ b/include/ignition/sensors/ImageGaussianNoiseModel.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -13,64 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_SENSORS_IMAGEGAUSSIANNOISEMODEL_HH_ -#define IGNITION_SENSORS_IMAGEGAUSSIANNOISEMODEL_HH_ - -#include - -// TODO(louise) Remove these pragmas once ign-rendering is disabling the -// warnings -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif - -#include "ignition/sensors/config.hh" -#include "ignition/sensors/GaussianNoiseModel.hh" -#include "ignition/sensors/rendering/Export.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // Forward declarations - class ImageGaussianNoiseModelPrivate; - - /** \class ImageGaussianNoiseModel GaussianNoiseModel.hh \ - ignition/sensors/GaussianNoiseModel.hh - **/ - /// \brief Gaussian noise class for image sensors - class IGNITION_SENSORS_RENDERING_VISIBLE ImageGaussianNoiseModel : - public GaussianNoiseModel - { - /// \brief Constructor. - public: ImageGaussianNoiseModel(); - - /// \brief Destructor. - public: virtual ~ImageGaussianNoiseModel(); - - // Documentation inherited. - public: virtual void Load(const sdf::Noise &_sdf) override; - - // Documentation inherited. - public: virtual void SetCamera(rendering::CameraPtr _camera); - - /// Documentation inherited - public: virtual void Print(std::ostream &_out) const override; - - /// \brief Private data pointer. - private: ImageGaussianNoiseModelPrivate *dataPtr = nullptr; - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/ImageNoise.hh b/include/ignition/sensors/ImageNoise.hh index 20dc2606..dd1f3cf0 100644 --- a/include/ignition/sensors/ImageNoise.hh +++ b/include/ignition/sensors/ImageNoise.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -13,53 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_SENSORS_IMAGENOISE_HH_ -#define IGNITION_SENSORS_IMAGENOISE_HH_ - -#include - -#include - -#include "ignition/sensors/config.hh" -#include "ignition/sensors/SensorTypes.hh" -#include "ignition/sensors/rendering/Export.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // Forward declarations - class NoisePrivate; - - /// \class NoiseFactory Noise.hh ignition/sensors/Noise.hh - /// \brief Use this noise manager for creating and loading noise models. - class IGNITION_SENSORS_RENDERING_VISIBLE ImageNoiseFactory - { - /// \brief Load a noise model based on the input sdf parameters and - /// sensor type. - /// \param[in] _sdf Noise sdf parameters. - /// \param[in] _sensorType Type of sensor. This is currently used to - /// distinguish between image and non image sensors in order to create - /// the appropriate noise model. - /// \return Pointer to the noise model created. - public: static NoisePtr NewNoiseModel(sdf::ElementPtr _sdf, - const std::string &_sensorType = ""); - - /// \brief Load a noise model based on the input sdf parameters and - /// sensor type. - /// \param[in] _sdf Noise sdf parameters. - /// \param[in] _sensorType Type of sensor. This is currently used to - /// distinguish between image and non image sensors in order to create - /// the appropriate noise model. - /// \return Pointer to the noise model created. - public: static NoisePtr NewNoiseModel(const sdf::Noise &_sdf, - const std::string &_sensorType = ""); - }; - } - } -} -#endif +#include +#include diff --git a/include/ignition/sensors/ImuSensor.hh b/include/ignition/sensors/ImuSensor.hh index 2da73df8..8ab34787 100644 --- a/include/ignition/sensors/ImuSensor.hh +++ b/include/ignition/sensors/ImuSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -13,123 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_IMUSENSOR_HH_ -#define IGNITION_SENSORS_IMUSENSOR_HH_ - -#include - -#include - -#include -#include -#include + */ +#include #include -#include - -#include "ignition/sensors/Sensor.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - /// \brief forward declarations - class ImuSensorPrivate; - - /// \brief Imu Sensor Class - /// - /// An imu sensor that reports linear acceleration, angular velocity, and - /// orientation - class IGNITION_SENSORS_IMU_VISIBLE ImuSensor : public Sensor - { - /// \brief constructor - public: ImuSensor(); - - /// \brief destructor - public: virtual ~ImuSensor(); - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(sdf::ElementPtr _sdf) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Update the sensor and generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; - - /// \brief Set the angular velocity of the imu - /// \param[in] _angularVel Angular velocity of the imu in body frame - /// expressed in radians per second - public: void SetAngularVelocity(const math::Vector3d &_angularVel); - - /// \brief Get the angular velocity of the imu - /// \return Angular velocity of the imu in body frame, expressed in - /// radians per second. - public: math::Vector3d AngularVelocity() const; - - /// \brief Set the linear acceleration of the imu - /// \param[in] _linearAcc Linear accceleration of the imu in body frame - /// expressed in meters per second squared. - public: void SetLinearAcceleration(const math::Vector3d &_linearAcc); - - /// \brief Get the linear acceleration of the imu - /// \return Linear acceleration of the imu in local frame, expressed in - /// meters per second squared. - public: math::Vector3d LinearAcceleration() const; - - /// \brief Set the world pose of the imu - /// \param[in] _pose Pose in world frame - public: void SetWorldPose(const math::Pose3d _pose); - - /// \brief Get the world pose of the imu - /// \return Pose in world frame. - public: math::Pose3d WorldPose() const; - - /// \brief Set the orientation reference, i.e. initial imu - /// orientation. Imu orientation data generated will be relative to this - /// reference frame. - /// \param[in] _orientation Reference orientation - public: void SetOrientationReference( - const math::Quaterniond &_orient); - - /// \brief Get the world orienation reference of the imu - /// \return Orientation reference in world frame - public: math::Quaterniond OrientationReference() const; - - /// \brief Get the orienation of the imu with respect to reference frame - /// \return Orientation in reference frame - public: math::Quaterniond Orientation() const; - - /// \brief Set the gravity vector - /// \param[in] _gravity gravity vector in meters per second squared. - public: void SetGravity(const math::Vector3d &_gravity); - - /// \brief Get the gravity vector - /// \return Gravity vectory in meters per second squared. - public: math::Vector3d Gravity() const; - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif diff --git a/include/ignition/sensors/Lidar.hh b/include/ignition/sensors/Lidar.hh index 0deecae5..f0d50f8c 100644 --- a/include/ignition/sensors/Lidar.hh +++ b/include/ignition/sensors/Lidar.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -13,258 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_LIDAR_HH_ -#define IGNITION_SENSORS_LIDAR_HH_ + */ -#include -#include -#include - -#include -#include - -#include "ignition/sensors/lidar/Export.hh" -#include "ignition/sensors/RenderingSensor.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - /// \brief forward declarations - class LidarPrivate; - - /// \brief Lidar Sensor Class - /// - /// This class creates laser scans using. It's measures the range - /// from the origin of the center to points on the visual geometry in the - /// scene. - /// - /// It offers both an ignition-transport interface and a direct C++ API - /// to access the image data. The API works by setting a callback to be - /// called with image data. - class IGNITION_SENSORS_LIDAR_VISIBLE Lidar : public RenderingSensor - { - /// \brief constructor - public: Lidar(); - - /// \brief destructor - public: virtual ~Lidar(); - - /// \brief Force the sensor to generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; - - /// \brief Apply noise to the laser buffer, if noise has been - /// configured. This should be called before PublishLidarScan if you - /// want the scan data to contain noise. - public: void ApplyNoise(); - - /// \brief Publish LaserScan message - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual bool PublishLidarScan(const common::Time &_now); - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(sdf::ElementPtr _sdf) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: void SetParent(const std::string &_parent) override; - - /// \brief Create Lidar sensor - public: virtual bool CreateLidar(); - - /// \brief Finalize the ray - protected: virtual void Fini(); - - /// \brief Get the minimum angle - /// \return The minimum angle - public: ignition::math::Angle AngleMin() const; - - /// \brief Set the scan minimum angle - /// \param[in] _angle The minimum angle - public: void SetAngleMin(const double _angle); - - /// \brief Get the maximum angle - /// \return the maximum angle - public: ignition::math::Angle AngleMax() const; - - /// \brief Set the scan maximum angle - /// \param[in] _angle The maximum angle - public: void SetAngleMax(const double _angle); - - /// \brief Get radians between each range - /// \return Return angle resolution - public: double AngleResolution() const; - - /// \brief Get the minimum range - /// \return The minimum range - public: double RangeMin() const; - - /// \brief Get the maximum range - /// \return The maximum range - public: double RangeMax() const; - - /// \brief Get the range resolution - /// If RangeResolution is 1, the number of simulated rays is equal - /// to the number of returned range readings. If it's less than 1, - /// fewer simulated rays than actual returned range readings are - /// used, the results are interpolated from two nearest neighbors, - /// and vice versa. - /// \return The Range Resolution - public: double RangeResolution() const; - - /// \brief Get the ray count - /// \return The number of rays - public: unsigned int RayCount() const; - - /// \brief Get the range count - /// \return The number of ranges - public: unsigned int RangeCount() const; - - /// \brief Get the vertical scan line count - /// \return The number of scan lines vertically - public: unsigned int VerticalRayCount() const; - - /// \brief Get the vertical scan line count - /// \return The number of scan lines vertically - public: unsigned int VerticalRangeCount() const; - - /// \brief Get the vertical scan bottom angle - /// \return The minimum angle of the scan block - public: ignition::math::Angle VerticalAngleMin() const; - - /// \brief Set the vertical scan bottom angle - /// \param[in] _angle The minimum angle of the scan block - public: void SetVerticalAngleMin(const double _angle); - - /// \brief Get the vertical scan line top angle - /// \return The Maximum angle of the scan block - public: ignition::math::Angle VerticalAngleMax() const; - - /// \brief Set the vertical scan line top angle - /// \param[in] _angle The Maximum angle of the scan block - public: void SetVerticalAngleMax(const double _angle); - - /// \brief Get the vertical angle in radians between each range - /// \return Resolution of the angle - public: double VerticalAngleResolution() const; - - /// \brief Get detected range for a ray. - /// Warning: If you are accessing all the ray data in a loop - /// it's possible that the Ray will update in the middle of - /// your access loop. This means some data will come from one - /// scan, and some from another scan. You can solve this - /// problem by using SetActive(false) - /// SetActive(true). - /// \param[in] _index Index of specific ray - /// \return Returns RangeMax for no detection. - public: double Range(const int _index) const; - - /// \brief Get all the ranges - /// \param[out] _range A vector that will contain all the range data - public: void Ranges(std::vector &_ranges) const; - - /// \brief Get detected retro (intensity) value for a ray. - /// Warning: If you are accessing all the ray data in a loop - /// it's possible that the Ray will update in the middle of - /// your access loop. This means some data will come from one - /// scan, and some from another scan. You can solve this - /// problem by using SetActive(false) - /// SetActive(true). - /// \param[in] _index Index of specific ray - /// \return Intensity value of ray - public: double Retro(const int _index) const; - - /// \brief Get detected fiducial value for a ray. - /// Warning: If you are accessing all the ray data in a loop - /// it's possible that the Ray will update in the middle of - /// your access loop. This means some data will come from one - /// scan, and some from another scan. You can solve this - /// problem by using SetActive(false) - /// SetActive(true). - /// \param[in] _index Index of specific ray - /// \return Fiducial value of ray - public: int Fiducial(const unsigned int _index) const; - - /// \brief Gets if sensor is horizontal - /// \return True if horizontal, false if not - public: bool IsHorizontal() const; - - /// \brief Return the ratio of horizontal ray count to vertical ray - /// count. - /// - /// A ray count is the number of simulated rays. Whereas a range count - /// is the total number of data points returned. When range count - /// != ray count, then values are interpolated between rays. - public: double RayCountRatio() const; - - /// \brief Return the ratio of horizontal range count to vertical - /// range count. - /// - /// A ray count is the number of simulated rays. Whereas a range count - /// is the total number of data points returned. When range count - /// != ray count, then values are interpolated between rays. - public: double RangeCountRatio() const; - - /// \brief Get the horizontal field of view of the laser sensor. - /// \return The horizontal field of view of the laser sensor. - public: double HorzFOV() const; - - /// \brief Get the vertical field-of-view. - /// \return Vertical field of view. - public: double VertFOV() const; - - // Documentation inherited - public: virtual bool IsActive() const; - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Just a mutex for thread safety - public: mutable std::mutex lidarMutex; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - - /// \brief Raw buffer of laser data. - public: float *laserBuffer = nullptr; - - /// \brief true if Load() has been called and was successful - public: bool initialized = false; - - /// \brief Set a callback to be called when data is generated. - /// \param[in] _callback This callback will be called every time the - /// sensor generates data. The Update function will be blocked while the - /// callbacks are executed. - /// \remark Do not block inside of the callback. - /// \return A connection pointer that must remain in scope. When the - /// connection pointer falls out of scope, the connection is broken. - public: virtual ignition::common::ConnectionPtr ConnectNewLidarFrame( - std::function _subscriber); - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/LogicalCameraSensor.hh b/include/ignition/sensors/LogicalCameraSensor.hh index cb4d33fd..fcc3a067 100644 --- a/include/ignition/sensors/LogicalCameraSensor.hh +++ b/include/ignition/sensors/LogicalCameraSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -13,113 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_LOGICALCAMERASENSOR_HH_ -#define IGNITION_SENSORS_LOGICALCAMERASENSOR_HH_ + */ -#include -#include -#include - -#include - -#include -#include -#include - -#include - -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif - -#include "ignition/sensors/config.hh" -#include "ignition/sensors/Export.hh" -#include "ignition/sensors/logical_camera/Export.hh" -#include "ignition/sensors/Sensor.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - /// \brief forward declarations - class LogicalCameraSensorPrivate; - - /// \brief Logical Camera Sensor Class - /// - /// A logical camera reports locations of objects. This camera finds models - /// within the sensor's frustum and publishes information about the models - /// on the sensor's topic. - class IGNITION_SENSORS_LOGICAL_CAMERA_VISIBLE LogicalCameraSensor - : public Sensor - { - /// \brief constructor - public: LogicalCameraSensor(); - - /// \brief destructor - public: virtual ~LogicalCameraSensor(); - - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(sdf::ElementPtr _sdf) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Force the sensor to generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; - - /// \brief Get the near distance. This is the distance from the - /// frustum's vertex to the closest plane. - /// \return Near distance. - public: double Near() const; - - /// \brief Get the far distance. This is the distance from the - /// frustum's vertex to the farthest plane. - /// \return Far distance. - public: double Far() const; - - /// \brief Set the models currently in the world - /// \param[in] _models A map of model names to their world pose. - public: void SetModelPoses(std::map &&_models); - - /// \brief Get the horizontal field of view. The field of view is the - /// angle between the frustum's vertex and the edges of the near or far - /// plane. This value represents the horizontal angle. - /// \return The field of view. - public: ignition::math::Angle HorizontalFOV() const; - - /// \brief Get the aspect ratio, which is the width divided by height - /// of the near or far planes. - /// \return The frustum's aspect ratio. - public: double AspectRatio() const; - - /// \brief Get the latest image. An image is an instance of - /// msgs::LogicalCameraImage, which contains a list of detected models. - /// \return List of detected models. - public: msgs::LogicalCameraImage Image() const; - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/MagnetometerSensor.hh b/include/ignition/sensors/MagnetometerSensor.hh index f6ca8c08..85c84180 100644 --- a/include/ignition/sensors/MagnetometerSensor.hh +++ b/include/ignition/sensors/MagnetometerSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -13,92 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_MAGNETOMETERSENSOR_HH_ -#define IGNITION_SENSORS_MAGNETOMETERSENSOR_HH_ - -#include - -#include - -#include -#include -#include + */ +#include #include -#include - -#include "ignition/sensors/Sensor.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - /// \brief forward declarations - class MagnetometerSensorPrivate; - - /// \brief Magnetometer Sensor Class - /// - /// A magnetometer reports the magnetic field vector - class IGNITION_SENSORS_MAGNETOMETER_VISIBLE MagnetometerSensor - : public Sensor - { - /// \brief constructor - public: MagnetometerSensor(); - - /// \brief destructor - public: virtual ~MagnetometerSensor(); - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(sdf::ElementPtr _sdf) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Update the sensor and generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; - - /// \brief Set the world pose of the sensor - /// \param[in] _pose Pose in world frame - public: void SetWorldPose(const math::Pose3d _pose); - - /// \brief Get the world pose of the sensor - /// \return Pose in world frame. - public: math::Pose3d WorldPose() const; - - /// \brief Set the magnetic field vector in world frame - /// \param[in] _field Magnetic field vector in world frame. - public: void SetWorldMagneticField(const math::Vector3d &_field); - - /// \brief Get the magnetic field vector in world frame - /// \return Magnetic field vector in world frame - public: math::Vector3d WorldMagneticField() const; - - /// \brief Get the magnetic field vector in body frame - /// \return Magnetic field vector in body frame - public: math::Vector3d MagneticField() const; - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif diff --git a/include/ignition/sensors/Manager.hh b/include/ignition/sensors/Manager.hh index 00967496..c38ba08d 100644 --- a/include/ignition/sensors/Manager.hh +++ b/include/ignition/sensors/Manager.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * 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. @@ -13,206 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_MANAGER_HH_ -#define IGNITION_SENSORS_MANAGER_HH_ + */ -#include -#include -#include -#include -#include -#include -#include -#include +#include #include -#include -#include - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // Forward declarations - class ManagerPrivate; - - /// \brief Loads and runs sensors - /// - /// This class is responsible for loading and running sensors, and - /// providing sensors with common environments to generat data from. - /// - /// The primary interface through which to load a sensor is LoadSensor(). - /// This takes an sdf element pointer that should be configured with - /// everything the sensor will need. Custom sensors configuration must - /// be in the tag of the sdf::Element. The manager will - /// dynamically load the sensor library and update it. - /// \remarks This class is not thread safe. - class IGNITION_SENSORS_VISIBLE Manager - { - /// \brief constructor - public: Manager(); - - /// \brief destructor - public: virtual ~Manager(); - - /// \brief Initialize the sensor library without rendering or physics. - /// \return True if successfully initialized, false if not - public: bool Init(); - - /// \brief Create a sensor from SDF with a known sensor type. - /// - /// This creates sensors by looking at the given sdf element. - /// Sensors created with this API offer an ignition-transport interface. - /// If you need a direct C++ interface to the data, you must get the - /// sensor pointer and cast to the correct type. - /// - /// A tag may have multiple tags. A SensorId will be - /// returned for each plugin that is described in SDF. - /// If there are no tags then one of the plugins shipped with - /// this library will be loaded. For example, a tag with - /// but no will load a CameraSensor from - /// ignition-sensors-camera. - /// \sa Sensor() - /// \param[in] _sdf pointer to the sdf element - /// \return A pointer to the created sensor. nullptr returned on - /// error. - public: template - T *CreateSensor(sdf::Sensor _sdf) - { - ignition::sensors::SensorId id = this->CreateSensor(_sdf); - - if (id != NO_SENSOR) - { - T *result = dynamic_cast(this->Sensor(id)); - - if (!result) - ignerr << "SDF sensor type does not match template type\n"; - - return result; - } - - ignerr << "Failed to create sensor of type[" - << _sdf.TypeStr() << "]\n"; - return nullptr; - } - - /// \brief Create a sensor from SDF with a known sensor type. - /// - /// This creates sensors by looking at the given sdf element. - /// Sensors created with this API offer an ignition-transport interface. - /// If you need a direct C++ interface to the data, you must get the - /// sensor pointer and cast to the correct type. - /// - /// A tag may have multiple tags. A SensorId will be - /// returned for each plugin that is described in SDF. - /// If there are no tags then one of the plugins shipped with - /// this library will be loaded. For example, a tag with - /// but no will load a CameraSensor from - /// ignition-sensors-camera. - /// \sa Sensor() - /// \param[in] _sdf pointer to the sdf element - /// \return A pointer to the created sensor. nullptr returned on - /// error. - public: template - T *CreateSensor(sdf::ElementPtr _sdf) - { - ignition::sensors::SensorId id = this->CreateSensor(_sdf); - - if (id != NO_SENSOR) - { - T *result = dynamic_cast(this->Sensor(id)); - - if (nullptr == result) - { - ignerr << "Failed to create sensor [" << id << "] of type [" - << _sdf->Get("type") - << "]. SDF sensor type does not match template type." - << std::endl; - } - - return result; - } - - ignerr << "Failed to create sensor of type [" - << _sdf->Get("type") << "]\n"; - return nullptr; - } - - /// \brief Create a sensor from SDF without a known sensor type. - /// - /// This creates sensors by looking at the given sdf element. - /// Sensors created with this API offer an ignition-transport interface. - /// If you need a direct C++ interface to the data, you must get the - /// sensor pointer and cast to the correct type. - /// - /// A tag may have multiple tags. A SensorId will be - /// returned for each plugin that is described in SDF. - /// If there are no tags then one of the plugins shipped with - /// this library will be loaded. For example, a tag with - /// but no will load a CameraSensor from - /// ignition-sensors-camera. - /// \sa Sensor() - /// \param[in] _sdf pointer to the sdf element - /// \return A sensor id that refers to the created sensor. NO_SENSOR - /// is returned on erro. - public: ignition::sensors::SensorId CreateSensor(sdf::ElementPtr _sdf); - - /// \brief Create a sensor from SDF without a known sensor type. - /// - /// This creates sensors by looking at the given sdf element. - /// Sensors created with this API offer an ignition-transport interface. - /// If you need a direct C++ interface to the data, you must get the - /// sensor pointer and cast to the correct type. - /// - /// A tag may have multiple tags. A SensorId will be - /// returned for each plugin that is described in SDF. - /// If there are no tags then one of the plugins shipped with - /// this library will be loaded. For example, a tag with - /// but no will load a CameraSensor from - /// ignition-sensors-camera. - /// \sa Sensor() - /// \param[in] _sdf SDF sensor DOM object - /// \return A sensor id that refers to the created sensor. NO_SENSOR - /// is returned on erro. - public: ignition::sensors::SensorId CreateSensor(const sdf::Sensor &_sdf); - - - /// \brief Get an instance of a loaded sensor by sensor id - /// \param[in] _id Idenitifier of the sensor. - /// \return Pointer to the sensor, nullptr on error. - public: ignition::sensors::Sensor *Sensor( - ignition::sensors::SensorId _id); - - /// \brief Remove a sensor by ID - /// \param[in] _sensorId ID of the sensor to remove - /// \return True if the sensor exists and removed. - public: bool Remove(const ignition::sensors::SensorId _id); - - /// \brief Run the sensor generation one step. - /// \param _time: The current simulated time - /// \param _force: If true, all sensors are forced to update. Otherwise - /// a sensor will update based on it's Hz rate. - public: void RunOnce(const ignition::common::Time &_time, - bool _force = false); - - /// \brief Adds colon delimited paths sensor plugins may be - public: void AddPluginPaths(const std::string &_path); - - /// \brief load a plugin and return a shared_ptr - /// \param[in] _filename Sensor plugin file to load. - /// \return Pointer to the new sensor, nullptr on error. - private: ignition::sensors::SensorId LoadSensorPlugin( - const std::string &_filename, sdf::ElementPtr _sdf); - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief private data pointer - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif diff --git a/include/ignition/sensors/Noise.hh b/include/ignition/sensors/Noise.hh index 405bfb79..8c445c6a 100644 --- a/include/ignition/sensors/Noise.hh +++ b/include/ignition/sensors/Noise.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -13,113 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ - -#ifndef IGNITION_SENSORS_NOISE_HH_ -#define IGNITION_SENSORS_NOISE_HH_ - -#include -#include -#include + */ +#include #include -#include -#include - -#include - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // Forward declarations - class NoisePrivate; - - /// \class NoiseFactory Noise.hh ignition/sensors/Noise.hh - /// \brief Use this noise manager for creating and loading noise models. - class IGNITION_SENSORS_VISIBLE NoiseFactory - { - /// \brief Load a noise model based on the input sdf parameters and - /// sensor type. - /// \param[in] _sdf Noise sdf parameters. - /// \param[in] _sensorType Type of sensor. This is currently used to - /// distinguish between image and non image sensors in order to create - /// the appropriate noise model. - /// \return Pointer to the noise model created. - public: static NoisePtr NewNoiseModel(sdf::ElementPtr _sdf, - const std::string &_sensorType = ""); - - /// \brief Load a noise model based on the input sdf parameters and - /// sensor type. - /// \param[in] _sdf Noise sdf parameters. - /// \param[in] _sensorType Type of sensor. This is currently used to - /// distinguish between image and non image sensors in order to create - /// the appropriate noise model. - /// \return Pointer to the noise model created. - public: static NoisePtr NewNoiseModel(const sdf::Noise &_sdf, - const std::string &_sensorType = ""); - }; - - /// \brief Which noise types we support - enum class IGNITION_SENSORS_VISIBLE NoiseType : int - { - NONE = 0, - CUSTOM = 1, - GAUSSIAN = 2 - }; - - /// \class Noise Noise.hh ignition/sensors/Noise.hh - /// \brief Noise models for sensor output signals. - class IGNITION_SENSORS_VISIBLE Noise - { - /// \brief Constructor. This should not be called directly unless creating - /// an empty noise model. Use NoiseFactory::NewNoiseModel to instantiate - /// a new noise model. - /// \param[in] _type Type of noise model. - /// \sa NoiseFactory::NewNoiseModel - public: explicit Noise(NoiseType _type); - - /// \brief Destructor. - public: virtual ~Noise(); - - /// \brief Load noise parameters from sdf. - /// \param[in] _sdf SDF Noise DOM object. - public: virtual void Load(const sdf::Noise &_sdf); - - /// \brief Apply noise to input data value. - /// \param[in] _in Input data value. - /// \param[in] _dt Input data time step. - /// \return Data with noise applied. - public: double Apply(double _in, double _dt = 0.0); - - /// \brief Apply noise to input data value. This gets overriden by - /// derived classes, and called by Apply. - /// \param[in] _in Input data value. - /// \param[in] _dt Input data time step. - /// \return Data with noise applied. - public: virtual double ApplyImpl(double _in, double _dt); - - /// \brief Accessor for NoiseType. - /// \return Type of noise currently in use. - public: NoiseType Type() const; - - /// \brief Register a custom noise callback. - /// \param[in] _cb Callback function for applying a custom noise model. - /// This is useful if users want to use their own noise model from a - /// sensor plugin. - public: virtual void SetCustomNoiseCallback( - std::function _cb); - - /// \brief Output information about the noise model. - /// \param[in] _out Output stream - public: virtual void Print(std::ostream &_out) const; - - /// \brief Private data pointer - private: NoisePrivate *dataPtr = nullptr; - }; - } - } -} -#endif diff --git a/include/ignition/sensors/RenderingEvents.hh b/include/ignition/sensors/RenderingEvents.hh index 7912d012..ec04546f 100644 --- a/include/ignition/sensors/RenderingEvents.hh +++ b/include/ignition/sensors/RenderingEvents.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -13,56 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ - -#ifndef IGNITION_SENSORS_RENDERINGEVENTS_HH_ -#define IGNITION_SENSORS_RENDERINGEVENTS_HH_ - -#include -#include - -// TODO(louise) Remove these pragmas once ign-rendering is disabling the -// warnings -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif + */ +#include #include -#include - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - class IGNITION_SENSORS_RENDERING_VISIBLE RenderingEvents - { - /// \brief Set a callback to be called when the scene is changed. - /// - /// \param[in] _callback This callback will be called every time the - /// scene is changed. - /// \remark Do not block inside of the callback. - /// \return A connection pointer that must remain in scope. When the - /// connection pointer falls out of scope, the connection is broken. - public: static ignition::common::ConnectionPtr ConnectSceneChangeCallback( - std::function - _callback); - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Event that is used to trigger callbacks when the scene - /// is changed - public: static ignition::common::EventT< - void(const ignition::rendering::ScenePtr &)> sceneEvent; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} -#endif diff --git a/include/ignition/sensors/RenderingSensor.hh b/include/ignition/sensors/RenderingSensor.hh index d39a0f26..bf04bd29 100644 --- a/include/ignition/sensors/RenderingSensor.hh +++ b/include/ignition/sensors/RenderingSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -13,87 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_RENDERINGSENSOR_HH_ -#define IGNITION_SENSORS_RENDERINGSENSOR_HH_ + */ -#include - -#include - -// TODO(louise) Remove these pragmas once ign-rendering is disabling the -// warnings -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif - -#include "ignition/sensors/rendering/Export.hh" -#include "ignition/sensors/Sensor.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - /// \brief forward declarations - class RenderingSensorPrivate; - - /// \brief a rendering sensor class - /// - /// This class is a base for all rendering sensor classes. It provides - /// interface to ignition rendering objects - class IGNITION_SENSORS_RENDERING_VISIBLE RenderingSensor - : public Sensor - { - /// \brief constructor - protected: RenderingSensor(); - - /// \brief destructor - public: virtual ~RenderingSensor(); - - /// \brief Set the rendering scene. - /// - /// \param[in] _scene Pointer to the scene - public: virtual void SetScene(rendering::ScenePtr _scene); - - /// \brief Get the rendering scene. - public: rendering::ScenePtr Scene() const; - - /// \brief Render update. This performs the actual render operation. - public: void Render(); - - /// \brief Set whether to update the scene graph manually. If set to true, - /// it is expected that rendering::Scene::PreRender is called manually - /// before calling Render() - /// \param[in] _manual True to enable manual scene graph update - public: void SetManualSceneUpdate(bool _manual); - - /// \brief Get whether the scene graph is updated manually. Defaults to - /// false. - /// \return True if manual scene graph update is enabled, false otherwise - /// \sa SetManualSceneUpdate - public: bool ManualSceneUpdate() const; - - /// \brief Add a rendering::Sensor. Its render updates will be handled - /// by this base class. - /// \param[in] _sensor Sensor to add. - protected: void AddSensor(rendering::SensorPtr _sensor); - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \internal - /// \brief Data pointer for private data - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/RgbdCameraSensor.hh b/include/ignition/sensors/RgbdCameraSensor.hh index 33c7b7fc..df4ad7be 100644 --- a/include/ignition/sensors/RgbdCameraSensor.hh +++ b/include/ignition/sensors/RgbdCameraSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -13,90 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_RGBDCAMERASENSOR_HH_ -#define IGNITION_SENSORS_RGBDCAMERASENSOR_HH_ + */ -#include - -#include - -#include -#include - -#include "ignition/sensors/CameraSensor.hh" -#include "ignition/sensors/config.hh" -#include "ignition/sensors/rgbd_camera/Export.hh" -#include "ignition/sensors/Export.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // forward declarations - class RgbdCameraSensorPrivate; - - /// \brief RGBD camera sensor class. - /// - /// This class creates a few types of sensor data from an ignition - /// rendering scene: - /// * RGB image (same as CameraSensor) - /// * Depth image (same as DepthCamera) - /// * (future / todo) Color point cloud - /// The scene must be created in advance and given to Manager::Init(). - /// It offers both an ignition-transport interface and a direct C++ API - /// to access the image data. The API works by setting a callback to be - /// called with image data. - class IGNITION_SENSORS_RGBD_CAMERA_VISIBLE RgbdCameraSensor - : public CameraSensor - { - /// \brief constructor - public: RgbdCameraSensor(); - - /// \brief destructor - public: virtual ~RgbdCameraSensor(); - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Force the sensor to generate data - /// \param[in] _now The current time - /// \return true if the update was successful - public: virtual bool Update(const common::Time &_now) override; - - /// \brief Set the rendering scene. - /// \param[in] _scene Pointer to the scene - public: virtual void SetScene( - ignition::rendering::ScenePtr _scene) override; - - /// \brief Get image width. - /// \return width of the image - public: virtual unsigned int ImageWidth() const override; - - /// \brief Get image height. - /// \return height of the image - public: virtual unsigned int ImageHeight() const override; - - /// \brief Create an RGB camera and a depth camera. - /// \return True on success. - private: bool CreateCameras(); - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/Sensor.hh b/include/ignition/sensors/Sensor.hh index c4d4be70..888809eb 100644 --- a/include/ignition/sensors/Sensor.hh +++ b/include/ignition/sensors/Sensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017 Open Source Robotics Foundation + * 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. @@ -13,204 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_SENSOR_HH_ -#define IGNITION_SENSORS_SENSOR_HH_ + */ -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif - -#include -#include -#include - -#include -#include -#include +#include #include -#include -#include - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - /// \brief A string used to identify a sensor - using SensorId = std::size_t; - const SensorId NO_SENSOR = 0; - - /// \brief forward declarations - class SensorPrivate; - - /// \brief a base sensor class - /// - /// This class is a base for all sensor classes. It parses some common - /// SDF elements in the tag and is responsible for making sure - /// sensors update at the right time. - class IGNITION_SENSORS_VISIBLE Sensor - { - /// \brief constructor - protected: Sensor(); - - /// \brief destructor - public: virtual ~Sensor(); - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF or inside of - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf); - - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF or inside of - /// \return true if loading was successful - public: virtual bool Load(sdf::ElementPtr _sdf); - - /// \brief Initialize values in the sensor - public: virtual bool Init(); - - /// \brief Force the sensor to generate data - /// - /// This method must be overridden by sensors. Subclasses should not - /// not make a decision about whether or not they need to update. The - /// Sensor class will make sure Update() is called at the correct time. - /// - /// If a subclass wants to have a variable update rate it should call - /// SetUpdateRate(). - /// - /// A subclass should return false if there was an error while updating - /// \param[in] _now The current time - /// \return true if the update was successfull - /// \sa SetUpdateRate() - public: virtual bool Update(const common::Time &_now) = 0; - - /// \brief Return the next time the sensor will generate data - public: common::Time NextUpdateTime() const; - - /// \brief Update the sensor. - /// - /// This is called by the manager, and is responsible for determining - /// if this sensor needs to generate data at this time. If so, the - /// subclasses' Update() method will be called. - /// \param[in] _now The current time - /// \param[in] _force Force the update to happen even if it's not time - /// \return True if the update was triggered (_force was true or _now - /// >= next_update_time) and the sensor's - /// bool Sensor::Update(const common::Time &_now) function returned true. - /// False otherwise. - /// \remarks If forced the NextUpdateTime() will be unchanged. - /// \sa virtual bool Update(const common::Time &_name) = 0 - public: bool Update(const common::Time &_now, const bool _force); - - /// \brief Get the update rate of the sensor. - /// - /// The update rate is the number of times per second a sensor should - /// generate and output data. - /// \return _hz update rate of sensor. - public: double UpdateRate() const; - - /// \brief Set the update rate of the sensor. An update rate of zero means - /// that the sensor is updated every cycle. It's zero by default. - /// \detail Negative rates become zero. - /// \param[in] _hz Update rate of sensor in Hertz. - public: void SetUpdateRate(const double _hz); - - /// \brief Get the current pose. - /// \return Current pose of the sensor. - public: ignition::math::Pose3d Pose() const; - - /// \brief Update the pose of the sensor - public: void SetPose(const ignition::math::Pose3d &_pose); - - /// \brief Set the parent of the sensor - public: virtual void SetParent(const std::string &_parent); - - /// \brief Get name. - /// \return Name of sensor. - public: std::string Name() const; - - /// \brief FrameId. - /// \return FrameId of sensor. - public: std::string FrameId() const; - - /// \brief Set Frame ID of the sensor - /// \param[in] _frameId Frame ID of the sensor - public: void SetFrameId(const std::string &_frameId); - - /// \brief Get topic where sensor data is published. - /// \return Topic sensor publishes data to - public: std::string Topic() const; - - /// \brief Set topic where sensor data is published. - /// \param[in] _topic Topic sensor publishes data to. - /// \return True if a valid topic was set. - public: bool SetTopic(const std::string &_topic); - - /// \brief Get flag state for enabling performance metrics publication. - /// \return True if performance metrics are enabled, false otherwise. - public: bool EnableMetrics() const; - - /// \brief Set flag to enable publishing performance metrics - /// \param[in] _enableMetrics True to enable. - public: void SetEnableMetrics(bool _enableMetrics); - - /// \brief Get parent link of the sensor. - /// \return Parent link of sensor. - public: std::string Parent() const; - - /// \brief Get the sensor's ID. - /// \return The sensor's ID. - public: SensorId Id() const; - - /// \brief Get the SDF used to load this sensor. - /// \return Pointer to an SDF element that contains initialization - /// information for this sensor. - public: sdf::ElementPtr SDF() const; - - /// \brief Add a sequence number to an ignition::msgs::Header. This - /// function can be called by a sensor that wants to add a sequence - /// number to a sensor message in order to have improved - /// accountability for generated sensor data. - /// - /// This function will add the following key-value pair to the `data` - /// field in the provided ignition::msgs::Header msg. - /// - /// * key: "seq" - /// * value: `sequence_number` - /// - /// If the "seq" key already exists, then the value will be set - /// without adding another key-value pair. - /// - /// The `sequence_number` starts at zero, when a sensor is created, - /// and is incremented by one each time this function is called. - /// \param[in,out] _msg The header which will receive the sequence. - /// \param[in] _seqKey Name of the sequence to use. - public: void AddSequence(ignition::msgs::Header *_msg, - const std::string &_seqKey = "default"); - - /// \brief Publishes information about the performance of the sensor. - /// This method is called by Update(). - /// \param[in] _now Current time. - public: void PublishMetrics( - const std::chrono::duration &_now); - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \internal - /// \brief Data pointer for private data - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif diff --git a/include/ignition/sensors/SensorFactory.hh b/include/ignition/sensors/SensorFactory.hh index 32004a21..4f048a96 100644 --- a/include/ignition/sensors/SensorFactory.hh +++ b/include/ignition/sensors/SensorFactory.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -13,183 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_SENSORFACTORY_HH_ -#define IGNITION_SENSORS_SENSORFACTORY_HH_ - -#include -#include -#include -#include - -#include -#include -#include + */ +#include #include -#include - -#include "ignition/sensors/Sensor.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // forward declaration - class SensorFactoryPrivate; - - /// \brief Base sensor plugin interface - class IGNITION_SENSORS_VISIBLE SensorPlugin - { - /// \brief Allows using shorter APIS in common::PluginLoader - public: IGN_COMMON_SPECIALIZE_INTERFACE(ignition::sensors::SensorPlugin) - - /// \brief Instantiate new sensor - /// \return New sensor - public: virtual Sensor *New() = 0; - }; - - /// \brief Templated class for instantiating sensors of the specified type - /// \tparam Type of sensor being instantiated. - template - class SensorTypePlugin : public SensorPlugin - { - // Documentation inherited - public: SensorType *New() override - { - return new SensorType(); - }; - }; - - /// \brief A factory class for creating sensors - /// This class wll load a sensor plugin based on the given sensor type and - /// instantiates a sensor object - /// - class IGNITION_SENSORS_VISIBLE SensorFactory - { - /// \brief Constructor - public: SensorFactory(); - - /// \brief Destructor - public: ~SensorFactory(); - - /// \brief Create a sensor from a SDF DOM object with a known sensor type. - /// - /// This creates sensors by looking at the given SDF DOM object. - /// Sensors created with this API offer an ignition-transport interface. - /// If you need a direct C++ interface to the data, you must get the - /// sensor pointer and cast to the correct type. - /// - /// \sa Sensor() - /// \param[in] _sdf SDF Sensor DOM object. - /// \return A pointer to the created sensor. nullptr returned on - /// error. - public: template - std::unique_ptr CreateSensor(const sdf::Sensor &_sdf) - { - auto sensor = SensorFactory::CreateSensor(_sdf); - - if (sensor) - { - std::unique_ptr result( - dynamic_cast(sensor.release())); - - if (!result) - ignerr << "SDF sensor type does not match template type\n"; - - return result; - } - - ignerr << "Failed to create sensor of type[" - << _sdf.TypeStr() << "]\n"; - return nullptr; - } - - /// \brief Create a sensor from SDF with a known sensor type. - /// - /// This creates sensors by looking at the given sdf element. - /// Sensors created with this API offer an ignition-transport interface. - /// If you need a direct C++ interface to the data, you must get the - /// sensor pointer and cast to the correct type. - /// - /// \sa Sensor() - /// \param[in] _sdf pointer to the sdf element - /// \return A pointer to the created sensor. nullptr returned on - /// error. - public: template - std::unique_ptr CreateSensor(sdf::ElementPtr _sdf) - { - auto sensor = SensorFactory::CreateSensor(_sdf); - - if (sensor) - { - std::unique_ptr result( - dynamic_cast(sensor.release())); - - if (!result) - ignerr << "SDF sensor type does not match template type\n"; - - return result; - } - - ignerr << "Failed to create sensor of type[" - << _sdf->Get("type") << "]\n"; - return nullptr; - } - - /// \brief Create a sensor from SDF without a known sensor type. - /// - /// This creates sensors by looking at the given sdf element. - /// Sensors created with this API offer an ignition-transport interface. - /// If you need a direct C++ interface to the data, you must get the - /// sensor pointer and cast to the correct type. - /// - /// \sa Sensor() - /// \param[in] _sdf pointer to the sdf element - /// \return A sensor id that refers to the created sensor. Null is - /// is returned on error. - public: std::unique_ptr CreateSensor(sdf::ElementPtr _sdf); - - /// \brief Create a sensor from an SDF Sensor DOM object without a known - /// sensor type. - /// - /// This creates sensors by looking at the given SDF Sensor DOM - /// object. - /// Sensors created with this API offer an ignition-transport interface. - /// If you need a direct C++ interface to the data, you must get the - /// sensor pointer and cast to the correct type. - /// - /// \sa Sensor() - /// \param[in] _sdf SDF Sensor DOM object. - /// \return A sensor id that refers to the created sensor. Null is - /// is returned on error. - public: std::unique_ptr CreateSensor(const sdf::Sensor &_sdf); - - /// \brief Add additional path to search for sensor plugins - /// \param[in] _path Search path - public: void AddPluginPaths(const std::string &_path); - - /// \brief load a plugin and return a pointer - /// \param[in] _filename Sensor plugin file to load. - /// \return Pointer to the new sensor, nullptr on error. - private: std::shared_ptr LoadSensorPlugin( - const std::string &_filename); - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief private data pointer - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - - /// \brief Sensor registration macro - #define IGN_SENSORS_REGISTER_SENSOR(classname) \ - IGN_COMMON_REGISTER_SINGLE_PLUGIN(\ - ignition::sensors::SensorTypePlugin, \ - ignition::sensors::SensorPlugin) - } - } -} - -#endif diff --git a/include/ignition/sensors/SensorTypes.hh b/include/ignition/sensors/SensorTypes.hh index 479e671b..43953a29 100644 --- a/include/ignition/sensors/SensorTypes.hh +++ b/include/ignition/sensors/SensorTypes.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. @@ -13,169 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_SENSORTYPES_HH_ -#define IGNITION_SENSORS_SENSORTYPES_HH_ + */ -#include -#include - -#include +#include #include -#include - -/// \file -/// \ingroup ignition_sensors -/// \brief Forward declarations and typedefs for sensors -namespace ignition -{ - /// \ingroup ignition_sensors - /// \brief Sensors namespace - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // Forward declarations. - class AltimeterSensor; - class CameraSensor; - class GpuLidarSensor; - class GaussianNoiseModel; - class ImageGaussianNoiseModel; - class Noise; - class Sensor; - - /// \def SensorPtr - /// \brief Shared pointer to Sensor - typedef std::shared_ptr SensorPtr; - - /// \def CameraSensorPtr - /// \brief Shared pointer to CameraSensor - typedef std::shared_ptr CameraSensorPtr; - - /// \def GpuLidarSensorPtr - /// \brief Shared pointer to GpuLidarSensor - typedef std::shared_ptr GpuLidarSensorPtr; - - /// \def NoisePtr - /// \brief Shared pointer to Noise - typedef std::shared_ptr NoisePtr; - - /// \def GaussianNoisePtr - /// \brief Shared pointer to Noise - typedef std::shared_ptr GaussianNoiseModelPtr; - - /// \brief Shared pointer to Noise - typedef std::shared_ptr - ImageGaussianNoiseModelPtr; - - /// \def Sensor_V - /// \brief Vector of Sensor shared pointers - typedef std::vector Sensor_V; - - /// \def CameraSensor_V - /// \brief Vector of CameraSensor shared pointers - typedef std::vector CameraSensor_V; - - /// \def GpuLidarSensor_V - /// \brief Vector of GpuLidarSensor shared pointers - typedef std::vector GpuLidarSensor_V; - - /// \def SensorNoiseType - /// \brief Eumeration of all sensor noise types - enum SensorNoiseType - { - /// \internal - /// \brief Indicator used to create an iterator over the enum. Do not - /// use this. - SENSOR_NOISE_TYPE_BEGIN = 0, - - /// \brief Noise streams for the Camera sensor - /// \sa CameraSensor - NO_NOISE = SENSOR_NOISE_TYPE_BEGIN, - - /// \brief Noise streams for the Camera sensor - /// \sa CameraSensor - CAMERA_NOISE = 1, - - /// \brief Magnetometer body-frame X axis noise in Tesla - /// \sa MagnetometerSensor - MAGNETOMETER_X_NOISE_TESLA = 2, - - /// \brief Magnetometer body-frame Y axis noise in Tesla - /// \sa MagnetometerSensor - MAGNETOMETER_Y_NOISE_TESLA = 3, - - /// \brief Magnetometer body-frame Z axis noise in Tesla - /// \sa MagnetometerSensor - MAGNETOMETER_Z_NOISE_TESLA = 4, - - /// \brief Vertical noise stream for the altimeter sensor - /// \sa AltimeterSensor - ALTIMETER_VERTICAL_POSITION_NOISE_METERS = 5, - - /// \brief Velocity noise streams for the altimeter sensor - /// \sa AltimeterSensor - ALTIMETER_VERTICAL_VELOCITY_NOISE_METERS_PER_S = 6, - - /// \brief Air Pressure noise streams for the air pressure sensor - /// \sa AirPressureSensor - AIR_PRESSURE_NOISE_PASCALS = 7, - - /// \brief Accelerometer body-frame X axis noise in m/s^2 - /// \sa ImuSensor - ACCELEROMETER_X_NOISE_M_S_S = 8, - - /// \brief Accelerometer body-frame Y axis noise in m/s^2 - /// \sa ImuSensor - ACCELEROMETER_Y_NOISE_M_S_S = 9, - - /// \brief Accelerometer body-frame Z axis noise in m/s^2 - /// \sa ImuSensor - ACCELEROMETER_Z_NOISE_M_S_S = 10, - - /// \brief Gyroscope body-frame X axis noise in m/s^2 - /// \sa ImuSensor - GYROSCOPE_X_NOISE_RAD_S = 11, - - /// \brief Gyroscope body-frame X axis noise in m/s^2 - /// \sa ImuSensor - GYROSCOPE_Y_NOISE_RAD_S = 12, - - /// \brief Gyroscope body-frame X axis noise in m/s^2 - /// \sa ImuSensor - GYROSCOPE_Z_NOISE_RAD_S = 13, - - /// \brief Noise streams for the Lidar sensor - /// \sa Lidar - LIDAR_NOISE = 14, - - /// \internal - /// \brief Indicator used to create an iterator over the enum. Do not - /// use this. - SENSOR_NOISE_TYPE_END - }; - /// \} - - /// \brief SensorCategory is used to categorize sensors. This is used to - /// put sensors into different threads. - enum SensorCategory - { - // IMAGE must be the first element, and it must start with 0. Do not - // change this! See SensorManager::sensorContainers for reference. - /// \brief Image based sensor class. This type requires the rendering - /// engine. - IMAGE = 0, - - /// \brief Ray based sensor class. - RAY = 1, - - /// \brief A type of sensor is not a RAY or IMAGE sensor. - OTHER = 2, - - /// \brief Number of Sensor Categories - CATEGORY_COUNT = 3 - }; - } - } -} -#endif diff --git a/include/ignition/sensors/ThermalCameraSensor.hh b/include/ignition/sensors/ThermalCameraSensor.hh index 1164b122..c3132e57 100644 --- a/include/ignition/sensors/ThermalCameraSensor.hh +++ b/include/ignition/sensors/ThermalCameraSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -13,173 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_THERMALCAMERASENSOR_HH_ -#define IGNITION_SENSORS_THERMALCAMERASENSOR_HH_ + */ -#include -#include -#include - -#include - -#include -#include -#include -#include - -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4005) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif - -// TODO(louise) Remove these pragmas once ign-rendering is disabling the -// warnings -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif - -#include "ignition/sensors/thermal_camera/Export.hh" -#include "ignition/sensors/CameraSensor.hh" -#include "ignition/sensors/Export.hh" -#include "ignition/sensors/Sensor.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // forward declarations - class ThermalCameraSensorPrivate; - - /// \brief Thermal camera sensor class. - /// - /// This class creates thermal image from an ignition rendering scene. - /// The scene must be created in advance and given to Manager::Init(). - /// It offers both an ignition-transport interface and a direct C++ API - /// to access the image data. The API works by setting a callback to be - /// called with image data. - class IGNITION_SENSORS_THERMAL_CAMERA_VISIBLE ThermalCameraSensor - : public CameraSensor - { - /// \brief constructor - public: ThermalCameraSensor(); - - /// \brief destructor - public: virtual ~ThermalCameraSensor(); - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(sdf::ElementPtr _sdf) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Force the sensor to generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; - - /// \brief Force the sensor to generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual rendering::ThermalCameraPtr ThermalCamera(); - - /// \brief Thermal data callback used to get the data from the sensor - /// \param[in] _scan pointer to the data from the sensor - /// \param[in] _width width of the thermal image - /// \param[in] _height height of the thermal image - /// \param[in] _channel bytes used for the thermal data - /// \param[in] _format string with the format - public: void OnNewThermalFrame(const uint16_t *_scan, - unsigned int _width, unsigned int _height, - unsigned int _channels, - const std::string &_format); - - /// \brief Set a callback to be called when image frame data is - /// generated. - /// \param[in] _callback This callback will be called every time the - /// camera produces image data. The Update function will be blocked - /// while the callbacks are executed. - /// \remark Do not block inside of the callback. - /// \return A connection pointer that must remain in scope. When the - /// connection pointer falls out of scope, the connection is broken. - public: common::ConnectionPtr ConnectImageCallback( - std::function _callback); - - /// \brief Set the rendering scene. - /// \param[in] _scene Pointer to the scene - public: virtual void SetScene( - ignition::rendering::ScenePtr _scene) override; - - /// \brief Get image width. - /// \return width of the image - public: virtual unsigned int ImageWidth() const override; - - /// \brief Get image height. - /// \return height of the image - public: virtual unsigned int ImageHeight() const override; - - /// \brief Set the ambient temperature of the environment - /// \param[in] _ambient Ambient temperature in kelvin - public: virtual void SetAmbientTemperature(float _ambient); - - /// \brief Set the range of ambient temperature - /// \param[in] _range The ambient temperature ranges from - /// (ambient - range/2) to (ambient + range/2). - public: virtual void SetAmbientTemperatureRange(float _range); - - /// \brief Set the minimum temperature the sensor can detect - /// \param[in] _min Min temperature in kelvin - public: virtual void SetMinTemperature(float _min); - - /// \brief Set the maximum temperature the sensor can detect - /// \param[in] _max Max temperature in kelvin - public: virtual void SetMaxTemperature(float _max); - - /// \brief Set the temperature linear resolution. The thermal image data - /// returned will be temperature in kelvin / resolution. - /// Typical values are 0.01 (10mK), 0.1 (100mK), or 0.04 to simulate - /// 14 bit format. - /// \param[in] resolution Temperature linear resolution - public: virtual void SetLinearResolution(float _resolution); - - /// \brief Create a camera in a scene - /// \return True on success. - private: bool CreateCamera(); - - /// \brief Callback that is triggered when the scene changes on - /// the Manager. - /// \param[in] _scene Pointer to the new scene. - private: void OnSceneChange(ignition::rendering::ScenePtr /*_scene*/) - { } - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/air_pressure/Export.hh b/include/ignition/sensors/air_pressure/Export.hh new file mode 100644 index 00000000..e42e8a24 --- /dev/null +++ b/include/ignition/sensors/air_pressure/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/altimeter/Export.hh b/include/ignition/sensors/altimeter/Export.hh new file mode 100644 index 00000000..4eb4cdb9 --- /dev/null +++ b/include/ignition/sensors/altimeter/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/camera/Export.hh b/include/ignition/sensors/camera/Export.hh new file mode 100644 index 00000000..9daecc36 --- /dev/null +++ b/include/ignition/sensors/camera/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/config.hh b/include/ignition/sensors/config.hh new file mode 100644 index 00000000..2ac07896 --- /dev/null +++ b/include/ignition/sensors/config.hh @@ -0,0 +1,42 @@ +/* + * 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_SENSORS__CONFIG_HH_ +#define IGNITION_SENSORS__CONFIG_HH_ + +#include + +/* Version number */ +// #define IGNITION_SENSORS_MAJOR_VERSION GZ_SENSORS_MAJOR_VERSION +// #define IGNITION_SENSORS_MINOR_VERSION GZ_SENSORS_MINOR_VERSION +// #define IGNITION_SENSORS_PATCH_VERSION GZ_SENSORS_PATCH_VERSION + +// #define IGNITION_SENSORS_VERSION GZ_SENSORS_VERSION +// #define IGNITION_SENSORS_VERSION_FULL GZ_SENSORS_VERSION_FULL + +// #define IGNITION_SENSORS_VERSION_NAMESPACE GZ_SENSORS_VERSION_NAMESPACE + +// #define IGNITION_SENSORS_VERSION_HEADER GZ_SENSORS_VERSION_HEADER + +/* #undef BUILD_TYPE_PROFILE */ +/* #undef BUILD_TYPE_DEBUG */ +/* #undef BUILD_TYPE_RELEASE */ + +// #define IGN_SENSORS_PLUGIN_PATH GZ_SENSORS_PLUGIN_PATH +// #define IGN_SENSORS_PLUGIN_NAME(name) GZ_SENSORS_PLUGIN_NAME(name) + +#endif diff --git a/include/ignition/sensors/depth_camera/Export.hh b/include/ignition/sensors/depth_camera/Export.hh new file mode 100644 index 00000000..65f7b883 --- /dev/null +++ b/include/ignition/sensors/depth_camera/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/gpu_lidar/Export.hh b/include/ignition/sensors/gpu_lidar/Export.hh new file mode 100644 index 00000000..68d622d5 --- /dev/null +++ b/include/ignition/sensors/gpu_lidar/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/imu/Export.hh b/include/ignition/sensors/imu/Export.hh new file mode 100644 index 00000000..8b72f096 --- /dev/null +++ b/include/ignition/sensors/imu/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/lidar/Export.hh b/include/ignition/sensors/lidar/Export.hh new file mode 100644 index 00000000..ab3b0160 --- /dev/null +++ b/include/ignition/sensors/lidar/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/logical_camera/Export.hh b/include/ignition/sensors/logical_camera/Export.hh new file mode 100644 index 00000000..16b13bf0 --- /dev/null +++ b/include/ignition/sensors/logical_camera/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/magnetometer/Export.hh b/include/ignition/sensors/magnetometer/Export.hh new file mode 100644 index 00000000..83d0397b --- /dev/null +++ b/include/ignition/sensors/magnetometer/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/rendering/Export.hh b/include/ignition/sensors/rendering/Export.hh new file mode 100644 index 00000000..c59400d9 --- /dev/null +++ b/include/ignition/sensors/rendering/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/rgbd_camera/Export.hh b/include/ignition/sensors/rgbd_camera/Export.hh new file mode 100644 index 00000000..76ab66e3 --- /dev/null +++ b/include/ignition/sensors/rgbd_camera/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/thermal_camera/Export.hh b/include/ignition/sensors/thermal_camera/Export.hh new file mode 100644 index 00000000..dd37a865 --- /dev/null +++ b/include/ignition/sensors/thermal_camera/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc index dc020688..07fadc1c 100644 --- a/src/AirPressureSensor.cc +++ b/src/AirPressureSensor.cc @@ -20,21 +20,21 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif -#include -#include +#include +#include -#include "ignition/sensors/GaussianNoiseModel.hh" -#include "ignition/sensors/Noise.hh" -#include "ignition/sensors/SensorTypes.hh" -#include "ignition/sensors/SensorFactory.hh" -#include "ignition/sensors/AirPressureSensor.hh" +#include "gz/sensors/GaussianNoiseModel.hh" +#include "gz/sensors/Noise.hh" +#include "gz/sensors/SensorTypes.hh" +#include "gz/sensors/SensorFactory.hh" +#include "gz/sensors/AirPressureSensor.hh" -using namespace ignition; +using namespace gz; using namespace sensors; // Constants. These constants from from RotorS: @@ -51,7 +51,7 @@ static constexpr double kAirConstantDimensionless = kGravityMagnitude * (kGasConstantNmPerKmolKelvin * -kTempLapseKelvinPerMeter); /// \brief Private data for AirPressureSensor -class ignition::sensors::AirPressureSensorPrivate +class gz::sensors::AirPressureSensorPrivate { /// \brief node to create publisher public: transport::Node node; diff --git a/src/AltimeterSensor.cc b/src/AltimeterSensor.cc index f6a4edc9..0018c852 100644 --- a/src/AltimeterSensor.cc +++ b/src/AltimeterSensor.cc @@ -20,24 +20,24 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif -#include -#include +#include +#include -#include "ignition/sensors/AltimeterSensor.hh" -#include "ignition/sensors/Noise.hh" -#include "ignition/sensors/SensorFactory.hh" -#include "ignition/sensors/SensorTypes.hh" +#include "gz/sensors/AltimeterSensor.hh" +#include "gz/sensors/Noise.hh" +#include "gz/sensors/SensorFactory.hh" +#include "gz/sensors/SensorTypes.hh" -using namespace ignition; +using namespace gz; using namespace sensors; /// \brief Private data for AltimeterSensor -class ignition::sensors::AltimeterSensorPrivate +class gz::sensors::AltimeterSensorPrivate { /// \brief node to create publisher public: transport::Node node; diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 5429ab01..e340c74d 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -19,35 +19,35 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif #include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ignition/sensors/CameraSensor.hh" -#include "ignition/sensors/ImageGaussianNoiseModel.hh" -#include "ignition/sensors/ImageNoise.hh" -#include "ignition/sensors/Manager.hh" -#include "ignition/sensors/RenderingEvents.hh" -#include "ignition/sensors/SensorFactory.hh" -#include "ignition/sensors/SensorTypes.hh" - -using namespace ignition; +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gz/sensors/CameraSensor.hh" +#include "gz/sensors/ImageGaussianNoiseModel.hh" +#include "gz/sensors/ImageNoise.hh" +#include "gz/sensors/Manager.hh" +#include "gz/sensors/RenderingEvents.hh" +#include "gz/sensors/SensorFactory.hh" +#include "gz/sensors/SensorTypes.hh" + +using namespace gz; using namespace sensors; /// \brief Private data for CameraSensor -class ignition::sensors::CameraSensorPrivate +class gz::sensors::CameraSensorPrivate { /// \brief Save an image /// \param[in] _data the image data to be saved diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index b148def7..3c077019 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -17,9 +17,9 @@ #include #include -#include -#include -#include +#include +#include +#include sdf::ElementPtr cameraToBadSdf() { @@ -137,21 +137,21 @@ class Camera_TEST : public ::testing::Test // Documentation inherited protected: void SetUp() override { - ignition::common::Console::SetVerbosity(4); + gz::common::Console::SetVerbosity(4); } }; ////////////////////////////////////////////////// TEST(Camera_TEST, CreateCamera) { - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; sdf::ElementPtr camSdf = CameraToSdf("camera", "my_camera", 60.0, "/cam", true, true); // Create a CameraSensor - ignition::sensors::CameraSensor *cam = - mgr.CreateSensor(camSdf); + gz::sensors::CameraSensor *cam = + mgr.CreateSensor(camSdf); // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, cam); @@ -162,7 +162,7 @@ TEST(Camera_TEST, CreateCamera) // however camera is not loaded because a rendering scene is missing so // updates will not be successful and image size will be 0 - EXPECT_FALSE(cam->Update(ignition::common::Time())); + EXPECT_FALSE(cam->Update(gz::common::Time())); EXPECT_EQ(0u, cam->ImageWidth()); EXPECT_EQ(0u, cam->ImageHeight()); @@ -170,8 +170,8 @@ TEST(Camera_TEST, CreateCamera) sdf::ElementPtr camBadSdf = cameraToBadSdf(); // Create a CameraSensor - ignition::sensors::CameraSensor *badCam = - mgr.CreateSensor(camBadSdf); + gz::sensors::CameraSensor *badCam = + mgr.CreateSensor(camBadSdf); EXPECT_TRUE(badCam == nullptr); } @@ -185,7 +185,7 @@ TEST(Camera_TEST, Topic) const bool visualize = 1; // Factory - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; // Default topic { @@ -194,12 +194,12 @@ TEST(Camera_TEST, Topic) visualize); auto sensorId = mgr.CreateSensor(cameraSdf); - EXPECT_NE(ignition::sensors::NO_SENSOR, sensorId); + EXPECT_NE(gz::sensors::NO_SENSOR, sensorId); auto sensor = mgr.Sensor(sensorId); EXPECT_NE(nullptr, sensor); - auto camera = dynamic_cast(sensor); + auto camera = dynamic_cast(sensor); ASSERT_NE(nullptr, camera); EXPECT_EQ("/camera", camera->Topic()); @@ -212,12 +212,12 @@ TEST(Camera_TEST, Topic) visualize); auto sensorId = mgr.CreateSensor(cameraSdf); - EXPECT_NE(ignition::sensors::NO_SENSOR, sensorId); + EXPECT_NE(gz::sensors::NO_SENSOR, sensorId); auto sensor = mgr.Sensor(sensorId); EXPECT_NE(nullptr, sensor); - auto camera = dynamic_cast(sensor); + auto camera = dynamic_cast(sensor); ASSERT_NE(nullptr, camera); EXPECT_EQ("/topic_with_spaces/characters", camera->Topic()); @@ -230,7 +230,7 @@ TEST(Camera_TEST, Topic) visualize); auto sensorId = mgr.CreateSensor(cameraSdf); - EXPECT_EQ(ignition::sensors::NO_SENSOR, sensorId); + EXPECT_EQ(gz::sensors::NO_SENSOR, sensorId); } } diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 64b97ca2..9f3edbce 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -19,29 +19,29 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif #include -#include -#include -#include -#include +#include +#include +#include +#include -#include -#include +#include +#include -#include +#include -#include "ignition/sensors/DepthCameraSensor.hh" -#include "ignition/sensors/Manager.hh" -#include "ignition/sensors/SensorFactory.hh" -#include "ignition/sensors/ImageGaussianNoiseModel.hh" -#include "ignition/sensors/ImageNoise.hh" -#include "ignition/sensors/RenderingEvents.hh" +#include "gz/sensors/DepthCameraSensor.hh" +#include "gz/sensors/Manager.hh" +#include "gz/sensors/SensorFactory.hh" +#include "gz/sensors/ImageGaussianNoiseModel.hh" +#include "gz/sensors/ImageNoise.hh" +#include "gz/sensors/RenderingEvents.hh" #include "PointCloudUtil.hh" @@ -52,7 +52,7 @@ #endif /// \brief Private data for DepthCameraSensor -class ignition::sensors::DepthCameraSensorPrivate +class gz::sensors::DepthCameraSensorPrivate { /// \brief Save an image /// \param[in] _data the image data to be saved @@ -64,7 +64,7 @@ class ignition::sensors::DepthCameraSensorPrivate /// of the path was not possible. /// \sa ImageSaver public: bool SaveImage(const float *_data, unsigned int _width, - unsigned int _height, ignition::common::Image::PixelFormatType _format); + unsigned int _height, gz::common::Image::PixelFormatType _format); /// \brief Helper function to convert depth data to depth image /// \param[in] _data depth data @@ -84,7 +84,7 @@ class ignition::sensors::DepthCameraSensorPrivate public: bool initialized = false; /// \brief Rendering camera - public: ignition::rendering::DepthCameraPtr depthCamera; + public: gz::rendering::DepthCameraPtr depthCamera; /// \brief Depth data buffer. public: float *depthBuffer = nullptr; @@ -99,24 +99,24 @@ class ignition::sensors::DepthCameraSensorPrivate public: float near = 0.0; /// \brief Pointer to an image to be published - public: ignition::rendering::Image image; + public: gz::rendering::Image image; /// \brief Noise added to sensor data public: std::map noises; /// \brief Event that is used to trigger callbacks when a new image /// is generated - public: ignition::common::EventT< - void(const ignition::msgs::Image &)> imageEvent; + public: gz::common::EventT< + void(const gz::msgs::Image &)> imageEvent; /// \brief Connection from depth camera with new depth data - public: ignition::common::ConnectionPtr depthConnection; + public: gz::common::ConnectionPtr depthConnection; /// \brief Connection from depth camera with new point cloud data - public: ignition::common::ConnectionPtr pointCloudConnection; + public: gz::common::ConnectionPtr pointCloudConnection; /// \brief Connection to the Manager's scene change event. - public: ignition::common::ConnectionPtr sceneChangeConnection; + public: gz::common::ConnectionPtr sceneChangeConnection; /// \brief Just a mutex for thread safety public: std::mutex mutex; @@ -147,7 +147,7 @@ class ignition::sensors::DepthCameraSensorPrivate public: transport::Node::Publisher pointPub; }; -using namespace ignition; +using namespace gz; using namespace sensors; ////////////////////////////////////////////////// diff --git a/src/GaussianNoiseModel.cc b/src/GaussianNoiseModel.cc index f3343022..0791aa9e 100644 --- a/src/GaussianNoiseModel.cc +++ b/src/GaussianNoiseModel.cc @@ -20,16 +20,16 @@ #include #endif -#include "ignition/sensors/GaussianNoiseModel.hh" -#include -#include +#include "gz/sensors/GaussianNoiseModel.hh" +#include +#include -#include "ignition/common/Console.hh" +#include "gz/common/Console.hh" -using namespace ignition; +using namespace gz; using namespace sensors; -class ignition::sensors::GaussianNoiseModelPrivate +class gz::sensors::GaussianNoiseModelPrivate { /// \brief If type starts with GAUSSIAN, the mean of the distribution /// from which we sample when adding noise. diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc index 7d199361..60b09f6a 100644 --- a/src/GpuLidarSensor.cc +++ b/src/GpuLidarSensor.cc @@ -19,33 +19,33 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif -#include -#include -#include -#include +#include +#include +#include +#include -#include "ignition/sensors/GpuLidarSensor.hh" -#include "ignition/sensors/SensorFactory.hh" +#include "gz/sensors/GpuLidarSensor.hh" +#include "gz/sensors/SensorFactory.hh" -using namespace ignition::sensors; +using namespace gz::sensors; /// \brief Private data for the GpuLidar class -class ignition::sensors::GpuLidarSensorPrivate +class gz::sensors::GpuLidarSensorPrivate { /// \brief Fill the point cloud packed message /// \param[in] _laserBuffer Lidar data buffer. public: void FillPointCloudMsg(const float *_laserBuffer); /// \brief Rendering camera - public: ignition::rendering::GpuRaysPtr gpuRays; + public: gz::rendering::GpuRaysPtr gpuRays; /// \brief Connection to the Manager's scene change event. - public: ignition::common::ConnectionPtr sceneChangeConnection; + public: gz::common::ConnectionPtr sceneChangeConnection; /// \brief The point cloud message. public: msgs::PointCloudPacked pointMsg; @@ -78,7 +78,7 @@ GpuLidarSensor::~GpuLidarSensor() } ///////////////////////////////////////////////// -void GpuLidarSensor::SetScene(ignition::rendering::ScenePtr _scene) +void GpuLidarSensor::SetScene(gz::rendering::ScenePtr _scene) { std::lock_guard lock(this->lidarMutex); // APIs make it possible for the scene pointer to change @@ -94,7 +94,7 @@ void GpuLidarSensor::SetScene(ignition::rendering::ScenePtr _scene) ////////////////////////////////////////////////// void GpuLidarSensor::RemoveGpuRays( - ignition::rendering::ScenePtr _scene) + gz::rendering::ScenePtr _scene) { if (_scene) { @@ -135,7 +135,7 @@ bool GpuLidarSensor::Load(const sdf::Sensor &_sdf) this->SetTopic(this->Topic() + "/points"); this->dataPtr->pointPub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->Topic()); if (!this->dataPtr->pointPub) @@ -217,7 +217,7 @@ bool GpuLidarSensor::CreateLidar() } ////////////////////////////////////////////////// -bool GpuLidarSensor::Update(const ignition::common::Time &_now) +bool GpuLidarSensor::Update(const gz::common::Time &_now) { IGN_PROFILE("GpuLidarSensor::Update"); if (!this->initialized) @@ -272,7 +272,7 @@ bool GpuLidarSensor::Update(const ignition::common::Time &_now) } ///////////////////////////////////////////////// -ignition::common::ConnectionPtr GpuLidarSensor::ConnectNewLidarFrame( +gz::common::ConnectionPtr GpuLidarSensor::ConnectNewLidarFrame( std::function _subscriber) @@ -281,7 +281,7 @@ ignition::common::ConnectionPtr GpuLidarSensor::ConnectNewLidarFrame( } ///////////////////////////////////////////////// -ignition::rendering::GpuRaysPtr GpuLidarSensor::GpuRays() const +gz::rendering::GpuRaysPtr GpuLidarSensor::GpuRays() const { return this->dataPtr->gpuRays; } @@ -293,13 +293,13 @@ bool GpuLidarSensor::IsHorizontal() const } ////////////////////////////////////////////////// -ignition::math::Angle GpuLidarSensor::HFOV() const +gz::math::Angle GpuLidarSensor::HFOV() const { return this->dataPtr->gpuRays->HFOV(); } ////////////////////////////////////////////////// -ignition::math::Angle GpuLidarSensor::VFOV() const +gz::math::Angle GpuLidarSensor::VFOV() const { return this->dataPtr->gpuRays->VFOV(); } diff --git a/src/ImageGaussianNoiseModel.cc b/src/ImageGaussianNoiseModel.cc index 00af1ab6..8e7aa5f0 100644 --- a/src/ImageGaussianNoiseModel.cc +++ b/src/ImageGaussianNoiseModel.cc @@ -20,19 +20,19 @@ #include #endif -#include +#include -#include -#include -#include -#include +#include +#include +#include +#include -#include "ignition/sensors/ImageGaussianNoiseModel.hh" +#include "gz/sensors/ImageGaussianNoiseModel.hh" -using namespace ignition; +using namespace gz; using namespace sensors; -class ignition::sensors::ImageGaussianNoiseModelPrivate +class gz::sensors::ImageGaussianNoiseModelPrivate { /// \brief If type starts with GAUSSIAN, the mean of the distribution /// from which we sample when adding noise. diff --git a/src/ImageNoise.cc b/src/ImageNoise.cc index 4ae51480..51309154 100644 --- a/src/ImageNoise.cc +++ b/src/ImageNoise.cc @@ -21,12 +21,12 @@ #include #endif -#include "ignition/common/Console.hh" +#include "gz/common/Console.hh" -#include "ignition/sensors/ImageNoise.hh" -#include "ignition/sensors/ImageGaussianNoiseModel.hh" +#include "gz/sensors/ImageNoise.hh" +#include "gz/sensors/ImageGaussianNoiseModel.hh" -using namespace ignition; +using namespace gz; using namespace sensors; ////////////////////////////////////////////////// diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 90536618..09702712 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -19,24 +19,24 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif -#include -#include +#include +#include -#include "ignition/sensors/ImuSensor.hh" -#include "ignition/sensors/Noise.hh" -#include "ignition/sensors/SensorFactory.hh" -#include "ignition/sensors/SensorTypes.hh" +#include "gz/sensors/ImuSensor.hh" +#include "gz/sensors/Noise.hh" +#include "gz/sensors/SensorFactory.hh" +#include "gz/sensors/SensorTypes.hh" -using namespace ignition; +using namespace gz; using namespace sensors; /// \brief Private data for ImuSensor -class ignition::sensors::ImuSensorPrivate +class gz::sensors::ImuSensorPrivate { /// \brief node to create publisher public: transport::Node node; diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index 5614ba4f..be54afae 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -19,7 +19,7 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif @@ -27,13 +27,12 @@ #include #include -#include +#include +#include +#include +#include -#include -#include -#include - -using namespace ignition; +using namespace gz; // Default values for use with ADIS16448 IMU // These values come from the Rotors default values: diff --git a/src/Lidar.cc b/src/Lidar.cc index eac868c9..d57275bf 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -19,27 +19,27 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif -#include -#include -#include -#include +#include +#include +#include +#include #include -#include "ignition/sensors/GaussianNoiseModel.hh" -#include "ignition/sensors/Lidar.hh" -#include "ignition/sensors/Noise.hh" -#include "ignition/sensors/SensorFactory.hh" -#include "ignition/sensors/SensorTypes.hh" +#include "gz/sensors/GaussianNoiseModel.hh" +#include "gz/sensors/Lidar.hh" +#include "gz/sensors/Noise.hh" +#include "gz/sensors/SensorFactory.hh" +#include "gz/sensors/SensorTypes.hh" -using namespace ignition::sensors; +using namespace gz::sensors; /// \brief Private data for Lidar class -class ignition::sensors::LidarPrivate +class gz::sensors::LidarPrivate { /// \brief node to create publisher public: transport::Node node; @@ -48,7 +48,7 @@ class ignition::sensors::LidarPrivate public: transport::Node::Publisher pub; /// \brief Laser message to publish data. - public: ignition::msgs::LaserScan laserMsg; + public: gz::msgs::LaserScan laserMsg; /// \brief Noise added to sensor data public: std::map noises; @@ -120,7 +120,7 @@ bool Lidar::Load(const sdf::Sensor &_sdf) this->SetTopic("/lidar"); this->dataPtr->pub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->Topic()); if (!this->dataPtr->pub) { @@ -189,7 +189,7 @@ bool Lidar::Load(sdf::ElementPtr _sdf) } ///////////////////////////////////////////////// -ignition::common::ConnectionPtr Lidar::ConnectNewLidarFrame( +gz::common::ConnectionPtr Lidar::ConnectNewLidarFrame( std::function /*_subscriber*/) @@ -198,7 +198,7 @@ ignition::common::ConnectionPtr Lidar::ConnectNewLidarFrame( } ////////////////////////////////////////////////// -bool Lidar::Update(const ignition::common::Time &/*_now*/) +bool Lidar::Update(const gz::common::Time &/*_now*/) { ignerr << "No lidar data being updated.\n"; return false; @@ -216,7 +216,7 @@ void Lidar::ApplyNoise() int index = j * this->RayCount() + i; double range = this->laserBuffer[index*3]; range = this->dataPtr->noises[LIDAR_NOISE]->Apply(range); - range = ignition::math::clamp(range, + range = gz::math::clamp(range, this->RangeMin(), this->RangeMax()); this->laserBuffer[index*3] = range; } @@ -225,7 +225,7 @@ void Lidar::ApplyNoise() } ////////////////////////////////////////////////// -bool Lidar::PublishLidarScan(const ignition::common::Time &_now) +bool Lidar::PublishLidarScan(const gz::common::Time &_now) { IGN_PROFILE("Lidar::PublishLidarScan"); if (!this->laserBuffer) @@ -259,8 +259,8 @@ bool Lidar::PublishLidarScan(const ignition::common::Time &_now) this->dataPtr->laserMsg.clear_intensities(); for (int i = 0; i < numRays; ++i) { - this->dataPtr->laserMsg.add_ranges(ignition::math::NAN_F); - this->dataPtr->laserMsg.add_intensities(ignition::math::NAN_F); + this->dataPtr->laserMsg.add_ranges(gz::math::NAN_F); + this->dataPtr->laserMsg.add_intensities(gz::math::NAN_F); } } @@ -271,7 +271,7 @@ bool Lidar::PublishLidarScan(const ignition::common::Time &_now) int index = j * this->RangeCount() + i; double range = this->laserBuffer[index*3]; - range = ignition::math::isnan(range) ? this->RangeMax() : range; + range = gz::math::isnan(range) ? this->RangeMax() : range; this->dataPtr->laserMsg.set_ranges(index, range); this->dataPtr->laserMsg.set_intensities(index, this->laserBuffer[index * 3 + 1]); @@ -299,7 +299,7 @@ double Lidar::RangeCountRatio() const } ////////////////////////////////////////////////// -ignition::math::Angle Lidar::AngleMin() const +gz::math::Angle Lidar::AngleMin() const { return this->dataPtr->sdfLidar.HorizontalScanMinAngle(); } @@ -311,7 +311,7 @@ void Lidar::SetAngleMin(double _angle) } ////////////////////////////////////////////////// -ignition::math::Angle Lidar::AngleMax() const +gz::math::Angle Lidar::AngleMax() const { return this->dataPtr->sdfLidar.HorizontalScanMaxAngle(); } @@ -384,7 +384,7 @@ void Lidar::SetParent(const std::string &_parent) } ////////////////////////////////////////////////// -ignition::math::Angle Lidar::VerticalAngleMin() const +gz::math::Angle Lidar::VerticalAngleMin() const { return this->dataPtr->sdfLidar.VerticalScanMinAngle(); } @@ -396,7 +396,7 @@ void Lidar::SetVerticalAngleMin(const double _angle) } ////////////////////////////////////////////////// -ignition::math::Angle Lidar::VerticalAngleMax() const +gz::math::Angle Lidar::VerticalAngleMax() const { return this->dataPtr->sdfLidar.VerticalScanMaxAngle(); } diff --git a/src/Lidar_TEST.cc b/src/Lidar_TEST.cc index c93efc40..adec10d7 100644 --- a/src/Lidar_TEST.cc +++ b/src/Lidar_TEST.cc @@ -19,19 +19,21 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif #include #include -#include -#include +#include +#include -#include -#include -#include +#include +#include +#include + +using namespace ignition; sdf::ElementPtr LidarToSDF(const std::string &name, double update_rate, const std::string &topic, double horz_samples, double horz_resolution, @@ -102,7 +104,7 @@ class Lidar_TEST : public ::testing::Test // Documentation inherited protected: void SetUp() override { - ignition::common::Console::SetVerbosity(4); + gz::common::Console::SetVerbosity(4); } }; @@ -111,11 +113,11 @@ class Lidar_TEST : public ::testing::Test TEST(Lidar_TEST, CreateLaser) { // Create a sensor manager - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; // Create SDF describing a camera sensor const std::string name = "TestLidar"; - const std::string topic = "/ignition/sensors/test/lidar"; + const std::string topic = "/gz/sensors/test/lidar"; const double update_rate = 30; const double horz_samples = 640; const double horz_resolution = 1; @@ -137,7 +139,7 @@ TEST(Lidar_TEST, CreateLaser) range_resolution, range_min, range_max, always_on, visualize); // Create a CameraSensor - ignition::sensors::Lidar *sensor = mgr.CreateSensor( + gz::sensors::Lidar *sensor = mgr.CreateSensor( lidarSDF); EXPECT_FALSE(sensor->CreateLidar()); @@ -147,8 +149,8 @@ TEST(Lidar_TEST, CreateLaser) double angleRes = (sensor->AngleMax() - sensor->AngleMin()).Radian() / sensor->RayCount(); - EXPECT_EQ(sensor->AngleMin(), ignition::math::Angle(-1.396263)); - EXPECT_EQ(sensor->AngleMax(), ignition::math::Angle(1.396263)); + EXPECT_EQ(sensor->AngleMin(), gz::math::Angle(-1.396263)); + EXPECT_EQ(sensor->AngleMax(), gz::math::Angle(1.396263)); EXPECT_NEAR(sensor->RangeMin(), 0.08, 1e-6); EXPECT_NEAR(sensor->RangeMax(), 10.0, 1e-6); EXPECT_NEAR(sensor->AngleResolution(), angleRes, 1e-3); @@ -170,7 +172,7 @@ TEST(Lidar_TEST, CreateLaserFailures) sdfSensor.SetType(sdf::SensorType::CAMERA); sdf::Lidar sdfLidarSensor; - ignition::sensors::Lidar sensor; + gz::sensors::Lidar sensor; EXPECT_FALSE(sensor.Load(sdfSensor)); EXPECT_DOUBLE_EQ(0.0, sensor.Range(-1)); @@ -193,12 +195,12 @@ TEST(Lidar_TEST, CreateLaserFailures) sdfSensor.SetType(sdf::SensorType::LIDAR); sdfSensor.SetLidarSensor(sdfLidarSensor); - ignition::sensors::Lidar sensor2; + gz::sensors::Lidar sensor2; EXPECT_TRUE(sensor2.Load(sdfSensor)); EXPECT_FALSE(sensor2.Load(sdfSensor)); - ignition::sensors::Lidar sensor3; + gz::sensors::Lidar sensor3; sdfLidarSensor.SetHorizontalScanSamples(0); sdfSensor.SetLidarSensor(sdfLidarSensor); @@ -216,16 +218,16 @@ TEST(Lidar_TEST, CreateLaserFailures) sdfLidarSensor.SetLidarNoise(noise); sdfSensor.SetLidarSensor(sdfLidarSensor); - ignition::sensors::Lidar sensor4; + sensors::Lidar sensor4; EXPECT_TRUE(sensor4.Load(sdfSensor)); noise.SetType(sdf::NoiseType::GAUSSIAN_QUANTIZED); sdfLidarSensor.SetLidarNoise(noise); sdfSensor.SetLidarSensor(sdfLidarSensor); - ignition::sensors::Lidar sensor5; + sensors::Lidar sensor5; EXPECT_TRUE(sensor5.Load(sdfSensor)); - sensor.Update(ignition::common::Time(0.1)); + sensor.Update(common::Time(0.1)); } ////////////////////////////////////////////////// diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc index 943d1940..a143b5ba 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -26,22 +26,22 @@ #include -#include -#include +#include +#include -#include +#include -#include -#include +#include +#include -#include "ignition/sensors/SensorFactory.hh" -#include "ignition/sensors/LogicalCameraSensor.hh" +#include "gz/sensors/SensorFactory.hh" +#include "gz/sensors/LogicalCameraSensor.hh" -using namespace ignition; +using namespace gz; using namespace sensors; /// \brief Private data for LogicalCameraSensor -class ignition::sensors::LogicalCameraSensorPrivate +class gz::sensors::LogicalCameraSensorPrivate { /// \brief node to create publisher public: transport::Node node; diff --git a/src/MagnetometerSensor.cc b/src/MagnetometerSensor.cc index 4ea4ef10..f928abb0 100644 --- a/src/MagnetometerSensor.cc +++ b/src/MagnetometerSensor.cc @@ -19,25 +19,25 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif -#include -#include +#include +#include #include -#include "ignition/sensors/MagnetometerSensor.hh" -#include "ignition/sensors/Noise.hh" -#include "ignition/sensors/SensorFactory.hh" -#include "ignition/sensors/SensorTypes.hh" +#include "gz/sensors/MagnetometerSensor.hh" +#include "gz/sensors/Noise.hh" +#include "gz/sensors/SensorFactory.hh" +#include "gz/sensors/SensorTypes.hh" -using namespace ignition; +using namespace gz; using namespace sensors; /// \brief Private data for MagnetometerSensor -class ignition::sensors::MagnetometerSensorPrivate +class gz::sensors::MagnetometerSensorPrivate { /// \brief node to create publisher public: transport::Node node; diff --git a/src/Manager.cc b/src/Manager.cc index 1c9962c7..f855e241 100644 --- a/src/Manager.cc +++ b/src/Manager.cc @@ -15,21 +15,21 @@ * */ -#include "ignition/sensors/Manager.hh" +#include "gz/sensors/Manager.hh" #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -#include "ignition/sensors/config.hh" -#include "ignition/sensors/SensorFactory.hh" +#include "gz/sensors/config.hh" +#include "gz/sensors/SensorFactory.hh" -using namespace ignition::sensors; +using namespace gz::sensors; -class ignition::sensors::ManagerPrivate +class gz::sensors::ManagerPrivate { /// \brief constructor public: ManagerPrivate(); @@ -92,7 +92,7 @@ bool Manager::Remove(const SensorId _id) } ////////////////////////////////////////////////// -void Manager::RunOnce(const ignition::common::Time &_time, bool _force) +void Manager::RunOnce(const gz::common::Time &_time, bool _force) { IGN_PROFILE("SensorManager::RunOnce"); for (auto &s : this->dataPtr->sensors) diff --git a/src/Manager_TEST.cc b/src/Manager_TEST.cc index a57b2f46..23a92dfd 100644 --- a/src/Manager_TEST.cc +++ b/src/Manager_TEST.cc @@ -16,7 +16,7 @@ */ #include -#include +#include /// \brief Test sensor manager class Manager_TEST : public ::testing::Test @@ -24,33 +24,33 @@ class Manager_TEST : public ::testing::Test // Documentation inherited protected: void SetUp() override { - ignition::common::Console::SetVerbosity(4); + gz::common::Console::SetVerbosity(4); } }; ////////////////////////////////////////////////// TEST(Manager, construct) { - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; EXPECT_TRUE(mgr.Init()); sdf::ElementPtr ptr; - ignition::sensors::SensorId id = mgr.CreateSensor(ptr); - EXPECT_EQ(id, ignition::sensors::NO_SENSOR); + gz::sensors::SensorId id = mgr.CreateSensor(ptr); + EXPECT_EQ(id, gz::sensors::NO_SENSOR); - ignition::sensors::Sensor *sensor = mgr.Sensor(0); + gz::sensors::Sensor *sensor = mgr.Sensor(0); EXPECT_EQ(sensor, nullptr); - EXPECT_FALSE(mgr.Remove(ignition::sensors::NO_SENSOR)); + EXPECT_FALSE(mgr.Remove(gz::sensors::NO_SENSOR)); } ////////////////////////////////////////////////// TEST(Manager, removeSensor) { - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; EXPECT_TRUE(mgr.Init()); - EXPECT_FALSE(mgr.Remove(ignition::sensors::NO_SENSOR)); + EXPECT_FALSE(mgr.Remove(gz::sensors::NO_SENSOR)); // \todo(nkoenig) Add a sensor, then remove it } diff --git a/src/Noise.cc b/src/Noise.cc index 4d25945a..c61437bb 100644 --- a/src/Noise.cc +++ b/src/Noise.cc @@ -23,15 +23,15 @@ #include -#include +#include -#include "ignition/sensors/GaussianNoiseModel.hh" -#include "ignition/sensors/Noise.hh" +#include "gz/sensors/GaussianNoiseModel.hh" +#include "gz/sensors/Noise.hh" -using namespace ignition; +using namespace gz; using namespace sensors; -class ignition::sensors::NoisePrivate +class gz::sensors::NoisePrivate { /// \brief Which type of noise we're applying public: NoiseType type = NoiseType::NONE; diff --git a/src/Noise_TEST.cc b/src/Noise_TEST.cc index b15aba84..28f5dde4 100644 --- a/src/Noise_TEST.cc +++ b/src/Noise_TEST.cc @@ -25,10 +25,10 @@ #include -#include "ignition/sensors/Noise.hh" -#include "ignition/sensors/GaussianNoiseModel.hh" +#include "gz/sensors/Noise.hh" +#include "gz/sensors/GaussianNoiseModel.hh" -using namespace ignition; +using namespace gz; const unsigned int g_applyCount = 100; diff --git a/src/PointCloudUtil.cc b/src/PointCloudUtil.cc index b7b839b5..93377bfb 100644 --- a/src/PointCloudUtil.cc +++ b/src/PointCloudUtil.cc @@ -19,7 +19,7 @@ #include -using namespace ignition; +using namespace gz; using namespace sensors; ////////////////////////////////////////////////// diff --git a/src/PointCloudUtil.hh b/src/PointCloudUtil.hh index adeac656..fd569f17 100644 --- a/src/PointCloudUtil.hh +++ b/src/PointCloudUtil.hh @@ -15,22 +15,22 @@ * */ -#ifndef IGNITION_SENSORS_POINTCLOUDUTIL_HH_ -#define IGNITION_SENSORS_POINTCLOUDUTIL_HH_ +#ifndef GZ_SENSORS_POINTCLOUDUTIL_HH_ +#define GZ_SENSORS_POINTCLOUDUTIL_HH_ #if defined(_MSC_VER) #pragma warning(push) #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif -#include +#include -#include "ignition/sensors/config.hh" -#include "ignition/sensors/Export.hh" +#include "gz/sensors/config.hh" +#include "gz/sensors/Export.hh" #ifndef _WIN32 # define PointCloudUtil_EXPORTS_API diff --git a/src/RenderingEvents.cc b/src/RenderingEvents.cc index 834930f2..12c7d42b 100644 --- a/src/RenderingEvents.cc +++ b/src/RenderingEvents.cc @@ -15,16 +15,16 @@ * */ -#include "ignition/sensors/RenderingEvents.hh" +#include "gz/sensors/RenderingEvents.hh" -using namespace ignition::sensors; +using namespace gz::sensors; -ignition::common::EventT +gz::common::EventT RenderingEvents::sceneEvent; ///////////////////////////////////////////////// -ignition::common::ConnectionPtr RenderingEvents::ConnectSceneChangeCallback( - std::function _callback) +gz::common::ConnectionPtr RenderingEvents::ConnectSceneChangeCallback( + std::function _callback) { return sceneEvent.Connect(_callback); } diff --git a/src/RenderingSensor.cc b/src/RenderingSensor.cc index 7744dc5f..1c71fd33 100644 --- a/src/RenderingSensor.cc +++ b/src/RenderingSensor.cc @@ -15,17 +15,17 @@ * */ -#include +#include -#include +#include -#include "ignition/sensors/RenderingSensor.hh" +#include "gz/sensors/RenderingSensor.hh" /// \brief Private data class for RenderingSensor -class ignition::sensors::RenderingSensorPrivate +class gz::sensors::RenderingSensorPrivate { /// \brief Pointer to the scene - public: ignition::rendering::ScenePtr scene; + public: gz::rendering::ScenePtr scene; /// \brief Manually update the rendering scene graph public: bool manualSceneUpdate = false; @@ -35,7 +35,7 @@ class ignition::sensors::RenderingSensorPrivate public: std::vector sensors; }; -using namespace ignition; +using namespace gz; using namespace sensors; ////////////////////////////////////////////////// diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 71f97d0f..40573311 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -20,33 +20,33 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include -#include +#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif -#include -#include -#include +#include +#include +#include -#include -#include +#include +#include -#include +#include #include -#include "ignition/sensors/ImageGaussianNoiseModel.hh" -#include "ignition/sensors/ImageNoise.hh" -#include "ignition/sensors/RgbdCameraSensor.hh" -#include "ignition/sensors/RenderingEvents.hh" -#include "ignition/sensors/SensorFactory.hh" +#include "gz/sensors/ImageGaussianNoiseModel.hh" +#include "gz/sensors/ImageNoise.hh" +#include "gz/sensors/RgbdCameraSensor.hh" +#include "gz/sensors/RenderingEvents.hh" +#include "gz/sensors/SensorFactory.hh" #include "PointCloudUtil.hh" /// \brief Private data for RgbdCameraSensor -class ignition::sensors::RgbdCameraSensorPrivate +class gz::sensors::RgbdCameraSensorPrivate { /// \brief Depth data callback used to get the data from the sensor /// \param[in] _scan pointer to the data from the sensor @@ -86,7 +86,7 @@ class ignition::sensors::RgbdCameraSensorPrivate public: bool initialized = false; /// \brief Rendering camera - public: ignition::rendering::DepthCameraPtr depthCamera; + public: gz::rendering::DepthCameraPtr depthCamera; /// \brief Depth data buffer. public: float *depthBuffer = nullptr; @@ -111,19 +111,19 @@ class ignition::sensors::RgbdCameraSensorPrivate public: unsigned int channels = 4; /// \brief Pointer to an image to be published - public: ignition::rendering::Image image; + public: gz::rendering::Image image; /// \brief Noise added to sensor data public: std::map noises; /// \brief Connection from depth camera with new depth data - public: ignition::common::ConnectionPtr depthConnection; + public: gz::common::ConnectionPtr depthConnection; /// \brief Connection from depth camera with new point cloud data - public: ignition::common::ConnectionPtr pointCloudConnection; + public: gz::common::ConnectionPtr pointCloudConnection; /// \brief Connection to the Manager's scene change event. - public: ignition::common::ConnectionPtr sceneChangeConnection; + public: gz::common::ConnectionPtr sceneChangeConnection; /// \brief Just a mutex for thread safety public: std::mutex mutex; @@ -139,7 +139,7 @@ class ignition::sensors::RgbdCameraSensorPrivate public: PointCloudUtil pointsUtil; }; -using namespace ignition; +using namespace gz; using namespace sensors; ////////////////////////////////////////////////// diff --git a/src/Sensor.cc b/src/Sensor.cc index 0561cf1c..1e35715e 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -15,20 +15,20 @@ * */ -#include "ignition/sensors/Sensor.hh" +#include "gz/sensors/Sensor.hh" #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include -using namespace ignition::sensors; +using namespace gz::sensors; -class ignition::sensors::SensorPrivate +class gz::sensors::SensorPrivate { /// \brief Populates fields from a DOM public: bool PopulateFromSDF(const sdf::Sensor &_sdf); @@ -58,7 +58,7 @@ class ignition::sensors::SensorPrivate public: std::string topic; /// \brief Pose of the sensor - public: ignition::math::Pose3d pose; + public: gz::math::Pose3d pose; /// \brief Flag to enable publishing performance metrics. public: bool enableMetrics{false}; @@ -67,7 +67,7 @@ class ignition::sensors::SensorPrivate public: double updateRate = 0.0; /// \brief What sim time should this sensor update at - public: ignition::common::Time nextUpdateTime; + public: gz::common::Time nextUpdateTime; /// \brief Last steady clock time reading from last Update call. public: std::chrono::time_point lastRealTime; @@ -76,10 +76,10 @@ class ignition::sensors::SensorPrivate public: std::chrono::duration lastUpdateTime{0}; /// \brief Transport node. - public: ignition::transport::Node node; + public: gz::transport::Node node; /// \brief Publishes the PerformanceSensorMetrics message. - public: ignition::transport::Node::Publisher performanceSensorMetricsPub; + public: gz::transport::Node::Publisher performanceSensorMetricsPub; /// \brief SDF element with sensor information. public: sdf::ElementPtr sdf = nullptr; @@ -320,7 +320,7 @@ void SensorPrivate::PublishMetrics(const std::chrono::duration &_now) } ////////////////////////////////////////////////// -ignition::math::Pose3d Sensor::Pose() const +gz::math::Pose3d Sensor::Pose() const { return this->dataPtr->pose; } @@ -338,7 +338,7 @@ void Sensor::SetParent(const std::string &_parent) } ////////////////////////////////////////////////// -void Sensor::SetPose(const ignition::math::Pose3d &_pose) +void Sensor::SetPose(const gz::math::Pose3d &_pose) { this->dataPtr->pose = _pose; } @@ -363,7 +363,7 @@ void Sensor::SetUpdateRate(const double _hz) } ////////////////////////////////////////////////// -bool Sensor::Update(const ignition::common::Time &_now, +bool Sensor::Update(const gz::common::Time &_now, const bool _force) { IGN_PROFILE("Sensor::Update"); @@ -388,7 +388,7 @@ bool Sensor::Update(const ignition::common::Time &_now, if (!_force && this->dataPtr->updateRate > 0.0) { // Update the time the plugin should be loaded - ignition::common::Time delta(1.0 / this->dataPtr->updateRate); + gz::common::Time delta(1.0 / this->dataPtr->updateRate); this->dataPtr->nextUpdateTime += delta; } @@ -396,13 +396,13 @@ bool Sensor::Update(const ignition::common::Time &_now, } ////////////////////////////////////////////////// -ignition::common::Time Sensor::NextUpdateTime() const +gz::common::Time Sensor::NextUpdateTime() const { return this->dataPtr->nextUpdateTime; } ///////////////////////////////////////////////// -void Sensor::AddSequence(ignition::msgs::Header *_msg, +void Sensor::AddSequence(gz::msgs::Header *_msg, const std::string &_seqKey) { std::string value = "0"; @@ -426,7 +426,7 @@ void Sensor::AddSequence(ignition::msgs::Header *_msg, } // Otherwise, add the sequence key-value pair. - ignition::msgs::Header::Map *map = _msg->add_data(); + gz::msgs::Header::Map *map = _msg->add_data(); map->set_key("seq"); map->add_value(value); } diff --git a/src/SensorFactory.cc b/src/SensorFactory.cc index 66797c74..2fc75034 100644 --- a/src/SensorFactory.cc +++ b/src/SensorFactory.cc @@ -15,15 +15,15 @@ * */ -#include -#include -#include -#include "ignition/sensors/config.hh" +#include +#include +#include +#include "gz/sensors/config.hh" -#include "ignition/sensors/SensorFactory.hh" +#include "gz/sensors/SensorFactory.hh" /// \brief Private data class for SensorFactory -class ignition::sensors::SensorFactoryPrivate +class gz::sensors::SensorFactoryPrivate { /// \brief Constructor public: SensorFactoryPrivate(); @@ -32,13 +32,13 @@ class ignition::sensors::SensorFactoryPrivate public: std::map> sensorPlugins; /// \brief Stores paths to search for on file system - public: ignition::common::SystemPaths systemPaths; + public: gz::common::SystemPaths systemPaths; /// \brief For loading plugins - public: ignition::common::PluginLoader pluginLoader; + public: gz::common::PluginLoader pluginLoader; }; -using namespace ignition; +using namespace gz; using namespace sensors; ////////////////////////////////////////////////// diff --git a/src/SensorTypes.cc b/src/SensorTypes.cc index 866cbbec..411bb569 100644 --- a/src/SensorTypes.cc +++ b/src/SensorTypes.cc @@ -15,9 +15,9 @@ * */ -#include "ignition/sensors/SensorTypes.hh" +#include "gz/sensors/SensorTypes.hh" -using namespace ignition; +using namespace gz; using namespace sensors; // Initialize enum iterator, and string converter diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index 5f9faffe..38bf3a71 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -19,24 +19,24 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif -#include +#include #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include -using namespace ignition; +using namespace gz; using namespace sensors; class TestSensor : public Sensor diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index d9f5d1f3..d2a48707 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -20,7 +20,7 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif @@ -28,23 +28,23 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include -#include -#include +#include +#include -#include +#include -#include "ignition/sensors/ThermalCameraSensor.hh" -#include "ignition/sensors/ImageGaussianNoiseModel.hh" -#include "ignition/sensors/RenderingEvents.hh" -#include "ignition/sensors/SensorFactory.hh" +#include "gz/sensors/ThermalCameraSensor.hh" +#include "gz/sensors/ImageGaussianNoiseModel.hh" +#include "gz/sensors/RenderingEvents.hh" +#include "gz/sensors/SensorFactory.hh" /// \brief Private data for ThermalCameraSensor -class ignition::sensors::ThermalCameraSensorPrivate +class gz::sensors::ThermalCameraSensorPrivate { /// \brief Save an image /// \param[in] _data the image data to be saved @@ -56,7 +56,7 @@ class ignition::sensors::ThermalCameraSensorPrivate /// of the path was not possible. /// \sa ImageSaver public: bool SaveImage(const uint16_t *_data, unsigned int _width, - unsigned int _height, ignition::common::Image::PixelFormatType _format); + unsigned int _height, gz::common::Image::PixelFormatType _format); /// \brief Helper function to convert temperature data to thermal image /// \param[in] _data temperature data @@ -73,7 +73,7 @@ class ignition::sensors::ThermalCameraSensorPrivate public: bool initialized = false; /// \brief Rendering camera - public: ignition::rendering::ThermalCameraPtr thermalCamera; + public: gz::rendering::ThermalCameraPtr thermalCamera; /// \brief Thermal data buffer. public: uint16_t *thermalBuffer = nullptr; @@ -86,21 +86,21 @@ class ignition::sensors::ThermalCameraSensorPrivate math::Vector2i::Zero; /// \brief Pointer to an image to be published - public: ignition::rendering::Image image; + public: gz::rendering::Image image; /// \brief Noise added to sensor data public: std::map noises; /// \brief Event that is used to trigger callbacks when a new image /// is generated - public: ignition::common::EventT< - void(const ignition::msgs::Image &)> imageEvent; + public: gz::common::EventT< + void(const gz::msgs::Image &)> imageEvent; /// \brief Connection from thermal camera with thermal data - public: ignition::common::ConnectionPtr thermalConnection; + public: gz::common::ConnectionPtr thermalConnection; /// \brief Connection to the Manager's scene change event. - public: ignition::common::ConnectionPtr sceneChangeConnection; + public: gz::common::ConnectionPtr sceneChangeConnection; /// \brief Just a mutex for thread safety public: std::mutex mutex; @@ -133,16 +133,16 @@ class ignition::sensors::ThermalCameraSensorPrivate public: float ambientRange = 0.0; /// \brief Min temperature the sensor can detect - public: float minTemp = -ignition::math::INF_F; + public: float minTemp = -gz::math::INF_F; /// \brief Max temperature the sensor can detect - public: float maxTemp = ignition::math::INF_F; + public: float maxTemp = gz::math::INF_F; /// \brief Linear resolution. Defaults to 10mK public: float resolution = 0.01f; }; -using namespace ignition; +using namespace gz; using namespace sensors; ////////////////////////////////////////////////// diff --git a/test/integration/TransportTestTools.hh b/test/integration/TransportTestTools.hh index 9281b961..bfc602fb 100644 --- a/test/integration/TransportTestTools.hh +++ b/test/integration/TransportTestTools.hh @@ -22,7 +22,7 @@ #include #include -#include +#include /// \brief class which simplifies waiting for a message to be received template @@ -71,7 +71,7 @@ class WaitForMessageTestHelper } /// \brief Node to subscribe to topics - public: ignition::transport::Node node; + public: gz::transport::Node node; /// \brief True if subscription was created public: bool subscriptionCreated = false; diff --git a/test/integration/air_pressure_plugin.cc b/test/integration/air_pressure_plugin.cc index 94cb2277..e9a7a5a2 100644 --- a/test/integration/air_pressure_plugin.cc +++ b/test/integration/air_pressure_plugin.cc @@ -19,16 +19,16 @@ #include -#include -#include -#include +#include +#include +#include #include "test_config.h" // NOLINT(build/include) #include "TransportTestTools.hh" /// \brief Helper function to create an air pressure sdf element sdf::ElementPtr AirPressureToSdf(const std::string &_name, - const ignition::math::Pose3d &_pose, const double _updateRate, + const gz::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, const bool _visualize) { @@ -60,7 +60,7 @@ sdf::ElementPtr AirPressureToSdf(const std::string &_name, /// \brief Helper function to create an air pressure sdf element with noise sdf::ElementPtr AirPressureToSdfWithNoise(const std::string &_name, - const ignition::math::Pose3d &_pose, const double _updateRate, + const gz::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, const bool _visualize, double _mean, double _stddev, double _bias) { @@ -105,7 +105,7 @@ class AirPressureSensorTest: public testing::Test // Documentation inherited protected: void SetUp() override { - ignition::common::Console::SetVerbosity(4); + gz::common::Console::SetVerbosity(4); } }; @@ -121,8 +121,8 @@ TEST_F(AirPressureSensorTest, CreateAirPressure) const bool visualize = 1; // Create sensor SDF - ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d sensorPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); sdf::ElementPtr airPressureSdf = AirPressureToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); @@ -130,17 +130,17 @@ TEST_F(AirPressureSensorTest, CreateAirPressure) sensorPose, updateRate, topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); // create the sensor using sensor factory - ignition::sensors::SensorFactory sf; - std::unique_ptr sensor = - sf.CreateSensor(airPressureSdf); + gz::sensors::SensorFactory sf; + std::unique_ptr sensor = + sf.CreateSensor(airPressureSdf); ASSERT_NE(nullptr, sensor); EXPECT_EQ(name, sensor->Name()); EXPECT_EQ(topic, sensor->Topic()); EXPECT_DOUBLE_EQ(updateRate, sensor->UpdateRate()); - std::unique_ptr sensorNoise = - sf.CreateSensor( + std::unique_ptr sensorNoise = + sf.CreateSensor( airPressureSdfNoise); ASSERT_NE(nullptr, sensorNoise); @@ -161,8 +161,8 @@ TEST_F(AirPressureSensorTest, SensorReadings) const bool visualize = 1; // Create sensor SDF - ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d sensorPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); sdf::ElementPtr airPressureSdf = AirPressureToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); @@ -171,19 +171,19 @@ TEST_F(AirPressureSensorTest, SensorReadings) // create the sensor using sensor factory // try creating without specifying the sensor type and then cast it - ignition::sensors::SensorFactory sf; - std::unique_ptr s = + gz::sensors::SensorFactory sf; + std::unique_ptr s = sf.CreateSensor(airPressureSdf); - std::unique_ptr sensor( - dynamic_cast(s.release())); + std::unique_ptr sensor( + dynamic_cast(s.release())); // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, sensor); - std::unique_ptr sNoise = + std::unique_ptr sNoise = sf.CreateSensor(airPressureSdfNoise); - std::unique_ptr sensorNoise( - dynamic_cast(sNoise.release())); + std::unique_ptr sensorNoise( + dynamic_cast(sNoise.release())); // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, sensorNoise); @@ -202,11 +202,11 @@ TEST_F(AirPressureSensorTest, SensorReadings) EXPECT_DOUBLE_EQ(vertRef, sensorNoise->ReferenceAltitude()); sensor->SetPose(sensorNoise->Pose() + - ignition::math::Pose3d(0, 0, 1.5, 0, 0, 0)); + gz::math::Pose3d(0, 0, 1.5, 0, 0, 0)); // verify msg received on the topic - WaitForMessageTestHelper msgHelper(topic); - sensor->Update(ignition::common::Time(1, 0)); + WaitForMessageTestHelper msgHelper(topic); + sensor->Update(gz::common::Time(1, 0)); EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; auto msg = msgHelper.Message(); EXPECT_EQ(1, msg.header().stamp().sec()); @@ -215,14 +215,14 @@ TEST_F(AirPressureSensorTest, SensorReadings) EXPECT_DOUBLE_EQ(0.0, msg.variance()); // verify msg with noise received on the topic - WaitForMessageTestHelper + WaitForMessageTestHelper msgHelperNoise(topicNoise); - sensorNoise->Update(ignition::common::Time(1, 0)); + sensorNoise->Update(gz::common::Time(1, 0)); EXPECT_TRUE(msgHelperNoise.WaitForMessage()) << msgHelperNoise; auto msgNoise = msgHelperNoise.Message(); EXPECT_EQ(1, msg.header().stamp().sec()); EXPECT_EQ(0, msg.header().stamp().nsec()); - EXPECT_FALSE(ignition::math::equal(101288.9657925308, msgNoise.pressure())); + EXPECT_FALSE(gz::math::equal(101288.9657925308, msgNoise.pressure())); EXPECT_DOUBLE_EQ(sqrt(0.2), msgNoise.variance()); } @@ -233,10 +233,10 @@ TEST_F(AirPressureSensorTest, Topic) const double updateRate = 30; const bool alwaysOn = 1; const bool visualize = 1; - auto sensorPose = ignition::math::Pose3d(); + auto sensorPose = gz::math::Pose3d(); // Factory - ignition::sensors::SensorFactory factory; + gz::sensors::SensorFactory factory; // Default topic { @@ -248,7 +248,7 @@ TEST_F(AirPressureSensorTest, Topic) EXPECT_NE(nullptr, sensor); auto airPressure = - dynamic_cast(sensor.release()); + dynamic_cast(sensor.release()); ASSERT_NE(nullptr, airPressure); EXPECT_EQ("/air_pressure", airPressure->Topic()); @@ -264,7 +264,7 @@ TEST_F(AirPressureSensorTest, Topic) EXPECT_NE(nullptr, sensor); auto airPressure = - dynamic_cast(sensor.release()); + dynamic_cast(sensor.release()); ASSERT_NE(nullptr, airPressure); EXPECT_EQ("/topic_with_spaces/characters", airPressure->Topic()); diff --git a/test/integration/altimeter_plugin.cc b/test/integration/altimeter_plugin.cc index b566cff4..62a9687c 100644 --- a/test/integration/altimeter_plugin.cc +++ b/test/integration/altimeter_plugin.cc @@ -19,16 +19,16 @@ #include -#include -#include -#include +#include +#include +#include #include "test_config.h" // NOLINT(build/include) #include "TransportTestTools.hh" /// \brief Helper function to create an altimeter sdf element sdf::ElementPtr AltimeterToSdf(const std::string &_name, - const ignition::math::Pose3d &_pose, const double _updateRate, + const gz::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, const bool _visualize) { @@ -60,7 +60,7 @@ sdf::ElementPtr AltimeterToSdf(const std::string &_name, /// \brief Helper function to create an altimeter sdf element with noise sdf::ElementPtr AltimeterToSdfWithNoise(const std::string &_name, - const ignition::math::Pose3d &_pose, const double _updateRate, + const gz::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, const bool _visualize, double _mean, double _stddev, double _bias) { @@ -112,7 +112,7 @@ class AltimeterSensorTest: public testing::Test // Documentation inherited protected: void SetUp() override { - ignition::common::Console::SetVerbosity(4); + gz::common::Console::SetVerbosity(4); } }; @@ -128,8 +128,8 @@ TEST_F(AltimeterSensorTest, CreateAltimeter) const bool visualize = 1; // Create sensor SDF - ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d sensorPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); sdf::ElementPtr altimeterSdf = AltimeterToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); @@ -137,17 +137,17 @@ TEST_F(AltimeterSensorTest, CreateAltimeter) updateRate, topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); // create the sensor using sensor factory - ignition::sensors::SensorFactory sf; - std::unique_ptr sensor = - sf.CreateSensor(altimeterSdf); + gz::sensors::SensorFactory sf; + std::unique_ptr sensor = + sf.CreateSensor(altimeterSdf); ASSERT_NE(nullptr, sensor); EXPECT_EQ(name, sensor->Name()); EXPECT_EQ(topic, sensor->Topic()); EXPECT_DOUBLE_EQ(updateRate, sensor->UpdateRate()); - std::unique_ptr sensorNoise = - sf.CreateSensor(altimeterSdfNoise); + std::unique_ptr sensorNoise = + sf.CreateSensor(altimeterSdfNoise); ASSERT_NE(nullptr, sensorNoise); EXPECT_EQ(name, sensorNoise->Name()); @@ -167,8 +167,8 @@ TEST_F(AltimeterSensorTest, SensorReadings) const bool visualize = 1; // Create sensor SDF - ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d sensorPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); sdf::ElementPtr altimeterSdf = AltimeterToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); @@ -177,19 +177,19 @@ TEST_F(AltimeterSensorTest, SensorReadings) // create the sensor using sensor factory // try creating without specifying the sensor type and then cast it - ignition::sensors::SensorFactory sf; - std::unique_ptr s = + gz::sensors::SensorFactory sf; + std::unique_ptr s = sf.CreateSensor(altimeterSdf); - std::unique_ptr sensor( - dynamic_cast(s.release())); + std::unique_ptr sensor( + dynamic_cast(s.release())); // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, sensor); - std::unique_ptr sNoise = + std::unique_ptr sNoise = sf.CreateSensor(altimeterSdfNoise); - std::unique_ptr sensorNoise( - dynamic_cast(sNoise.release())); + std::unique_ptr sensorNoise( + dynamic_cast(sNoise.release())); // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, sensorNoise); @@ -230,8 +230,8 @@ TEST_F(AltimeterSensorTest, SensorReadings) EXPECT_DOUBLE_EQ(pos - vertRef, sensorNoise->VerticalPosition()); // verify msg received on the topic - WaitForMessageTestHelper msgHelper(topic); - sensor->Update(ignition::common::Time(1, 0)); + WaitForMessageTestHelper msgHelper(topic); + sensor->Update(gz::common::Time(1, 0)); EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; auto msg = msgHelper.Message(); EXPECT_EQ(1, msg.header().stamp().sec()); @@ -241,17 +241,17 @@ TEST_F(AltimeterSensorTest, SensorReadings) EXPECT_DOUBLE_EQ(vertVel, msg.vertical_velocity()); // verify msg with noise received on the topic - WaitForMessageTestHelper + WaitForMessageTestHelper msgHelperNoise(topicNoise); - sensorNoise->Update(ignition::common::Time(1, 0)); + sensorNoise->Update(gz::common::Time(1, 0)); EXPECT_TRUE(msgHelperNoise.WaitForMessage()) << msgHelperNoise; auto msgNoise = msgHelperNoise.Message(); EXPECT_EQ(1, msg.header().stamp().sec()); EXPECT_EQ(0, msg.header().stamp().nsec()); EXPECT_DOUBLE_EQ(vertRef, msgNoise.vertical_reference()); - EXPECT_FALSE(ignition::math::equal(pos - vertRef, + EXPECT_FALSE(gz::math::equal(pos - vertRef, msgNoise.vertical_position())); - EXPECT_FALSE(ignition::math::equal(vertVel, msgNoise.vertical_velocity())); + EXPECT_FALSE(gz::math::equal(vertVel, msgNoise.vertical_velocity())); } ///////////////////////////////////////////////// @@ -261,10 +261,10 @@ TEST_F(AltimeterSensorTest, Topic) const double updateRate = 30; const bool alwaysOn = 1; const bool visualize = 1; - auto sensorPose = ignition::math::Pose3d(); + auto sensorPose = gz::math::Pose3d(); // Factory - ignition::sensors::SensorFactory factory; + gz::sensors::SensorFactory factory; // Default topic { @@ -276,7 +276,7 @@ TEST_F(AltimeterSensorTest, Topic) EXPECT_NE(nullptr, sensor); auto altimeter = - dynamic_cast(sensor.release()); + dynamic_cast(sensor.release()); ASSERT_NE(nullptr, altimeter); EXPECT_EQ("/altimeter", altimeter->Topic()); @@ -292,7 +292,7 @@ TEST_F(AltimeterSensorTest, Topic) EXPECT_NE(nullptr, sensor); auto altimeter = - dynamic_cast(sensor.release()); + dynamic_cast(sensor.release()); ASSERT_NE(nullptr, altimeter); EXPECT_EQ("/topic_with_spaces/characters", altimeter->Topic()); diff --git a/test/integration/camera_plugin.cc b/test/integration/camera_plugin.cc index 48adae79..fae2acc9 100644 --- a/test/integration/camera_plugin.cc +++ b/test/integration/camera_plugin.cc @@ -17,11 +17,11 @@ #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include // TODO(louise) Remove these pragmas once ign-rendering is disabling the // warnings @@ -29,9 +29,9 @@ #pragma warning(push) #pragma warning(disable: 4251) #endif -#include -#include -#include +#include +#include +#include #ifdef _WIN32 #pragma warning(pop) #endif @@ -52,10 +52,10 @@ std::mutex g_infoMutex; unsigned int g_infoCounter = 0; -ignition::msgs::CameraInfo g_infoMsg; +gz::msgs::CameraInfo g_infoMsg; ////////////////////////////////////////////////// -void OnCameraInfo(const ignition::msgs::CameraInfo & _msg) +void OnCameraInfo(const gz::msgs::CameraInfo & _msg) { g_infoMutex.lock(); g_infoCounter++; @@ -75,7 +75,7 @@ class CameraSensorTest: public testing::Test, void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) { // get the darn test data - std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", + std::string path = gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", "integration", "camera_sensor_builtin.sdf"); sdf::SDFPtr doc(new sdf::SDF()); sdf::init(doc); @@ -89,7 +89,7 @@ void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) auto sensorPtr = linkPtr->GetElement("sensor"); // Setup ign-rendering with an empty scene - auto *engine = ignition::rendering::engine(_renderEngine); + auto *engine = gz::rendering::engine(_renderEngine); if (!engine) { igndbg << "Engine '" << _renderEngine @@ -97,13 +97,13 @@ void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) return; } - ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); // do the test - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; - ignition::sensors::CameraSensor *sensor = - mgr.CreateSensor(sensorPtr); + gz::sensors::CameraSensor *sensor = + mgr.CreateSensor(sensorPtr); ASSERT_NE(sensor, nullptr); sensor->SetScene(scene); @@ -116,21 +116,21 @@ void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) // subscribe to the camera info topic std::string infoTopic = sensor->InfoTopic(); - ignition::transport::Node node; + gz::transport::Node node; node.Subscribe(infoTopic, &OnCameraInfo); - WaitForMessageTestHelper helperInfo(infoTopic); + WaitForMessageTestHelper helperInfo(infoTopic); std::string topic = "/test/integration/CameraPlugin_imagesWithBuiltinSDF"; - WaitForMessageTestHelper helper(topic); + WaitForMessageTestHelper helper(topic); // Update once to create image - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(gz::common::Time::Zero); EXPECT_TRUE(helper.WaitForMessage()) << helper; EXPECT_TRUE(helperInfo.WaitForMessage()) << helperInfo; // Check CameraInfo properties - ignition::msgs::CameraInfo infoMsg; + gz::msgs::CameraInfo infoMsg; g_infoMutex.lock(); EXPECT_EQ(1u, g_infoCounter); infoMsg.CopyFrom(g_infoMsg); @@ -169,7 +169,7 @@ void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) // Clean up engine->DestroyScene(scene); - ignition::rendering::unloadEngine(engine->Name()); + gz::rendering::unloadEngine(engine->Name()); } ////////////////////////////////////////////////// @@ -179,12 +179,12 @@ TEST_P(CameraSensorTest, ImagesWithBuiltinSDF) } INSTANTIATE_TEST_CASE_P(CameraSensor, CameraSensorTest, - RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); + RENDER_ENGINE_VALUES, gz::rendering::PrintToStringParam()); ////////////////////////////////////////////////// int main(int argc, char **argv) { - ignition::common::Console::SetVerbosity(4); + gz::common::Console::SetVerbosity(4); ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/test/integration/deprecated_TEST.cc b/test/integration/deprecated_TEST.cc new file mode 100644 index 00000000..8bdae68c --- /dev/null +++ b/test/integration/deprecated_TEST.cc @@ -0,0 +1,26 @@ +/* + * 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 + +///////////////////////////////////////////////// +// Make sure the ignition namespace still works +TEST(Deprecated, IgnitionNamespace) +{ + ignition::sensors::Lidar lidar; +} diff --git a/test/integration/depth_camera_plugin.cc b/test/integration/depth_camera_plugin.cc index 1594285e..f38fc9dc 100644 --- a/test/integration/depth_camera_plugin.cc +++ b/test/integration/depth_camera_plugin.cc @@ -22,16 +22,16 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #include #ifdef _WIN32 #pragma warning(pop) #endif -#include -#include -#include -#include +#include +#include +#include +#include // TODO(louise) Remove these pragmas once ign-rendering is disabling the // warnings @@ -39,11 +39,11 @@ #pragma warning(push) #pragma warning(disable: 4251) #endif -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #ifdef _WIN32 #pragma warning(pop) #endif @@ -62,14 +62,14 @@ float *g_depthBuffer = nullptr; std::mutex g_infoMutex; unsigned int g_infoCounter = 0; -ignition::msgs::CameraInfo g_infoMsg; +gz::msgs::CameraInfo g_infoMsg; std::mutex g_pcMutex; unsigned int g_pcCounter = 0; float *g_pointsXYZBuffer = nullptr; unsigned char *g_pointsRGBBuffer = nullptr; -void UnpackPointCloudMsg(const ignition::msgs::PointCloudPacked &_msg, +void UnpackPointCloudMsg(const gz::msgs::PointCloudPacked &_msg, float *_xyzBuffer, unsigned char *_rgbBuffer) { std::string msgBuffer = _msg.data(); @@ -107,7 +107,7 @@ void UnpackPointCloudMsg(const ignition::msgs::PointCloudPacked &_msg, } } -void OnCameraInfo(const ignition::msgs::CameraInfo & _msg) +void OnCameraInfo(const gz::msgs::CameraInfo & _msg) { g_infoMutex.lock(); g_infoCounter++; @@ -115,7 +115,7 @@ void OnCameraInfo(const ignition::msgs::CameraInfo & _msg) g_infoMutex.unlock(); } -void OnImage(const ignition::msgs::Image &_msg) +void OnImage(const gz::msgs::Image &_msg) { g_mutex.lock(); unsigned int depthSamples = _msg.width() * _msg.height(); @@ -127,7 +127,7 @@ void OnImage(const ignition::msgs::Image &_msg) g_mutex.unlock(); } -void OnPointCloud(const ignition::msgs::PointCloudPacked &_msg) +void OnPointCloud(const gz::msgs::PointCloudPacked &_msg) { g_pcMutex.lock(); @@ -155,7 +155,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( const std::string &_renderEngine) { // get the darn test data - std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", + std::string path = gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", "integration", "depth_camera_sensor_builtin.sdf"); sdf::SDFPtr doc(new sdf::SDF()); sdf::init(doc); @@ -180,7 +180,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( double near_ = clipPtr->Get("near"); double unitBoxSize = 1.0; - ignition::math::Vector3d boxPosition(3.0, 0.0, 0.0); + gz::math::Vector3d boxPosition(3.0, 0.0, 0.0); // If ogre is not the engine, don't run the test if ((_renderEngine.compare("ogre") != 0) && @@ -192,7 +192,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( } // Setup ign-rendering with an empty scene - auto *engine = ignition::rendering::engine(_renderEngine); + auto *engine = gz::rendering::engine(_renderEngine); if (!engine) { igndbg << "Engine '" << _renderEngine @@ -200,14 +200,14 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( return; } - ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); // Create an scene with a box in it scene->SetAmbientLight(0.3, 0.3, 0.3); - ignition::rendering::VisualPtr root = scene->RootVisual(); + gz::rendering::VisualPtr root = scene->RootVisual(); // create blue material - ignition::rendering::MaterialPtr blue = scene->CreateMaterial(); + gz::rendering::MaterialPtr blue = scene->CreateMaterial(); blue->SetAmbient(0.0, 0.0, 0.3); blue->SetDiffuse(0.0, 0.0, 0.8); blue->SetSpecular(0.5, 0.5, 0.5); @@ -215,7 +215,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( blue->SetReflectivity(0); // create box visual - ignition::rendering::VisualPtr box = scene->CreateVisual(); + gz::rendering::VisualPtr box = scene->CreateVisual(); box->AddGeometry(scene->CreateBox()); box->SetOrigin(0.0, 0.0, 0.0); box->SetLocalPosition(boxPosition); @@ -225,10 +225,10 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( root->AddChild(box); // do the test - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; - ignition::sensors::DepthCameraSensor *depthSensor = - mgr.CreateSensor(sensorPtr); + gz::sensors::DepthCameraSensor *depthSensor = + mgr.CreateSensor(sensorPtr); ASSERT_NE(depthSensor, nullptr); depthSensor->SetScene(scene); @@ -239,26 +239,26 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( std::string topic = "/test/integration/DepthCameraPlugin_imagesWithBuiltinSDF/image"; - WaitForMessageTestHelper helper(topic); + WaitForMessageTestHelper helper(topic); std::string pointsTopic = "/test/integration/DepthCameraPlugin_imagesWithBuiltinSDF/image/points"; - WaitForMessageTestHelper + WaitForMessageTestHelper pointsHelper(pointsTopic); std::string infoTopic = "/test/integration/DepthCameraPlugin_imagesWithBuiltinSDF/camera_info"; - WaitForMessageTestHelper infoHelper(infoTopic); + WaitForMessageTestHelper infoHelper(infoTopic); // Update once to create image - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(gz::common::Time::Zero); EXPECT_TRUE(helper.WaitForMessage()) << helper; EXPECT_TRUE(pointsHelper.WaitForMessage()) << pointsHelper; EXPECT_TRUE(infoHelper.WaitForMessage()) << infoHelper; // subscribe to the depth camera topic - ignition::transport::Node node; + gz::transport::Node node; node.Subscribe(topic, &OnImage); // subscribe to the depth camera points topic @@ -268,18 +268,18 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( node.Subscribe(infoTopic, &OnCameraInfo); // wait for a few depth camera frames - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(gz::common::Time::Zero, true); int midWidth = static_cast(depthSensor->ImageWidth() * 0.5); int midHeight = static_cast(depthSensor->ImageHeight() * 0.5); int mid = midHeight * depthSensor->ImageWidth() + midWidth -1; double expectedRangeAtMidPoint = boxPosition.X() - unitBoxSize * 0.5; - ignition::common::Time waitTime = ignition::common::Time(0.001); + gz::common::Time waitTime = gz::common::Time(0.001); int counter = 0; int infoCounter = 0; int pcCounter = 0; - ignition::msgs::CameraInfo infoMsg; + gz::msgs::CameraInfo infoMsg; for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0); ++sleep) { @@ -291,7 +291,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( infoCounter = g_infoCounter; infoMsg = g_infoMsg; g_infoMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + gz::common::Time::Sleep(waitTime); } g_mutex.lock(); g_infoMutex.lock(); @@ -310,9 +310,9 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( // The left and right side of the depth frame should be inf int left = midHeight * depthSensor->ImageWidth(); - EXPECT_DOUBLE_EQ(g_depthBuffer[left], ignition::math::INF_D); + EXPECT_DOUBLE_EQ(g_depthBuffer[left], gz::math::INF_D); int right = (midHeight+1) * depthSensor->ImageWidth() - 1; - EXPECT_DOUBLE_EQ(g_depthBuffer[right], ignition::math::INF_D); + EXPECT_DOUBLE_EQ(g_depthBuffer[right], gz::math::INF_D); g_infoMutex.unlock(); g_mutex.unlock(); @@ -323,7 +323,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( ASSERT_EQ(1, infoMsg.header().data(0).value().size()); EXPECT_EQ("camera1", infoMsg.header().data(0).value(0)); EXPECT_TRUE(infoMsg.has_distortion()); - EXPECT_EQ(ignition::msgs::CameraInfo::Distortion::PLUMB_BOB, + EXPECT_EQ(gz::msgs::CameraInfo::Distortion::PLUMB_BOB, infoMsg.distortion().model()); EXPECT_EQ(5, infoMsg.distortion().k().size()); EXPECT_TRUE(infoMsg.has_intrinsics()); @@ -334,12 +334,12 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( // Check that for a box really close it returns -inf root->RemoveChild(box); - ignition::math::Vector3d boxPositionNear( + gz::math::Vector3d boxPositionNear( unitBoxSize * 0.5 + near_ * 0.5, 0.0, 0.0); box->SetLocalPosition(boxPositionNear); root->AddChild(box); - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(gz::common::Time::Zero, true); for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0); ++sleep) { @@ -350,7 +350,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.lock(); infoCounter = g_infoCounter; g_infoMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + gz::common::Time::Sleep(waitTime); } g_mutex.lock(); @@ -364,18 +364,18 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( infoCounter = 0; pcCounter = 0; - EXPECT_DOUBLE_EQ(g_depthBuffer[mid], -ignition::math::INF_D); + EXPECT_DOUBLE_EQ(g_depthBuffer[mid], -gz::math::INF_D); g_infoMutex.unlock(); g_mutex.unlock(); // Check that for a box really far it returns inf root->RemoveChild(box); - ignition::math::Vector3d boxPositionFar( + gz::math::Vector3d boxPositionFar( unitBoxSize * 0.5 + far_ * 1.5, 0.0, 0.0); box->SetLocalPosition(boxPositionFar); root->AddChild(box); - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(gz::common::Time::Zero, true); for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0); ++sleep) { @@ -386,7 +386,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.lock(); infoCounter = g_infoCounter; g_infoMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + gz::common::Time::Sleep(waitTime); } g_mutex.lock(); g_infoMutex.lock(); @@ -399,19 +399,19 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( infoCounter = 0; pcCounter = 0; - EXPECT_DOUBLE_EQ(g_depthBuffer[mid], ignition::math::INF_D); + EXPECT_DOUBLE_EQ(g_depthBuffer[mid], gz::math::INF_D); g_infoMutex.unlock(); g_mutex.unlock(); // Check that the depth values for a box do not warp. root->RemoveChild(box); - ignition::math::Vector3d boxPositionFillFrame( + gz::math::Vector3d boxPositionFillFrame( unitBoxSize * 0.5 + 0.2, 0.0, 0.0); box->SetLocalPosition(boxPositionFillFrame); root->AddChild(box); - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(gz::common::Time::Zero, true); for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0 || pcCounter == 0); ++sleep) @@ -428,7 +428,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( pcCounter = g_pcCounter; g_pcMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + gz::common::Time::Sleep(waitTime); } g_mutex.lock(); g_infoMutex.lock(); @@ -496,7 +496,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( // Clean up engine->DestroyScene(scene); - ignition::rendering::unloadEngine(engine->Name()); + gz::rendering::unloadEngine(engine->Name()); } ////////////////////////////////////////////////// @@ -506,12 +506,12 @@ TEST_P(DepthCameraSensorTest, ImagesWithBuiltinSDF) } INSTANTIATE_TEST_CASE_P(DepthCameraSensor, DepthCameraSensorTest, - RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); + RENDER_ENGINE_VALUES, gz::rendering::PrintToStringParam()); ////////////////////////////////////////////////// int main(int argc, char **argv) { - ignition::common::Console::SetVerbosity(4); + gz::common::Console::SetVerbosity(4); ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/test/integration/gpu_lidar_sensor_plugin.cc b/test/integration/gpu_lidar_sensor_plugin.cc index ce2838b3..db3a02a9 100644 --- a/test/integration/gpu_lidar_sensor_plugin.cc +++ b/test/integration/gpu_lidar_sensor_plugin.cc @@ -17,15 +17,15 @@ #include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #ifdef _WIN32 #pragma warning(push) #pragma warning(disable: 4005) @@ -35,7 +35,7 @@ #ifdef _WIN32 #pragma warning(pop) #endif -#include +#include // TODO(louise) Remove these pragmas once ign-rendering is disabling the // warnings @@ -43,10 +43,10 @@ #pragma warning(push) #pragma warning(disable: 4251) #endif -#include -#include -#include -#include +#include +#include +#include +#include #ifdef _WIN32 #pragma warning(pop) #endif @@ -64,7 +64,7 @@ #define WAIT_TIME 0.02 sdf::ElementPtr GpuLidarToSdf(const std::string &name, - const ignition::math::Pose3d &pose, const double updateRate, + const gz::math::Pose3d &pose, const double updateRate, const std::string &topic, const double horzSamples, const double horzResolution, const double horzMinAngle, const double horzMaxAngle, const double vertSamples, @@ -121,8 +121,8 @@ sdf::ElementPtr GpuLidarToSdf(const std::string &name, } int g_laserCounter = 0; -std::vector laserMsgs; -std::vector pointMsgs; +std::vector laserMsgs; +std::vector pointMsgs; void OnNewLidarFrame(const float * /*_scan*/, unsigned int /*_width*/, unsigned int /*_height*/, unsigned int /*_channels*/, @@ -132,13 +132,13 @@ void OnNewLidarFrame(const float * /*_scan*/, unsigned int /*_width*/, } ///////////////////////////////////////////////// -void laserCb(const ignition::msgs::LaserScan &_msg) +void laserCb(const gz::msgs::LaserScan &_msg) { laserMsgs.push_back(_msg); } ///////////////////////////////////////////////// -void pointCb(const ignition::msgs::PointCloudPacked &_msg) +void pointCb(const gz::msgs::PointCloudPacked &_msg) { pointMsgs.push_back(_msg); } @@ -189,15 +189,15 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) const bool visualize = 1; // Create sensor description in SDF - ignition::math::Pose3d testPose(ignition::math::Vector3d(0, 0, 0.1), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d testPose(gz::math::Vector3d(0, 0, 0.1), + gz::math::Quaterniond::Identity); sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); // Setup ign-rendering with an empty scene - auto *engine = ignition::rendering::engine(_renderEngine); + auto *engine = gz::rendering::engine(_renderEngine); if (!engine) { igndbg << "Engine '" << _renderEngine @@ -205,25 +205,25 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) return; } - ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); - ignition::rendering::VisualPtr root = scene->RootVisual(); + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); + gz::rendering::VisualPtr root = scene->RootVisual(); // Create a sensor manager - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; // Create an scene with a box in it scene->SetAmbientLight(0.3, 0.3, 0.3); // Create a GpuLidarSensor - ignition::sensors::GpuLidarSensor *sensor = - mgr.CreateSensor(lidarSdf); + gz::sensors::GpuLidarSensor *sensor = + mgr.CreateSensor(lidarSdf); sensor->SetParent(parent); // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, sensor); sensor->SetScene(scene); // Set a callback on the lidar sensor to get a scan - ignition::common::ConnectionPtr c = + gz::common::ConnectionPtr c = sensor->ConnectNewLidarFrame( std::bind(&::OnNewLidarFrame, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, @@ -233,8 +233,8 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) double angleRes = (sensor->AngleMax() - sensor->AngleMin()).Radian() / sensor->RayCount(); - EXPECT_EQ(sensor->AngleMin(), ignition::math::Angle(horzMinAngle)); - EXPECT_EQ(sensor->AngleMax(), ignition::math::Angle(horzMaxAngle)); + EXPECT_EQ(sensor->AngleMin(), gz::math::Angle(horzMinAngle)); + EXPECT_EQ(sensor->AngleMax(), gz::math::Angle(horzMaxAngle)); EXPECT_NEAR(sensor->RangeMin(), rangeMin, 1e-6); EXPECT_NEAR(sensor->RangeMax(), rangeMax, 1e-6); EXPECT_NEAR(sensor->AngleResolution(), angleRes, 1e-3); @@ -251,10 +251,10 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) EXPECT_TRUE(sensor->IsActive()); g_laserCounter = 0; - WaitForMessageTestHelper helper(topic); + WaitForMessageTestHelper helper(topic); // Update once to verify that a message is sent - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(gz::common::Time::Zero); EXPECT_TRUE(helper.WaitForMessage()) << helper; @@ -269,7 +269,7 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) // Check that all the range values are +inf for (unsigned int i = 0; i < ranges.size(); ++i) { - EXPECT_DOUBLE_EQ(ranges[i], ignition::math::INF_D); + EXPECT_DOUBLE_EQ(ranges[i], gz::math::INF_D); EXPECT_DOUBLE_EQ(sensor->Range(i), ranges[i]); EXPECT_NEAR(sensor->Retro(i), 0, 1e-6); EXPECT_EQ(sensor->Fiducial(i), -1); @@ -278,7 +278,7 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) // Clean up c.reset(); engine->DestroyScene(scene); - ignition::rendering::unloadEngine(engine->Name()); + gz::rendering::unloadEngine(engine->Name()); } ///////////////////////////////////////////////// @@ -305,16 +305,16 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) const bool visualize = 1; // Create sensor SDF - ignition::math::Pose3d testPose(ignition::math::Vector3d(0.0, 0.0, 0.1), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d testPose(gz::math::Vector3d(0.0, 0.0, 0.1), + gz::math::Quaterniond::Identity); sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); // Create and populate scene - ignition::rendering::RenderEngine *engine = - ignition::rendering::engine(_renderEngine); + gz::rendering::RenderEngine *engine = + gz::rendering::engine(_renderEngine); if (!engine) { igndbg << "Engine '" << _renderEngine @@ -322,40 +322,40 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) return; } - ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); - ignition::rendering::VisualPtr root = scene->RootVisual(); + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); + gz::rendering::VisualPtr root = scene->RootVisual(); scene->SetAmbientLight(0.3, 0.3, 0.3); // Create testing box // box in the center - ignition::math::Pose3d box01Pose(ignition::math::Vector3d(1, 0, 0.5), - ignition::math::Quaterniond::Identity); - ignition::rendering::VisualPtr visualBox1 = scene->CreateVisual("TestBox1"); + gz::math::Pose3d box01Pose(gz::math::Vector3d(1, 0, 0.5), + gz::math::Quaterniond::Identity); + gz::rendering::VisualPtr visualBox1 = scene->CreateVisual("TestBox1"); visualBox1->AddGeometry(scene->CreateBox()); visualBox1->SetLocalPosition(box01Pose.Pos()); visualBox1->SetLocalRotation(box01Pose.Rot()); root->AddChild(visualBox1); // Create a sensor manager - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; // Create a GpuLidarSensor - ignition::sensors::GpuLidarSensor *sensor = - mgr.CreateSensor(lidarSdf); + gz::sensors::GpuLidarSensor *sensor = + mgr.CreateSensor(lidarSdf); // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, sensor); sensor->SetParent(parent); sensor->SetScene(scene); // subscribe to gpu lidar topic - ignition::transport::Node node; + gz::transport::Node node; node.Subscribe(topic, &::laserCb); node.Subscribe(topic + "/points", &::pointCb); - WaitForMessageTestHelper helper(topic); + WaitForMessageTestHelper helper(topic); // Update sensor - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(gz::common::Time::Zero, true); EXPECT_TRUE(helper.WaitForMessage()) << helper; int mid = horzSamples / 2; @@ -365,16 +365,16 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) abs(box01Pose.Pos().X()) - unitBoxSize/2; // Sensor 1 should see TestBox1 - EXPECT_DOUBLE_EQ(sensor->Range(0), ignition::math::INF_D); + EXPECT_DOUBLE_EQ(sensor->Range(0), gz::math::INF_D); EXPECT_NEAR(sensor->Range(mid), expectedRangeAtMidPointBox1, LASER_TOL); - EXPECT_DOUBLE_EQ(sensor->Range(last), ignition::math::INF_D); + EXPECT_DOUBLE_EQ(sensor->Range(last), gz::math::INF_D); // Make sure to wait to receive the message - ignition::common::Time waitTime = ignition::common::Time(0.01); + gz::common::Time waitTime = gz::common::Time(0.01); int i = 0; while ((laserMsgs.empty() || pointMsgs.empty()) && i < 300) { - ignition::common::Time::Sleep(waitTime); + gz::common::Time::Sleep(waitTime); i++; } EXPECT_LT(i, 300); @@ -383,10 +383,10 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) sensor->RayCount(); // Check we have the same values than using the sensors methods - EXPECT_DOUBLE_EQ(laserMsgs.back().ranges(0), ignition::math::INF_D); + EXPECT_DOUBLE_EQ(laserMsgs.back().ranges(0), gz::math::INF_D); EXPECT_NEAR(laserMsgs.back().ranges(mid), expectedRangeAtMidPointBox1, LASER_TOL); - EXPECT_DOUBLE_EQ(laserMsgs.back().ranges(last), ignition::math::INF_D); + EXPECT_DOUBLE_EQ(laserMsgs.back().ranges(last), gz::math::INF_D); EXPECT_EQ(laserMsgs.back().frame(), name); EXPECT_NEAR(laserMsgs.back().angle_min(), horzMinAngle, 1e-4); @@ -417,7 +417,7 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) // Clean up // engine->DestroyScene(scene); - ignition::rendering::unloadEngine(engine->Name()); + gz::rendering::unloadEngine(engine->Name()); } ///////////////////////////////////////////////// @@ -450,24 +450,24 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) const bool visualize = 1; // Create sensor SDF - ignition::math::Pose3d testPose1(ignition::math::Vector3d(0, 0, 0.1), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d testPose1(gz::math::Vector3d(0, 0, 0.1), + gz::math::Quaterniond::Identity); sdf::ElementPtr lidarSdf1 = GpuLidarToSdf(name1, testPose1, updateRate, topic1, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); // Create a second sensor SDF rotated - ignition::math::Pose3d testPose2(ignition::math::Vector3d(0, 0, 0.1), - ignition::math::Quaterniond(IGN_PI/2.0, 0, 0)); + gz::math::Pose3d testPose2(gz::math::Vector3d(0, 0, 0.1), + gz::math::Quaterniond(IGN_PI/2.0, 0, 0)); sdf::ElementPtr lidarSdf2 = GpuLidarToSdf(name2, testPose2, updateRate, topic2, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); // Create and populate scene - ignition::rendering::RenderEngine *engine = - ignition::rendering::engine(_renderEngine); + gz::rendering::RenderEngine *engine = + gz::rendering::engine(_renderEngine); if (!engine) { igndbg << "Engine '" << _renderEngine @@ -475,21 +475,21 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) return; } - ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); - ignition::rendering::VisualPtr root = scene->RootVisual(); + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); + gz::rendering::VisualPtr root = scene->RootVisual(); scene->SetAmbientLight(0.3, 0.3, 0.3); // Create a sensor manager - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; // Create a GpuLidarSensors - ignition::sensors::GpuLidarSensor *sensor1 = - mgr.CreateSensor(lidarSdf1); + gz::sensors::GpuLidarSensor *sensor1 = + mgr.CreateSensor(lidarSdf1); // Create second GpuLidarSensor - ignition::sensors::GpuLidarSensor *sensor2 = - mgr.CreateSensor(lidarSdf2); + gz::sensors::GpuLidarSensor *sensor2 = + mgr.CreateSensor(lidarSdf2); // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, sensor1); @@ -499,35 +499,35 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) // Create testing boxes // Box in the center - ignition::math::Pose3d box01Pose(ignition::math::Vector3d(3, 0, 0.5), - ignition::math::Quaterniond::Identity); - ignition::rendering::VisualPtr visualBox1 = scene->CreateVisual("UnitBox1"); + gz::math::Pose3d box01Pose(gz::math::Vector3d(3, 0, 0.5), + gz::math::Quaterniond::Identity); + gz::rendering::VisualPtr visualBox1 = scene->CreateVisual("UnitBox1"); visualBox1->AddGeometry(scene->CreateBox()); visualBox1->SetLocalPosition(box01Pose.Pos()); visualBox1->SetLocalRotation(box01Pose.Rot()); root->AddChild(visualBox1); // Box on the right of the first sensor - ignition::math::Pose3d box02Pose(ignition::math::Vector3d(0, -5, 0.5), - ignition::math::Quaterniond::Identity); - ignition::rendering::VisualPtr visualBox2 = scene->CreateVisual("UnitBox2"); + gz::math::Pose3d box02Pose(gz::math::Vector3d(0, -5, 0.5), + gz::math::Quaterniond::Identity); + gz::rendering::VisualPtr visualBox2 = scene->CreateVisual("UnitBox2"); visualBox2->AddGeometry(scene->CreateBox()); visualBox2->SetLocalPosition(box02Pose.Pos()); visualBox2->SetLocalRotation(box02Pose.Rot()); root->AddChild(visualBox2); // Box on the left of the sensor 1 but out of range - ignition::math::Pose3d box03Pose( - ignition::math::Vector3d(0, rangeMax + 1, 0.5), - ignition::math::Quaterniond::Identity); - ignition::rendering::VisualPtr visualBox3 = scene->CreateVisual("UnitBox3"); + gz::math::Pose3d box03Pose( + gz::math::Vector3d(0, rangeMax + 1, 0.5), + gz::math::Quaterniond::Identity); + gz::rendering::VisualPtr visualBox3 = scene->CreateVisual("UnitBox3"); visualBox3->AddGeometry(scene->CreateBox()); visualBox3->SetLocalPosition(box03Pose.Pos()); visualBox3->SetLocalRotation(box03Pose.Rot()); root->AddChild(visualBox3); // Update sensors - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(gz::common::Time::Zero); int mid = horzSamples / 2; int last = (horzSamples - 1); @@ -539,37 +539,37 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) EXPECT_NEAR(sensor1->Range(0), expectedRangeAtMidPointBox2, LASER_TOL); EXPECT_NEAR(sensor1->Range(mid), expectedRangeAtMidPointBox1, LASER_TOL); #ifndef __APPLE__ - // See https://github.com/ignitionrobotics/ign-sensors/issues/66 - EXPECT_DOUBLE_EQ(sensor1->Range(last), ignition::math::INF_D); + // See https://github.com/gazebosim/gz-sensors/issues/66 + EXPECT_DOUBLE_EQ(sensor1->Range(last), gz::math::INF_D); #endif // Only box01 should be visible to sensor 2 - EXPECT_DOUBLE_EQ(sensor2->Range(0), ignition::math::INF_D); + EXPECT_DOUBLE_EQ(sensor2->Range(0), gz::math::INF_D); EXPECT_NEAR(sensor2->Range(mid), expectedRangeAtMidPointBox1, LASER_TOL); - EXPECT_DOUBLE_EQ(sensor2->Range(last), ignition::math::INF_D); + EXPECT_DOUBLE_EQ(sensor2->Range(last), gz::math::INF_D); // Move all boxes out of range - ignition::math::Vector3d box1PositionFar( + gz::math::Vector3d box1PositionFar( rangeMax + 1, 0, 0); - ignition::math::Vector3d box2PositionFar( + gz::math::Vector3d box2PositionFar( 0, -(rangeMax + 1), 0); visualBox1->SetLocalPosition(box1PositionFar); visualBox2->SetLocalPosition(box2PositionFar); // Update sensors - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(gz::common::Time::Zero, true); // Verify values out of range for (unsigned int i = 0; i < sensor1->RayCount(); ++i) - EXPECT_DOUBLE_EQ(sensor1->Range(i), ignition::math::INF_D); + EXPECT_DOUBLE_EQ(sensor1->Range(i), gz::math::INF_D); for (unsigned int i = 0; i < sensor1->RayCount(); ++i) - EXPECT_DOUBLE_EQ(sensor2->Range(i), ignition::math::INF_D); + EXPECT_DOUBLE_EQ(sensor2->Range(i), gz::math::INF_D); // Clean up engine->DestroyScene(scene); - ignition::rendering::unloadEngine(engine->Name()); + gz::rendering::unloadEngine(engine->Name()); } ///////////////////////////////////////////////// @@ -595,16 +595,16 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) const bool visualize = 1; // Create sensor SDF - ignition::math::Pose3d testPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d testPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); sdf::ElementPtr lidarSdf = GpuLidarToSdf(name, testPose, updateRate, topic, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); // Create and populate scene - ignition::rendering::RenderEngine *engine = - ignition::rendering::engine(_renderEngine); + gz::rendering::RenderEngine *engine = + gz::rendering::engine(_renderEngine); if (!engine) { igndbg << "Engine '" << _renderEngine @@ -612,16 +612,16 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) return; } - ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); - ignition::rendering::VisualPtr root = scene->RootVisual(); + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); + gz::rendering::VisualPtr root = scene->RootVisual(); scene->SetAmbientLight(0.3, 0.3, 0.3); // Create testing boxes // box in the center - ignition::math::Pose3d box01Pose(ignition::math::Vector3d(1, 0, 0.5), - ignition::math::Quaterniond::Identity); - ignition::rendering::VisualPtr visualBox1 = + gz::math::Pose3d box01Pose(gz::math::Vector3d(1, 0, 0.5), + gz::math::Quaterniond::Identity); + gz::rendering::VisualPtr visualBox1 = scene->CreateVisual("VerticalTestBox1"); visualBox1->AddGeometry(scene->CreateBox()); visualBox1->SetLocalPosition(box01Pose.Pos()); @@ -629,18 +629,18 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) root->AddChild(visualBox1); // Create a sensor manager - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; // Create a GpuLidarSensor - ignition::sensors::GpuLidarSensor *sensor = - mgr.CreateSensor(lidarSdf); + gz::sensors::GpuLidarSensor *sensor = + mgr.CreateSensor(lidarSdf); // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, sensor); sensor->SetScene(scene); // Update sensor - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(gz::common::Time::Zero); unsigned int mid = horzSamples / 2; double unitBoxSize = 1.0; @@ -656,7 +656,7 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) double expectedRange = expectedRangeAtMidPoint / cos(angleStep); #ifndef __APPLE__ - // See https://github.com/ignitionrobotics/ign-sensors/issues/66 + // See https://github.com/gazebosim/gz-sensors/issues/66 EXPECT_NEAR(sensor->Range(i * horzSamples + mid), expectedRange, VERTICAL_LASER_TOL); #endif @@ -665,17 +665,17 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) // check that the values in the extremes are infinity EXPECT_DOUBLE_EQ(sensor->Range(i * horzSamples), - ignition::math::INF_D); + gz::math::INF_D); EXPECT_DOUBLE_EQ(sensor->Range(i * horzSamples + (horzSamples - 1)), - ignition::math::INF_D); + gz::math::INF_D); } // Move box out of range visualBox1->SetLocalPosition( - ignition::math::Vector3d(rangeMax + 1, 0, 0)); + gz::math::Vector3d(rangeMax + 1, 0, 0)); // Wait for a few more laser scans - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(gz::common::Time::Zero, true); // Verify all values are out of range for (unsigned int j = 0; j < sensor->VerticalRayCount(); ++j) @@ -683,13 +683,13 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) for (unsigned int i = 0; i < sensor->RayCount(); ++i) { EXPECT_DOUBLE_EQ(sensor->Range(j * sensor->RayCount() + i), - ignition::math::INF_D); + gz::math::INF_D); } } // Clean up engine->DestroyScene(scene); - ignition::rendering::unloadEngine(engine->Name()); + gz::rendering::unloadEngine(engine->Name()); } ///////////////////////////////////////////////// @@ -718,24 +718,24 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) const bool visualize = 1; // Create sensor SDF - ignition::math::Pose3d testPose1(ignition::math::Vector3d(0, 0, 0.1), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d testPose1(gz::math::Vector3d(0, 0, 0.1), + gz::math::Quaterniond::Identity); sdf::ElementPtr lidarSdf1 = GpuLidarToSdf(name1, testPose1, updateRate, topic1, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); // Create a second sensor SDF at an xy offset of 1 - ignition::math::Pose3d testPose2(ignition::math::Vector3d(1, 1, 0.1), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d testPose2(gz::math::Vector3d(1, 1, 0.1), + gz::math::Quaterniond::Identity); sdf::ElementPtr lidarSdf2 = GpuLidarToSdf(name2, testPose2, updateRate, topic2, horzSamples, horzResolution, horzMinAngle, horzMaxAngle, vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); // Create and populate scene - ignition::rendering::RenderEngine *engine = - ignition::rendering::engine(_renderEngine); + gz::rendering::RenderEngine *engine = + gz::rendering::engine(_renderEngine); if (!engine) { igndbg << "Engine '" << _renderEngine @@ -743,21 +743,21 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) return; } - ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); - ignition::rendering::VisualPtr root = scene->RootVisual(); + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); + gz::rendering::VisualPtr root = scene->RootVisual(); scene->SetAmbientLight(0.3, 0.3, 0.3); // Create a sensor manager - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; // Create a GpuLidarSensors - ignition::sensors::GpuLidarSensor *sensor1 = - mgr.CreateSensor(lidarSdf1); + gz::sensors::GpuLidarSensor *sensor1 = + mgr.CreateSensor(lidarSdf1); // Create second GpuLidarSensor - ignition::sensors::GpuLidarSensor *sensor2 = - mgr.CreateSensor(lidarSdf2); + gz::sensors::GpuLidarSensor *sensor2 = + mgr.CreateSensor(lidarSdf2); // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, sensor1); @@ -767,9 +767,9 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) // Create testing box // box in the center of lidar1 and right of lidar2 - ignition::math::Pose3d box01Pose(ignition::math::Vector3d(1, 0, 0.5), - ignition::math::Quaterniond::Identity); - ignition::rendering::VisualPtr visualBox1 = scene->CreateVisual("TestBox1"); + gz::math::Pose3d box01Pose(gz::math::Vector3d(1, 0, 0.5), + gz::math::Quaterniond::Identity); + gz::rendering::VisualPtr visualBox1 = scene->CreateVisual("TestBox1"); visualBox1->AddGeometry(scene->CreateBox()); visualBox1->SetLocalPosition(box01Pose.Pos()); visualBox1->SetLocalRotation(box01Pose.Rot()); @@ -785,7 +785,7 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) scene->PreRender(); // Render and update - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(gz::common::Time::Zero); int mid = horzSamples / 2; int last = (horzSamples - 1); @@ -794,25 +794,25 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) abs(box01Pose.Pos().X()) - unitBoxSize/2; // Sensor 1 should see box01 in front of it - EXPECT_DOUBLE_EQ(sensor1->Range(0), ignition::math::INF_D); + EXPECT_DOUBLE_EQ(sensor1->Range(0), gz::math::INF_D); EXPECT_NEAR(sensor1->Range(mid), expectedRangeAtMidPointBox1, LASER_TOL); #ifndef __APPLE__ - // See https://github.com/ignitionrobotics/ign-sensors/issues/66 - EXPECT_DOUBLE_EQ(sensor1->Range(last), ignition::math::INF_D); + // See https://github.com/gazebosim/gz-sensors/issues/66 + EXPECT_DOUBLE_EQ(sensor1->Range(last), gz::math::INF_D); #endif // Sensor 2 should see box01 to the right of it EXPECT_NEAR(sensor2->Range(0), expectedRangeAtMidPointBox1, LASER_TOL); - EXPECT_DOUBLE_EQ(sensor2->Range(mid), ignition::math::INF_D); + EXPECT_DOUBLE_EQ(sensor2->Range(mid), gz::math::INF_D); #ifndef __APPLE__ - // See https://github.com/ignitionrobotics/ign-sensors/issues/66 - EXPECT_DOUBLE_EQ(sensor2->Range(last), ignition::math::INF_D); + // See https://github.com/gazebosim/gz-sensors/issues/66 + EXPECT_DOUBLE_EQ(sensor2->Range(last), gz::math::INF_D); #endif // Clean up // engine->DestroyScene(scene); - ignition::rendering::unloadEngine(engine->Name()); + gz::rendering::unloadEngine(engine->Name()); } ///////////////////////////////////////////////// @@ -833,10 +833,10 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) const double rangeMax = 10.0; const bool alwaysOn = 1; const bool visualize = 1; - auto testPose = ignition::math::Pose3d(); + auto testPose = gz::math::Pose3d(); // Scene - auto engine = ignition::rendering::engine(_renderEngine); + auto engine = gz::rendering::engine(_renderEngine); if (!engine) { igndbg << "Engine '" << _renderEngine @@ -847,7 +847,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) EXPECT_NE(nullptr, scene); // Create a GpuLidarSensor - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; // Default topic @@ -859,12 +859,12 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); auto sensorId = mgr.CreateSensor(lidarSdf); - EXPECT_NE(ignition::sensors::NO_SENSOR, sensorId); + EXPECT_NE(gz::sensors::NO_SENSOR, sensorId); auto sensor = mgr.Sensor(sensorId); EXPECT_NE(nullptr, sensor); - auto lidar = dynamic_cast(sensor); + auto lidar = dynamic_cast(sensor); ASSERT_NE(nullptr, lidar); EXPECT_EQ("/lidar/points", lidar->Topic()); @@ -879,12 +879,12 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); auto sensorId = mgr.CreateSensor(lidarSdf); - EXPECT_NE(ignition::sensors::NO_SENSOR, sensorId); + EXPECT_NE(gz::sensors::NO_SENSOR, sensorId); auto sensor = mgr.Sensor(sensorId); EXPECT_NE(nullptr, sensor); - auto lidar = dynamic_cast(sensor); + auto lidar = dynamic_cast(sensor); ASSERT_NE(nullptr, lidar); EXPECT_EQ("/topic_with_spaces/characters/points", lidar->Topic()); @@ -899,7 +899,7 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); auto sensorId = mgr.CreateSensor(lidarSdf); - EXPECT_EQ(ignition::sensors::NO_SENSOR, sensorId); + EXPECT_EQ(gz::sensors::NO_SENSOR, sensorId); } } @@ -940,11 +940,11 @@ TEST_P(GpuLidarSensorTest, Topic) } INSTANTIATE_TEST_CASE_P(GpuLidarSensor, GpuLidarSensorTest, - RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); + RENDER_ENGINE_VALUES, gz::rendering::PrintToStringParam()); int main(int argc, char **argv) { - ignition::common::Console::SetVerbosity(4); + gz::common::Console::SetVerbosity(4); ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/test/integration/imu_plugin.cc b/test/integration/imu_plugin.cc index fe0daa5a..1189a360 100644 --- a/test/integration/imu_plugin.cc +++ b/test/integration/imu_plugin.cc @@ -19,15 +19,15 @@ #include -#include -#include +#include +#include #include "test_config.h" // NOLINT(build/include) #include "TransportTestTools.hh" /// \brief Helper function to create an imu sdf element sdf::ElementPtr ImuToSdf(const std::string &_name, - const ignition::math::Pose3d &_pose, const double _updateRate, + const gz::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, const bool _visualize) { @@ -63,7 +63,7 @@ class ImuSensorTest: public testing::Test // Documentation inherited protected: void SetUp() override { - ignition::common::Console::SetVerbosity(4); + gz::common::Console::SetVerbosity(4); } }; @@ -78,15 +78,15 @@ TEST_F(ImuSensorTest, CreateImu) const bool visualize = 1; // Create sensor SDF - ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d sensorPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); sdf::ElementPtr imuSdf = ImuToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); // create the sensor using sensor factory - ignition::sensors::SensorFactory sf; - std::unique_ptr sensor = - sf.CreateSensor(imuSdf); + gz::sensors::SensorFactory sf; + std::unique_ptr sensor = + sf.CreateSensor(imuSdf); ASSERT_NE(nullptr, sensor); EXPECT_EQ(name, sensor->Name()); @@ -105,68 +105,68 @@ TEST_F(ImuSensorTest, SensorReadings) const bool visualize = 1; // Create sensor SDF - ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d sensorPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); sdf::ElementPtr imuSdf = ImuToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); // create the sensor using sensor factory // try creating without specifying the sensor type and then cast it - ignition::sensors::SensorFactory sf; - std::unique_ptr s = + gz::sensors::SensorFactory sf; + std::unique_ptr s = sf.CreateSensor(imuSdf); - std::unique_ptr sensor( - dynamic_cast(s.release())); + std::unique_ptr sensor( + dynamic_cast(s.release())); // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, sensor); // subscribe to the topic - WaitForMessageTestHelper msgHelper(topic); + WaitForMessageTestHelper msgHelper(topic); // verify initial readings - EXPECT_EQ(ignition::math::Pose3d::Zero, sensor->WorldPose()); - EXPECT_EQ(ignition::math::Vector3d::Zero, sensor->LinearAcceleration()); - EXPECT_EQ(ignition::math::Vector3d::Zero, sensor->AngularVelocity()); - EXPECT_EQ(ignition::math::Vector3d::Zero, sensor->Gravity()); - EXPECT_EQ(ignition::math::Quaterniond::Identity, + EXPECT_EQ(gz::math::Pose3d::Zero, sensor->WorldPose()); + EXPECT_EQ(gz::math::Vector3d::Zero, sensor->LinearAcceleration()); + EXPECT_EQ(gz::math::Vector3d::Zero, sensor->AngularVelocity()); + EXPECT_EQ(gz::math::Vector3d::Zero, sensor->Gravity()); + EXPECT_EQ(gz::math::Quaterniond::Identity, sensor->OrientationReference()); - EXPECT_EQ(ignition::math::Quaterniond::Identity, sensor->Orientation()); + EXPECT_EQ(gz::math::Quaterniond::Identity, sensor->Orientation()); // 1. Verify imu readings at rest // set orientation reference and verify readings // this sets the initial imu rotation to be +90 degrees in z - ignition::math::Quaterniond orientRef( - ignition::math::Vector3d(0, 0, 1.57)); + gz::math::Quaterniond orientRef( + gz::math::Vector3d(0, 0, 1.57)); sensor->SetOrientationReference(orientRef); EXPECT_EQ(orientRef, sensor->OrientationReference()); // set gravity and verify - ignition::math::Vector3d gravity(0, 0, -4); + gz::math::Vector3d gravity(0, 0, -4); sensor->SetGravity(gravity); EXPECT_EQ(gravity, sensor->Gravity()); // set world pose and verify - ignition::math::Vector3d position(1, 0, 3); - ignition::math::Quaterniond orientation = - ignition::math::Quaterniond::Identity; - ignition::math::Pose3d pose(position, orientation); + gz::math::Vector3d position(1, 0, 3); + gz::math::Quaterniond orientation = + gz::math::Quaterniond::Identity; + gz::math::Pose3d pose(position, orientation); sensor->SetWorldPose(pose); EXPECT_EQ(pose, sensor->WorldPose()); // orientation should still be identity before update - EXPECT_EQ(ignition::math::Quaterniond::Identity, sensor->Orientation()); + EXPECT_EQ(gz::math::Quaterniond::Identity, sensor->Orientation()); // update sensor and verify new readings - EXPECT_TRUE(sensor->Update(ignition::common::Time(1, 0))); + EXPECT_TRUE(sensor->Update(gz::common::Time(1, 0))); EXPECT_EQ(orientRef, sensor->OrientationReference()); EXPECT_EQ(gravity, sensor->Gravity()); EXPECT_EQ(pose, sensor->WorldPose()); - EXPECT_EQ(ignition::math::Vector3d::Zero, sensor->AngularVelocity()); + EXPECT_EQ(gz::math::Vector3d::Zero, sensor->AngularVelocity()); EXPECT_EQ(-gravity, sensor->LinearAcceleration()); - EXPECT_EQ(ignition::math::Quaterniond(ignition::math::Vector3d(0, 0, -1.57)), + EXPECT_EQ(gz::math::Quaterniond(gz::math::Vector3d(0, 0, -1.57)), sensor->Orientation()); // verify msg received on the topic @@ -174,43 +174,43 @@ TEST_F(ImuSensorTest, SensorReadings) auto msg = msgHelper.Message(); EXPECT_EQ(1, msg.header().stamp().sec()); EXPECT_EQ(0, msg.header().stamp().nsec()); - EXPECT_EQ(ignition::math::Vector3d::Zero, - ignition::msgs::Convert(msg.angular_velocity())); - EXPECT_EQ(-gravity, ignition::msgs::Convert(msg.linear_acceleration())); - EXPECT_EQ(ignition::math::Quaterniond(ignition::math::Vector3d(0, 0, -1.57)), - ignition::msgs::Convert(msg.orientation())); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::msgs::Convert(msg.angular_velocity())); + EXPECT_EQ(-gravity, gz::msgs::Convert(msg.linear_acceleration())); + EXPECT_EQ(gz::math::Quaterniond(gz::math::Vector3d(0, 0, -1.57)), + gz::msgs::Convert(msg.orientation())); // 2. Turn imu upside down, give it some linear acc and angular velocity and // verify readings // set angular velocity and verify - ignition::math::Vector3d angularVel(1.0, 2.0, 3.0); + gz::math::Vector3d angularVel(1.0, 2.0, 3.0); sensor->SetAngularVelocity(angularVel); EXPECT_EQ(angularVel, sensor->AngularVelocity()); // set linear acceleration and verify - ignition::math::Vector3d linearAcc(0, 0, 3); + gz::math::Vector3d linearAcc(0, 0, 3); sensor->SetLinearAcceleration(linearAcc); EXPECT_EQ(linearAcc, sensor->LinearAcceleration()); // set orientation and verify - ignition::math::Quaterniond newOrientation(0, 3.14, 0); - ignition::math::Pose3d newPose(position, newOrientation); + gz::math::Quaterniond newOrientation(0, 3.14, 0); + gz::math::Pose3d newPose(position, newOrientation); sensor->SetWorldPose(newPose); EXPECT_EQ(newPose, sensor->WorldPose()); // update sensor and verify new readings - EXPECT_TRUE(sensor->Update(ignition::common::Time(2, 0))); + EXPECT_TRUE(sensor->Update(gz::common::Time(2, 0))); EXPECT_EQ(orientRef, sensor->OrientationReference()); EXPECT_EQ(gravity, sensor->Gravity()); EXPECT_EQ(angularVel, sensor->AngularVelocity()); EXPECT_EQ(newPose, sensor->WorldPose()); - ignition::math::Vector3d expectedLinAcc = linearAcc + gravity; + gz::math::Vector3d expectedLinAcc = linearAcc + gravity; EXPECT_NEAR(expectedLinAcc.X(), sensor->LinearAcceleration().X(), 1e-2); EXPECT_NEAR(expectedLinAcc.Y(), sensor->LinearAcceleration().Y(), 1e-6); EXPECT_NEAR(expectedLinAcc.Z(), sensor->LinearAcceleration().Z(), 1e-5); EXPECT_EQ( - ignition::math::Quaterniond(ignition::math::Vector3d(0, 3.14, -1.57)), + gz::math::Quaterniond(gz::math::Vector3d(0, 3.14, -1.57)), sensor->Orientation()); // verify updated msg @@ -219,15 +219,15 @@ TEST_F(ImuSensorTest, SensorReadings) EXPECT_EQ(2, msg.header().stamp().sec()); EXPECT_EQ(0, msg.header().stamp().nsec()); EXPECT_EQ(angularVel, - ignition::msgs::Convert(msg.angular_velocity())); - ignition::math::Vector3d actualLinAcc = - ignition::msgs::Convert(msg.linear_acceleration()); + gz::msgs::Convert(msg.angular_velocity())); + gz::math::Vector3d actualLinAcc = + gz::msgs::Convert(msg.linear_acceleration()); EXPECT_NEAR(expectedLinAcc.X(), actualLinAcc.X(), 1e-2); EXPECT_NEAR(expectedLinAcc.Y(), actualLinAcc.Y(), 1e-6); EXPECT_NEAR(expectedLinAcc.Z(), actualLinAcc.Z(), 1e-5); EXPECT_EQ( - ignition::math::Quaterniond(ignition::math::Vector3d(0, 3.14, -1.57)), - ignition::msgs::Convert(msg.orientation())); + gz::math::Quaterniond(gz::math::Vector3d(0, 3.14, -1.57)), + gz::msgs::Convert(msg.orientation())); } ///////////////////////////////////////////////// @@ -237,10 +237,10 @@ TEST_F(ImuSensorTest, Topic) const double updateRate = 30; const bool alwaysOn = 1; const bool visualize = 1; - auto sensorPose = ignition::math::Pose3d(); + auto sensorPose = gz::math::Pose3d(); // Factory - ignition::sensors::SensorFactory factory; + gz::sensors::SensorFactory factory; // Default topic { @@ -251,7 +251,7 @@ TEST_F(ImuSensorTest, Topic) auto sensor = factory.CreateSensor(imuSdf); EXPECT_NE(nullptr, sensor); - auto imu = dynamic_cast(sensor.release()); + auto imu = dynamic_cast(sensor.release()); ASSERT_NE(nullptr, imu); EXPECT_EQ("/imu", imu->Topic()); @@ -267,7 +267,7 @@ TEST_F(ImuSensorTest, Topic) EXPECT_NE(nullptr, sensor); auto imu = - dynamic_cast(sensor.release()); + dynamic_cast(sensor.release()); ASSERT_NE(nullptr, imu); EXPECT_EQ("/topic_with_spaces/characters", imu->Topic()); diff --git a/test/integration/logical_camera_plugin.cc b/test/integration/logical_camera_plugin.cc index bf362d89..f6a522bb 100644 --- a/test/integration/logical_camera_plugin.cc +++ b/test/integration/logical_camera_plugin.cc @@ -19,14 +19,14 @@ #include -#include -#include +#include +#include -#include -#include -#include +#include +#include +#include -#include +#include #ifdef _WIN32 #pragma warning(push) #pragma warning(disable: 4005) @@ -36,7 +36,7 @@ #ifdef _WIN32 #pragma warning(pop) #endif -#include +#include #include "test_config.h" // NOLINT(build/include) @@ -48,7 +48,7 @@ /// \brief Helper function to create a logical camera sdf element sdf::ElementPtr LogicalCameraToSdf(const std::string &_name, - const ignition::math::Pose3d &_pose, const double _updateRate, + const gz::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const double _near, const double _far, const double _horzFov, const double _aspectRatio, const bool _alwaysOn, @@ -92,7 +92,7 @@ class LogicalCameraSensorTest: public testing::Test // Documentation inherited protected: void SetUp() override { - ignition::common::Console::SetVerbosity(4); + gz::common::Console::SetVerbosity(4); } }; @@ -111,16 +111,16 @@ TEST_F(LogicalCameraSensorTest, CreateLogicalCamera) const bool visualize = 1; // Create sensor SDF - ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d sensorPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); sdf::ElementPtr logicalCameraSdf = LogicalCameraToSdf(name, sensorPose, updateRate, topic, near, far, horzFov, aspectRatio, alwaysOn, visualize); // create the sensor using sensor factory - ignition::sensors::SensorFactory sf; - std::unique_ptr sensor = - sf.CreateSensor(logicalCameraSdf); + gz::sensors::SensorFactory sf; + std::unique_ptr sensor = + sf.CreateSensor(logicalCameraSdf); ASSERT_NE(nullptr, sensor); EXPECT_EQ(name, sensor->Name()); @@ -148,19 +148,19 @@ TEST_F(LogicalCameraSensorTest, DetectBox) const bool visualize = 1; // Create sensor SDF - ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d sensorPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); sdf::ElementPtr logicalCameraSdf = LogicalCameraToSdf(name, sensorPose, updateRate, topic, near, far, horzFov, aspectRatio, alwaysOn, visualize); // create the sensor using sensor factory // try creating without specifying the sensor type and then cast it - ignition::sensors::SensorFactory sf; - std::unique_ptr s = + gz::sensors::SensorFactory sf; + std::unique_ptr s = sf.CreateSensor(logicalCameraSdf); - std::unique_ptr sensor( - dynamic_cast(s.release())); + std::unique_ptr sensor( + dynamic_cast(s.release())); // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, sensor); @@ -172,73 +172,73 @@ TEST_F(LogicalCameraSensorTest, DetectBox) // Create testing boxes // 1. box in the center std::string boxName = "TestBox"; - ignition::math::Pose3d boxPose(ignition::math::Vector3d(2, 0, 0.5), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d boxPose(gz::math::Vector3d(2, 0, 0.5), + gz::math::Quaterniond::Identity); - std::map modelPoses; + std::map modelPoses; modelPoses[boxName] = boxPose; sensor->SetModelPoses(std::move(modelPoses)); // update - sensor->Update(ignition::common::Time::Zero); + sensor->Update(gz::common::Time::Zero); // verify box is in image img = sensor->Image(); - EXPECT_EQ(sensorPose, ignition::msgs::Convert(img.pose())); + EXPECT_EQ(sensorPose, gz::msgs::Convert(img.pose())); EXPECT_EQ(1, img.model().size()); EXPECT_EQ(boxName, img.model(0).name()); - ignition::math::Pose3d boxPoseCameraFrame = boxPose - sensorPose; - EXPECT_EQ(boxPoseCameraFrame, ignition::msgs::Convert(img.model(0).pose())); + gz::math::Pose3d boxPoseCameraFrame = boxPose - sensorPose; + EXPECT_EQ(boxPoseCameraFrame, gz::msgs::Convert(img.model(0).pose())); // 2. test box outside of frustum - std::map modelPoses2; - ignition::math::Pose3d boxPose2(ignition::math::Vector3d(8, 0, 0.5), - ignition::math::Quaterniond::Identity); + std::map modelPoses2; + gz::math::Pose3d boxPose2(gz::math::Vector3d(8, 0, 0.5), + gz::math::Quaterniond::Identity); modelPoses2[boxName] = boxPose2; sensor->SetModelPoses(std::move(modelPoses2)); // update - sensor->Update(ignition::common::Time::Zero); + sensor->Update(gz::common::Time::Zero); // verify box is not in the image img = sensor->Image(); - EXPECT_EQ(sensorPose, ignition::msgs::Convert(img.pose())); + EXPECT_EQ(sensorPose, gz::msgs::Convert(img.pose())); EXPECT_EQ(0, img.model().size()); // 3. test with different sensor pose // camera now on y, orientated to face box - std::map modelPoses3; - ignition::math::Pose3d sensorPose3(ignition::math::Vector3d(2, 2, 0.5), - ignition::math::Quaterniond(0, 0, -1.57)); + std::map modelPoses3; + gz::math::Pose3d sensorPose3(gz::math::Vector3d(2, 2, 0.5), + gz::math::Quaterniond(0, 0, -1.57)); sensor->SetPose(sensorPose3); - ignition::math::Pose3d boxPose3(ignition::math::Vector3d(2, 0, 0.5), - ignition::math::Quaterniond(0, 0, 1.57)); + gz::math::Pose3d boxPose3(gz::math::Vector3d(2, 0, 0.5), + gz::math::Quaterniond(0, 0, 1.57)); modelPoses3[boxName] = boxPose3; sensor->SetModelPoses(std::move(modelPoses3)); // update - sensor->Update(ignition::common::Time::Zero); + sensor->Update(gz::common::Time::Zero); // verify box is in image img = sensor->Image(); - EXPECT_EQ(sensorPose3, ignition::msgs::Convert(img.pose())); + EXPECT_EQ(sensorPose3, gz::msgs::Convert(img.pose())); EXPECT_EQ(1, img.model().size()); EXPECT_EQ(boxName, img.model(0).name()); - ignition::math::Pose3d boxPose3CameraFrame = boxPose3 - sensorPose3; - EXPECT_EQ(boxPose3CameraFrame, ignition::msgs::Convert(img.model(0).pose())); + gz::math::Pose3d boxPose3CameraFrame = boxPose3 - sensorPose3; + EXPECT_EQ(boxPose3CameraFrame, gz::msgs::Convert(img.model(0).pose())); // 4. rotate camera away and image should be empty - ignition::math::Pose3d sensorPose4(ignition::math::Vector3d(2, 2, 0.5), - ignition::math::Quaterniond(0, 0, 0)); + gz::math::Pose3d sensorPose4(gz::math::Vector3d(2, 2, 0.5), + gz::math::Quaterniond(0, 0, 0)); sensor->SetPose(sensorPose4); // update - sensor->Update(ignition::common::Time::Zero); + sensor->Update(gz::common::Time::Zero); // verify box is no longer in the image img = sensor->Image(); - EXPECT_EQ(sensorPose4, ignition::msgs::Convert(img.pose())); + EXPECT_EQ(sensorPose4, gz::msgs::Convert(img.pose())); EXPECT_EQ(0, img.model().size()); } @@ -253,10 +253,10 @@ TEST_F(LogicalCameraSensorTest, Topic) const double aspectRatio = 1.778; const bool alwaysOn = 1; const bool visualize = 1; - auto sensorPose = ignition::math::Pose3d(); + auto sensorPose = gz::math::Pose3d(); // Factory - ignition::sensors::SensorFactory factory; + gz::sensors::SensorFactory factory; // Default topic { @@ -269,7 +269,7 @@ TEST_F(LogicalCameraSensorTest, Topic) EXPECT_NE(nullptr, sensor); auto logicalCamera = - dynamic_cast( + dynamic_cast( sensor.release()); ASSERT_NE(nullptr, logicalCamera); @@ -287,7 +287,7 @@ TEST_F(LogicalCameraSensorTest, Topic) EXPECT_NE(nullptr, sensor); auto logicalCamera = - dynamic_cast( + dynamic_cast( sensor.release()); ASSERT_NE(nullptr, logicalCamera); diff --git a/test/integration/magnetometer_plugin.cc b/test/integration/magnetometer_plugin.cc index 7751171b..c94e8663 100644 --- a/test/integration/magnetometer_plugin.cc +++ b/test/integration/magnetometer_plugin.cc @@ -19,15 +19,15 @@ #include -#include -#include +#include +#include #include "test_config.h" // NOLINT(build/include) #include "TransportTestTools.hh" /// \brief Helper function to create an magnetometer sdf element sdf::ElementPtr MagnetometerToSdf(const std::string &_name, - const ignition::math::Pose3d &_pose, const double _updateRate, + const gz::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, const bool _visualize) { @@ -59,7 +59,7 @@ sdf::ElementPtr MagnetometerToSdf(const std::string &_name, /// \brief Helper function to create an magnetometer sdf element sdf::ElementPtr MagnetometerToSdfWithNoise(const std::string &_name, - const ignition::math::Pose3d &_pose, const double _updateRate, + const gz::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, const bool _visualize, double _mean, double _stddev, double _bias) { @@ -118,7 +118,7 @@ class MagnetometerSensorTest: public testing::Test // Documentation inherited protected: void SetUp() override { - ignition::common::Console::SetVerbosity(4); + gz::common::Console::SetVerbosity(4); } }; @@ -134,8 +134,8 @@ TEST_F(MagnetometerSensorTest, CreateMagnetometer) const bool visualize = 1; // Create sensor SDF - ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d sensorPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); sdf::ElementPtr magnetometerSdf = MagnetometerToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); @@ -143,13 +143,13 @@ TEST_F(MagnetometerSensorTest, CreateMagnetometer) sensorPose, updateRate, noiseTopic, alwaysOn, visualize, 1.0, 0.2, 10.0); // create the sensor using sensor factory - ignition::sensors::SensorFactory sf; - std::unique_ptr sensor = - sf.CreateSensor(magnetometerSdf); + gz::sensors::SensorFactory sf; + std::unique_ptr sensor = + sf.CreateSensor(magnetometerSdf); ASSERT_NE(nullptr, sensor); - std::unique_ptr sensorNoise = - sf.CreateSensor( + std::unique_ptr sensorNoise = + sf.CreateSensor( magnetometerNoiseSdf); ASSERT_NE(nullptr, sensorNoise); @@ -173,8 +173,8 @@ TEST_F(MagnetometerSensorTest, SensorReadings) const bool visualize = 1; // Create sensor SDF - ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); + gz::math::Pose3d sensorPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); sdf::ElementPtr magnetometerSdf = MagnetometerToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); sdf::ElementPtr magnetometerSdfNoise = MagnetometerToSdfWithNoise(name, @@ -182,71 +182,71 @@ TEST_F(MagnetometerSensorTest, SensorReadings) // create the sensor using sensor factory // try creating without specifying the sensor type and then cast it - ignition::sensors::SensorFactory sf; - std::unique_ptr s = + gz::sensors::SensorFactory sf; + std::unique_ptr s = sf.CreateSensor(magnetometerSdf); - std::unique_ptr sensor( - dynamic_cast(s.release())); + std::unique_ptr sensor( + dynamic_cast(s.release())); - std::unique_ptr sNoise = + std::unique_ptr sNoise = sf.CreateSensor(magnetometerSdfNoise); - std::unique_ptr sensorNoise( - dynamic_cast(sNoise.release())); + std::unique_ptr sensorNoise( + dynamic_cast(sNoise.release())); // Make sure the above dynamic cast worked. ASSERT_NE(nullptr, sensor); ASSERT_NE(nullptr, sensorNoise); // subscribe to the topic - WaitForMessageTestHelper msgHelper(topic); + WaitForMessageTestHelper msgHelper(topic); // subscribe to the topic - WaitForMessageTestHelper msgHelperNoise( + WaitForMessageTestHelper msgHelperNoise( noiseTopic); // verify initial readings - EXPECT_EQ(ignition::math::Pose3d::Zero, sensor->WorldPose()); - EXPECT_EQ(ignition::math::Vector3d::Zero, sensor->WorldMagneticField()); - EXPECT_EQ(ignition::math::Vector3d::Zero, sensor->MagneticField()); + EXPECT_EQ(gz::math::Pose3d::Zero, sensor->WorldPose()); + EXPECT_EQ(gz::math::Vector3d::Zero, sensor->WorldMagneticField()); + EXPECT_EQ(gz::math::Vector3d::Zero, sensor->MagneticField()); // verify initial readings - EXPECT_EQ(ignition::math::Pose3d::Zero, sensorNoise->WorldPose()); - EXPECT_EQ(ignition::math::Vector3d::Zero, sensorNoise->WorldMagneticField()); - EXPECT_EQ(ignition::math::Vector3d::Zero, sensorNoise->MagneticField()); + EXPECT_EQ(gz::math::Pose3d::Zero, sensorNoise->WorldPose()); + EXPECT_EQ(gz::math::Vector3d::Zero, sensorNoise->WorldMagneticField()); + EXPECT_EQ(gz::math::Vector3d::Zero, sensorNoise->MagneticField()); // set magnetic field and verify - ignition::math::Vector3d worldField(1, 2, -4); + gz::math::Vector3d worldField(1, 2, -4); sensor->SetWorldMagneticField(worldField); EXPECT_EQ(worldField, sensor->WorldMagneticField()); - ignition::math::Vector3d worldFieldNoise(2, 1, -2); + gz::math::Vector3d worldFieldNoise(2, 1, -2); sensorNoise->SetWorldMagneticField(worldFieldNoise); EXPECT_EQ(worldFieldNoise, sensorNoise->WorldMagneticField()); // set world pose and verify - ignition::math::Vector3d position(1, 0, 3); - ignition::math::Quaterniond orientation = - ignition::math::Quaterniond::Identity; - ignition::math::Pose3d pose(position, orientation); + gz::math::Vector3d position(1, 0, 3); + gz::math::Quaterniond orientation = + gz::math::Quaterniond::Identity; + gz::math::Pose3d pose(position, orientation); sensor->SetWorldPose(pose); EXPECT_EQ(pose, sensor->WorldPose()); - ignition::math::Vector3d positionNoise(10, 20, 30); - ignition::math::Quaterniond orientationNoise = - ignition::math::Quaterniond::Identity; - ignition::math::Pose3d poseNoise(positionNoise, orientationNoise); + gz::math::Vector3d positionNoise(10, 20, 30); + gz::math::Quaterniond orientationNoise = + gz::math::Quaterniond::Identity; + gz::math::Pose3d poseNoise(positionNoise, orientationNoise); sensorNoise->SetWorldPose(poseNoise); EXPECT_EQ(poseNoise, sensorNoise->WorldPose()); // update sensor and verify new readings // there are not sensor rotations so the magnetic fields in body frame and // world frame should be the same - EXPECT_TRUE(sensor->Update(ignition::common::Time(1, 0))); + EXPECT_TRUE(sensor->Update(gz::common::Time(1, 0))); EXPECT_EQ(pose, sensor->WorldPose()); EXPECT_EQ(worldField, sensor->WorldMagneticField()); EXPECT_EQ(worldField, sensor->MagneticField()); - EXPECT_TRUE(sensorNoise->Update(ignition::common::Time(1, 0))); + EXPECT_TRUE(sensorNoise->Update(gz::common::Time(1, 0))); EXPECT_EQ(poseNoise, sensorNoise->WorldPose()); EXPECT_EQ(worldFieldNoise, sensorNoise->WorldMagneticField()); // There should be noise in the MagneticField @@ -257,7 +257,7 @@ TEST_F(MagnetometerSensorTest, SensorReadings) auto msg = msgHelper.Message(); EXPECT_EQ(1, msg.header().stamp().sec()); EXPECT_EQ(0, msg.header().stamp().nsec()); - EXPECT_EQ(worldField, ignition::msgs::Convert(msg.field_tesla())); + EXPECT_EQ(worldField, gz::msgs::Convert(msg.field_tesla())); // verify msg received on the noiseTopic EXPECT_TRUE(msgHelperNoise.WaitForMessage()) << msgHelperNoise; @@ -265,18 +265,18 @@ TEST_F(MagnetometerSensorTest, SensorReadings) EXPECT_EQ(1, msgNoise.header().stamp().sec()); EXPECT_EQ(0, msgNoise.header().stamp().nsec()); EXPECT_TRUE(worldFieldNoise != - ignition::msgs::Convert(msgNoise.field_tesla())); + gz::msgs::Convert(msgNoise.field_tesla())); // Rotate the magnetometer - ignition::math::Quaterniond newOrientation(0, 3.14, 1.57); - ignition::math::Pose3d newPose(position, newOrientation); + gz::math::Quaterniond newOrientation(0, 3.14, 1.57); + gz::math::Pose3d newPose(position, newOrientation); sensor->SetWorldPose(newPose); EXPECT_EQ(newPose, sensor->WorldPose()); // update sensor and verify new readings - EXPECT_TRUE(sensor->Update(ignition::common::Time(2, 0))); + EXPECT_TRUE(sensor->Update(gz::common::Time(2, 0))); EXPECT_EQ(worldField, sensor->WorldMagneticField()); - ignition::math::Vector3d localField = + gz::math::Vector3d localField = newOrientation.RotateVectorReverse(worldField); EXPECT_EQ(localField, sensor->MagneticField()); @@ -285,7 +285,7 @@ TEST_F(MagnetometerSensorTest, SensorReadings) msg = msgHelper.Message(); EXPECT_EQ(2, msg.header().stamp().sec()); EXPECT_EQ(0, msg.header().stamp().nsec()); - EXPECT_EQ(localField, ignition::msgs::Convert(msg.field_tesla())); + EXPECT_EQ(localField, gz::msgs::Convert(msg.field_tesla())); } ///////////////////////////////////////////////// @@ -295,10 +295,10 @@ TEST_F(MagnetometerSensorTest, Topic) const double updateRate = 30; const bool alwaysOn = 1; const bool visualize = 1; - auto sensorPose = ignition::math::Pose3d(); + auto sensorPose = gz::math::Pose3d(); // Factory - ignition::sensors::SensorFactory factory; + gz::sensors::SensorFactory factory; // Default topic { @@ -310,7 +310,7 @@ TEST_F(MagnetometerSensorTest, Topic) EXPECT_NE(nullptr, sensor); auto magnetometer = - dynamic_cast(sensor.release()); + dynamic_cast(sensor.release()); ASSERT_NE(nullptr, magnetometer); EXPECT_EQ("/magnetometer", magnetometer->Topic()); @@ -326,7 +326,7 @@ TEST_F(MagnetometerSensorTest, Topic) EXPECT_NE(nullptr, sensor); auto magnetometer = - dynamic_cast(sensor.release()); + dynamic_cast(sensor.release()); ASSERT_NE(nullptr, magnetometer); EXPECT_EQ("/topic_with_spaces/characters", magnetometer->Topic()); diff --git a/test/integration/rgbd_camera_plugin.cc b/test/integration/rgbd_camera_plugin.cc index 2b3ad150..e0ced44b 100644 --- a/test/integration/rgbd_camera_plugin.cc +++ b/test/integration/rgbd_camera_plugin.cc @@ -22,16 +22,16 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #include #ifdef _WIN32 #pragma warning(pop) #endif -#include -#include -#include -#include +#include +#include +#include +#include // TODO(louise) Remove these pragmas once ign-rendering is disabling the // warnings @@ -39,11 +39,11 @@ #pragma warning(push) #pragma warning(disable: 4251) #endif -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #ifdef _WIN32 #pragma warning(pop) #endif @@ -56,7 +56,7 @@ #define DEPTH_TOL 1e-4 #define DOUBLE_TOL 1e-6 -using namespace ignition; +using namespace gz; std::mutex g_mutex; unsigned int g_depthCounter = 0; @@ -374,7 +374,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( EXPECT_EQ(0u, mr); EXPECT_EQ(0u, mg); #ifndef __APPLE__ - // See https://github.com/ignitionrobotics/ign-sensors/issues/66 + // See https://github.com/gazebosim/gz-sensors/issues/66 EXPECT_GT(mb, 0u); #endif @@ -440,7 +440,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( EXPECT_EQ(0u, mr); EXPECT_EQ(0u, mg); #ifndef __APPLE__ - // See https://github.com/ignitionrobotics/ign-sensors/issues/66 + // See https://github.com/gazebosim/gz-sensors/issues/66 EXPECT_GT(mb, 0u); #endif diff --git a/test/integration/thermal_camera_plugin.cc b/test/integration/thermal_camera_plugin.cc index 77ec4b2d..5b1f71c4 100644 --- a/test/integration/thermal_camera_plugin.cc +++ b/test/integration/thermal_camera_plugin.cc @@ -22,16 +22,16 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #include #ifdef _WIN32 #pragma warning(pop) #endif -#include -#include -#include -#include +#include +#include +#include +#include // TODO(louise) Remove these pragmas once ign-rendering is disabling the // warnings @@ -39,10 +39,10 @@ #pragma warning(push) #pragma warning(disable: 4251) #endif -#include -#include -#include -#include +#include +#include +#include +#include #ifdef _WIN32 #pragma warning(pop) #endif @@ -58,9 +58,9 @@ uint16_t *g_thermalBuffer = nullptr; std::mutex g_infoMutex; unsigned int g_infoCounter = 0; -ignition::msgs::CameraInfo g_infoMsg; +gz::msgs::CameraInfo g_infoMsg; -void OnCameraInfo(const ignition::msgs::CameraInfo & _msg) +void OnCameraInfo(const gz::msgs::CameraInfo & _msg) { g_infoMutex.lock(); g_infoCounter++; @@ -68,7 +68,7 @@ void OnCameraInfo(const ignition::msgs::CameraInfo & _msg) g_infoMutex.unlock(); } -void OnImage(const ignition::msgs::Image &_msg) +void OnImage(const gz::msgs::Image &_msg) { g_mutex.lock(); unsigned int thermalSamples = _msg.width() * _msg.height(); @@ -91,7 +91,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( const std::string &_renderEngine) { // get the darn test data - std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", + std::string path = gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", "integration", "thermal_camera_sensor_builtin.sdf"); sdf::SDFPtr doc(new sdf::SDF()); sdf::init(doc); @@ -116,7 +116,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( double near_ = clipPtr->Get("near"); double unitBoxSize = 1.0; - ignition::math::Vector3d boxPosition(3.0, 0.0, 0.0); + gz::math::Vector3d boxPosition(3.0, 0.0, 0.0); // If ogre is not the engine, don't run the test if ((_renderEngine.compare("ogre") != 0) && @@ -128,7 +128,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( } // Setup ign-rendering with an empty scene - auto *engine = ignition::rendering::engine(_renderEngine); + auto *engine = gz::rendering::engine(_renderEngine); if (!engine) { igndbg << "Engine '" << _renderEngine @@ -136,14 +136,14 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( return; } - ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); // Create an scene with a box in it scene->SetAmbientLight(0.3, 0.3, 0.3); - ignition::rendering::VisualPtr root = scene->RootVisual(); + gz::rendering::VisualPtr root = scene->RootVisual(); // create box visual - ignition::rendering::VisualPtr box = scene->CreateVisual(); + gz::rendering::VisualPtr box = scene->CreateVisual(); box->AddGeometry(scene->CreateBox()); box->SetOrigin(0.0, 0.0, 0.0); box->SetLocalPosition(boxPosition); @@ -156,10 +156,10 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( root->AddChild(box); - ignition::sensors::Manager mgr; + gz::sensors::Manager mgr; - ignition::sensors::ThermalCameraSensor *thermalSensor = - mgr.CreateSensor(sensorPtr); + gz::sensors::ThermalCameraSensor *thermalSensor = + mgr.CreateSensor(sensorPtr); ASSERT_NE(thermalSensor, nullptr); float ambientTemp = 296.0f; @@ -175,27 +175,27 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( std::string topic = "/test/integration/ThermalCameraPlugin_imagesWithBuiltinSDF/image"; - WaitForMessageTestHelper helper(topic); + WaitForMessageTestHelper helper(topic); std::string infoTopic = "/test/integration/ThermalCameraPlugin_imagesWithBuiltinSDF/camera_info"; - WaitForMessageTestHelper infoHelper(infoTopic); + WaitForMessageTestHelper infoHelper(infoTopic); // Update once to create image - mgr.RunOnce(ignition::common::Time::Zero); + mgr.RunOnce(gz::common::Time::Zero); EXPECT_TRUE(helper.WaitForMessage()) << helper; EXPECT_TRUE(infoHelper.WaitForMessage()) << infoHelper; // subscribe to the thermal camera topic - ignition::transport::Node node; + gz::transport::Node node; node.Subscribe(topic, &OnImage); // subscribe to the thermal camera topic node.Subscribe(infoTopic, &OnCameraInfo); // wait for a few thermal camera frames - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(gz::common::Time::Zero, true); int midWidth = static_cast(thermalSensor->ImageWidth() * 0.5); int midHeight = static_cast(thermalSensor->ImageHeight() * 0.5); @@ -203,10 +203,10 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( int left = midHeight * thermalSensor->ImageWidth(); int right = (midHeight+1) * thermalSensor->ImageWidth() - 1; - ignition::common::Time waitTime = ignition::common::Time(0.001); + gz::common::Time waitTime = gz::common::Time(0.001); int counter = 0; int infoCounter = 0; - ignition::msgs::CameraInfo infoMsg; + gz::msgs::CameraInfo infoMsg; for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0); ++sleep) { @@ -218,7 +218,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( infoCounter = g_infoCounter; infoMsg = g_infoMsg; g_infoMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + gz::common::Time::Sleep(waitTime); } g_mutex.lock(); g_infoMutex.lock(); @@ -252,7 +252,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( ASSERT_EQ(1, infoMsg.header().data(0).value().size()); EXPECT_EQ("camera1", infoMsg.header().data(0).value(0)); EXPECT_TRUE(infoMsg.has_distortion()); - EXPECT_EQ(ignition::msgs::CameraInfo::Distortion::PLUMB_BOB, + EXPECT_EQ(gz::msgs::CameraInfo::Distortion::PLUMB_BOB, infoMsg.distortion().model()); EXPECT_EQ(5, infoMsg.distortion().k().size()); EXPECT_TRUE(infoMsg.has_intrinsics()); @@ -263,12 +263,12 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( // Check that for a box really close it returns box temperature root->RemoveChild(box); - ignition::math::Vector3d boxPositionNear( + gz::math::Vector3d boxPositionNear( unitBoxSize * 0.5 + near_ * 0.5, 0.0, 0.0); box->SetLocalPosition(boxPositionNear); root->AddChild(box); - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(gz::common::Time::Zero, true); for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0); ++sleep) { @@ -279,7 +279,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.lock(); infoCounter = g_infoCounter; g_infoMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + gz::common::Time::Sleep(waitTime); } g_mutex.lock(); @@ -297,12 +297,12 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( // Check that for a box really far it returns ambient temperature root->RemoveChild(box); - ignition::math::Vector3d boxPositionFar( + gz::math::Vector3d boxPositionFar( unitBoxSize * 0.5 + far_ * 1.5, 0.0, 0.0); box->SetLocalPosition(boxPositionFar); root->AddChild(box); - mgr.RunOnce(ignition::common::Time::Zero, true); + mgr.RunOnce(gz::common::Time::Zero, true); for (int sleep = 0; sleep < 300 && (counter == 0 || infoCounter == 0); ++sleep) { @@ -313,7 +313,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.lock(); infoCounter = g_infoCounter; g_infoMutex.unlock(); - ignition::common::Time::Sleep(waitTime); + gz::common::Time::Sleep(waitTime); } g_mutex.lock(); g_infoMutex.lock(); @@ -332,7 +332,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( // Clean up engine->DestroyScene(scene); - ignition::rendering::unloadEngine(engine->Name()); + gz::rendering::unloadEngine(engine->Name()); } ////////////////////////////////////////////////// @@ -342,12 +342,12 @@ TEST_P(ThermalCameraSensorTest, ImagesWithBuiltinSDF) } INSTANTIATE_TEST_CASE_P(ThermalCameraSensor, ThermalCameraSensorTest, - RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); + RENDER_ENGINE_VALUES, gz::rendering::PrintToStringParam()); ////////////////////////////////////////////////// int main(int argc, char **argv) { - ignition::common::Console::SetVerbosity(4); + gz::common::Console::SetVerbosity(4); ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/test/test_config.h.in b/test/test_config.h.in index 50eac1b4..34d294dd 100644 --- a/test/test_config.h.in +++ b/test/test_config.h.in @@ -1,5 +1,5 @@ -#ifndef IGNITION_SENSORS_TEST_CONFIG_HH_ -#define IGNITION_SENSORS_TEST_CONFIG_HH_ +#ifndef GZ_SENSORS_TEST_CONFIG_HH_ +#define GZ_SENSORS_TEST_CONFIG_HH_ #define PROJECT_SOURCE_PATH "${PROJECT_SOURCE_DIR}" #define PROJECT_BUILD_PATH "${PROJECT_BINARY_DIR}" @@ -7,7 +7,7 @@ /// \brief Helper macro to instantiate gtest for different engines #define RENDER_ENGINE_VALUES ::testing::ValuesIn(\ - ignition::rendering::TestValues()) + gz::rendering::TestValues()) // Configure tests based on installed render engines #define WITH_OGRE_AND_NO_OGRE2 (WITH_OGRE && !WITH_OGRE2) @@ -36,7 +36,7 @@ static const std::vector kRenderEngineTestValues{"none"}; #endif #include -#include +#include namespace ignition { diff --git a/tutorials.md.in b/tutorials.md.in index a9539cdf..ad43aff0 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -1,13 +1,13 @@ \page tutorials Tutorials -Welcome to the Ignition @GZ_DESIGNATION_CAP@ tutorials. These tutorials +Welcome to the Gazebo @GZ_DESIGNATION_CAP@ tutorials. These tutorials will guide you through the process of understanding the capabilities of the -Ignition @GZ_DESIGNATION_CAP@ library and how to use the library effectively. +Gazebo @GZ_DESIGNATION_CAP@ library and how to use the library effectively. **The tutorials** 1. \subpage introduction "Introduction" -2. \subpage thermalcameraigngazebo "Thermal Camera in Ignition Gazebo": Using a thermal camera in Ignition Gazebo to detect objects of specific temperatures in camera images. +2. \subpage thermalcameraigngazebo "Thermal Camera in Gazebo Sim": Using a thermal camera in Gazebo Sim to detect objects of specific temperatures in camera images. ## License diff --git a/tutorials/01_intro.md b/tutorials/01_intro.md index 9d99dea9..3d7889fc 100644 --- a/tutorials/01_intro.md +++ b/tutorials/01_intro.md @@ -2,12 +2,12 @@ Next Tutorial: \ref installation -## What is Ignition Sensors? +## What is Gazebo Sensors? -Ignition Senors is an open source library that provides a set of sensor and -noise models accessible through a C++ interface. The goal of Ignition +Gazebo Senors is an open source library that provides a set of sensor and +noise models accessible through a C++ interface. The goal of Gazebo Sensors is to generate realistic sensor data suitable for use in robotic -applications and simulation. The code behind Ignition Sensors was originally +applications and simulation. The code behind Gazebo Sensors was originally developed as a suite of sensor models internal to [Gazebo](http://gazebosim.org). With the addition of some refactoring, the high-quality sensor models previously contained within Gazebo are now diff --git a/tutorials/02_install.md b/tutorials/02_install.md index 498444be..d25f73d0 100644 --- a/tutorials/02_install.md +++ b/tutorials/02_install.md @@ -15,7 +15,7 @@ The source install instructions should be used if you need the very latest softw sudo apt-get update ``` -1. Install Ignition Sensors +1. Install Gazebo Sensors ``` # This installs ign-sensors3. Change the number after libignition-sensors to the version you want sudo apt install libignition-sensors3-dev @@ -32,14 +32,14 @@ necessary prerequisites followed by building from source. ### Prerequisites -Ignition Sensors requires: +Gazebo Sensors requires: - * [Ignition CMake](https://ignitionrobotics.org/libs/cmake) - * [Ignition Math](https://ignitionrobotics.org/libs/math) - * [Ignition Common](https://ignitionrobotics.org/libs/common) - * [Ignition Transport](https://ignitionrobotics.org/libs/transport) - * [Ignition Rendering](https://ignitionrobotics.org/libs/rendering) - * [Ignition Msgs](https://ignitionrobotics.org/libs/msgs) + * [Gazebo CMake](https://gazebosim.org/libs/cmake) + * [Gazebo Math](https://gazebosim.org/libs/math) + * [Gazebo Common](https://gazebosim.org/libs/common) + * [Gazebo Transport](https://gazebosim.org/libs/transport) + * [Gazebo Rendering](https://gazebosim.org/libs/rendering) + * [Gazebo Msgs](https://gazebosim.org/libs/msgs) * [SDFormat](https://github.com/osrf/sdformat) * [Protobuf3](https://developers.google.com/protocol-buffers/) @@ -57,7 +57,7 @@ Ignition Sensors requires: 4. Clone the repository ``` - git clone https://github.com/ignitionrobotics/ign-sensors + git clone https://github.com/gazebosim/gz-sensors ``` 5. Configure and build @@ -74,7 +74,7 @@ Ignition Sensors requires: #### Install Prerequisites -First, follow the [ign-cmake](https://github.com/ignitionrobotics/ign-cmake) tutorial for installing Conda, Visual Studio, CMake, etc., prerequisites, and creating a Conda environment. +First, follow the [ign-cmake](https://github.com/gazebosim/gz-cmake) tutorial for installing Conda, Visual Studio, CMake, etc., prerequisites, and creating a Conda environment. Navigate to `condabin` if necessary to use the `conda` command (i.e., if Conda is not in your `PATH` environment variable. You can find the location of `condabin` in Anaconda Prompt, `where conda`). @@ -85,13 +85,13 @@ conda create -n ign-ws conda activate ign-ws ``` -Install Ignition dependencies, replacing `<#>` with the desired versions: +Install Gazebo dependencies, replacing `<#>` with the desired versions: ``` conda install libignition-cmake<#> libignition-common<#> libignition-math<#> libignition-transport<#> libignition-msgs<#> libignition-plugin<#> --channel conda-forge ``` -Before [ign-rendering](https://github.com/ignitionrobotics/ign-rendering) becomes available on conda-forge, follow its tutorial to build it from source. +Before [ign-rendering](https://github.com/gazebosim/gz-rendering) becomes available on conda-forge, follow its tutorial to build it from source. #### Build from source @@ -103,7 +103,7 @@ Before [ign-rendering](https://github.com/ignitionrobotics/ign-rendering) become 1. Navigate to where you would like to build the library, and clone the repository. ``` # Optionally, append `-b ign-sensors#` (replace # with a number) to check out a specific version - git clone https://github.com/ignitionrobotics/ign-sensors.git + git clone https://github.com/gazebosim/gz-sensors.git ``` 1. Configure and build @@ -126,7 +126,7 @@ Before [ign-rendering](https://github.com/ignitionrobotics/ign-rendering) become # Documentation -API and tutorials can be found at [https://ignitionrobotics.org/libs/sensors](https://ignitionrobotics.org/libs/sensors). +API and tutorials can be found at [https://gazebosim.org/libs/sensors](https://gazebosim.org/libs/sensors). You can also generate the documentation from a clone of this repository by following these steps. @@ -137,7 +137,7 @@ You can also generate the documentation from a clone of this repository by follo 2. Clone the repository ``` - git clone https://github.com/ignitionrobotics/ign-sensors + git clone https://github.com/gazebosim/gz-sensors ``` 3. Configure and build the documentation. diff --git a/tutorials/thermal_camera.md b/tutorials/thermal_camera.md index f48ed9d1..e5110590 100644 --- a/tutorials/thermal_camera.md +++ b/tutorials/thermal_camera.md @@ -1,14 +1,14 @@ -\page thermalcameraigngazebo Thermal Camera in Ignition Gazebo +\page thermalcameraigngazebo Thermal Camera in Gazebo Sim -In this tutorial, we will discuss how to use a thermal camera sensor in [Ignition Gazebo](https://ignitionrobotics.org/libs/gazebo). +In this tutorial, we will discuss how to use a thermal camera sensor in [Gazebo Sim](https://gazebosim.org/libs/gazebo). There are currently a few limitations with the thermal camera, which will be mentioned at the end of the tutorial. ## Requirements -Since this tutorial will show how to use a thermal camera sensor in Ignition Gazebo, you'll need to have Ignition Gazebo installed. -We recommend installing all Ignition libraries, using version Citadel or newer (the thermal camera is not available in Ignition versions prior to Citadel). -If you need to install Ignition, [pick the version you'd like to use](https://ignitionrobotics.org/docs) and then follow the installation instructions. +Since this tutorial will show how to use a thermal camera sensor in Gazebo Sim, you'll need to have Gazebo Sim installed. +We recommend installing all Gazebo libraries, using version Citadel or newer (the thermal camera is not available in Gazebo versions prior to Citadel). +If you need to install Gazebo, [pick the version you'd like to use](https://gazebosim.org/docs) and then follow the installation instructions. ## Setting up the thermal camera @@ -83,10 +83,10 @@ As we can see, we define a sensor with the following SDF elements: - ``: The image size, in pixels. - ``: The near and far clip planes. Objects are only rendered if they're within these planes. * ``: Whether the sensor will always be updated (indicated by `1`) or not (indicated by `0`). -This is currently unused by Ignition Gazebo. +This is currently unused by Gazebo Sim. * ``: The sensor's update rate, in Hz. * ``: Whether the sensor should be visualized in the GUI (indicated by `true`) or not (indicated by `false`). -This is currently unused by Ignition Gazebo. +This is currently unused by Gazebo Sim. * ``: The name of the topic where sensor data is published. This is needed for visualization. ## Assigning a temperature to a model @@ -132,7 +132,7 @@ Here's an example of a box model that has a temperature assigned to it: + name="gz::sim::systems::Thermal"> 200.0 @@ -145,7 +145,7 @@ Most of the code above is for the model - here's the key piece for temperature a ```xml + name="gz::sim::systems::Thermal"> 200.0 ``` @@ -167,7 +167,7 @@ You should see something similar to this: @image html files/thermal_camera/thermal_camera_demo.png The window in the top-left corner shows the thermal camera's output. -Taking a look at the [SDF file](https://github.com/ignitionrobotics/ign-gazebo/blob/c3391b1b664d1ec2b931d9a4ac757bde33b2a27b/examples/worlds/thermal_camera.sdf) for this example shows that the shapes were assigned the following temperatures: +Taking a look at the [SDF file](https://github.com/gazebosim/gz-gazebo/blob/c3391b1b664d1ec2b931d9a4ac757bde33b2a27b/examples/worlds/thermal_camera.sdf) for this example shows that the shapes were assigned the following temperatures: * sphere: 600 Kelvin * box: 200 Kelvin * cylinder: 400 Kelvin @@ -183,7 +183,7 @@ An easy way to move objects in the world is by using `Transform Control`: ## Processing the thermal camera's output -In the example above, the thermal camera publishes an [image message](https://github.com/ignitionrobotics/ign-msgs/blob/46a08597e6b6037adc98025cdc09dfbf0f4467a6/proto/ignition/msgs/image.proto) to the `/thermal_camera` topic whenever the camera has a new image. +In the example above, the thermal camera publishes an [image message](https://github.com/gazebosim/gz-msgs/blob/46a08597e6b6037adc98025cdc09dfbf0f4467a6/proto/ignition/msgs/image.proto) to the `/thermal_camera` topic whenever the camera has a new image. We can observe the contents of a single message by running the following command: ``` @@ -205,14 +205,14 @@ Here's an example thermal camera subscriber that performs this conversion: #include #include -#include +#include // used to set the proper resolution of the camera's output (10mK) double linearResolution = 0.01; // a callback function that is triggered whenever the thermal camera // topic receives a new image message -void OnImage(const ignition::msgs::Image &_msg) +void OnImage(const gz::msgs::Image &_msg) { // convert the serialized image data to 16 bit temperature values unsigned int thermalSamples = _msg.width() * _msg.height(); @@ -240,14 +240,14 @@ void OnImage(const ignition::msgs::Image &_msg) int main(int argc, char **argv) { - ignition::transport::Node node; + gz::transport::Node node; if (!node.Subscribe("/thermal_camera", &OnImage)) { std::cerr << "Error subscribing to the thermal camera topic" << std::endl; return -1; } - ignition::transport::waitForShutdown(); + gz::transport::waitForShutdown(); } ``` From f6771958ac1c38d0cddd6f3d55e9c7d0ee9e7688 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 30 Nov 2022 14:24:36 -0800 Subject: [PATCH 02/26] 3.5.0 pre1 (#291) Signed-off-by: Nate Koenig Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 932cbdef..bb4c4e6c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-sensors3 VERSION 3.4.0) +project(ignition-sensors3 VERSION 3.5.0) #============================================================================ # Find ignition-cmake @@ -15,6 +15,7 @@ find_package(ignition-cmake2 2.13 REQUIRED) #============================================================================ ign_configure_project( REPLACE_IGNITION_INCLUDE_PATH gz/sensors + VERSION_SUFFIX pre1 ) #============================================================================ From 425359ef283a344ab08a842d4658ef7e1dd9c7d4 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 30 Nov 2022 16:52:57 -0800 Subject: [PATCH 03/26] 3.5.0 release (#292) Signed-off-by: Nate Koenig Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 2 +- Changelog.md | 20 ++++++++++++++++++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bb4c4e6c..041cae04 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(ignition-cmake2 2.13 REQUIRED) #============================================================================ ign_configure_project( REPLACE_IGNITION_INCLUDE_PATH gz/sensors - VERSION_SUFFIX pre1 + VERSION_SUFFIX ) #============================================================================ diff --git a/Changelog.md b/Changelog.md index 071c3ab8..c8e4b932 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,25 @@ ## Gazebo Sensors 3 +### Gazebo Sensors 3.5.0 (2022-11-30) + +1. Add missing DEPENDS_ON_COMPONENTS parameters. + * [Pull request #262](https://github.com/gazebosim/gz-sensors/pull/262) + +1. Improved coverage Lidar. + * [Pull request #277](https://github.com/gazebosim/gz-sensors/pull/277) + +1. Improved noise coverage. + * [Pull request #278](https://github.com/gazebosim/gz-sensors/pull/278) + +1. Camera: configure projection matrix from SDFormat. + * [Pull request #249](https://github.com/gazebosim/gz-sensors/pull/249) + +1. RgbdCameraSensor.cc: fix include. + * [Pull request #280](https://github.com/gazebosim/gz-sensors/pull/280) + +1. Ignition to Gazebo header migration. + * [Pull request #260](https://github.com/gazebosim/gz-sensors/pull/260) + ### Gazebo Sensors 3.4.0 (2022-08-16) 1. Remove redundant namespace references From 55e9974a43499d2436cb3824355193d4e6183947 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 31 Jan 2023 16:19:49 -0800 Subject: [PATCH 04/26] Forward port 3 to 6 Signed-off-by: Nate Koenig --- include/gz/sensors/AirPressureSensor.hh | 9 +- include/gz/sensors/AltimeterSensor.hh | 9 +- include/gz/sensors/BoundingBoxCameraSensor.hh | 117 ++++++++++++++ include/gz/sensors/BrownDistortionModel.hh | 87 +++++++++++ include/gz/sensors/CameraSensor.hh | 23 ++- include/gz/sensors/DepthCameraSensor.hh | 20 ++- include/gz/sensors/Distortion.hh | 108 +++++++++++++ include/gz/sensors/ForceTorqueSensor.hh | 132 ++++++++++++++++ include/gz/sensors/GpuLidarSensor.hh | 14 +- .../gz/sensors/ImageBrownDistortionModel.hh | 76 ++++++++++ include/gz/sensors/ImageDistortion.hh | 67 ++++++++ include/gz/sensors/ImuSensor.hh | 57 ++++++- include/gz/sensors/Lidar.hh | 15 +- include/gz/sensors/LogicalCameraSensor.hh | 10 +- include/gz/sensors/MagnetometerSensor.hh | 9 +- include/gz/sensors/Manager.hh | 129 ++++++---------- include/gz/sensors/NavSatSensor.hh | 133 ++++++++++++++++ include/gz/sensors/RgbdCameraSensor.hh | 29 +++- .../gz/sensors/SegmentationCameraSensor.hh | 141 +++++++++++++++++ include/gz/sensors/Sensor.hh | 30 +++- include/gz/sensors/SensorFactory.hh | 143 ++++++++++-------- include/gz/sensors/SensorTypes.hh | 81 ++++++++++ include/gz/sensors/ThermalCameraSensor.hh | 12 +- include/gz/sensors/Util.hh | 68 +++++++++ .../sensors/BoundingBoxCameraSensor.hh | 106 +------------ .../ignition/sensors/BrownDistortionModel.hh | 74 +-------- include/ignition/sensors/Distortion.hh | 93 +----------- include/ignition/sensors/ForceTorqueSensor.hh | 119 +-------------- .../sensors/ImageBrownDistortionModel.hh | 63 +------- include/ignition/sensors/ImageDistortion.hh | 54 +------ include/ignition/sensors/NavSatSensor.hh | 122 +-------------- .../sensors/SegmentationCameraSensor.hh | 130 +--------------- include/ignition/sensors/Util.hh | 57 +------ .../sensors/boundingbox_camera/Export.hh | 19 +++ .../ignition/sensors/force_torque/Export.hh | 19 +++ include/ignition/sensors/navsat/Export.hh | 19 +++ .../sensors/segmentation_camera/Export.hh | 19 +++ 37 files changed, 1440 insertions(+), 973 deletions(-) create mode 100644 include/gz/sensors/BoundingBoxCameraSensor.hh create mode 100644 include/gz/sensors/BrownDistortionModel.hh create mode 100644 include/gz/sensors/Distortion.hh create mode 100644 include/gz/sensors/ForceTorqueSensor.hh create mode 100644 include/gz/sensors/ImageBrownDistortionModel.hh create mode 100644 include/gz/sensors/ImageDistortion.hh create mode 100644 include/gz/sensors/NavSatSensor.hh create mode 100644 include/gz/sensors/SegmentationCameraSensor.hh create mode 100644 include/gz/sensors/Util.hh create mode 100644 include/ignition/sensors/boundingbox_camera/Export.hh create mode 100644 include/ignition/sensors/force_torque/Export.hh create mode 100644 include/ignition/sensors/navsat/Export.hh create mode 100644 include/ignition/sensors/segmentation_camera/Export.hh diff --git a/include/gz/sensors/AirPressureSensor.hh b/include/gz/sensors/AirPressureSensor.hh index 121028ca..7da1c32e 100644 --- a/include/gz/sensors/AirPressureSensor.hh +++ b/include/gz/sensors/AirPressureSensor.hh @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -68,7 +67,8 @@ namespace ignition /// \brief Update the sensor and generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Set the reference altitude. /// \param[in] _ref Verical reference position in meters @@ -78,6 +78,11 @@ namespace ignition /// \return Verical reference position in meters public: double ReferenceAltitude() const; + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Data pointer for private data /// \internal diff --git a/include/gz/sensors/AltimeterSensor.hh b/include/gz/sensors/AltimeterSensor.hh index c89affe1..0ad79db7 100644 --- a/include/gz/sensors/AltimeterSensor.hh +++ b/include/gz/sensors/AltimeterSensor.hh @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -68,7 +67,8 @@ namespace ignition /// \brief Update the sensor and generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Set the vertical reference position of the altimeter /// \param[in] _ref Verical reference position in meters @@ -95,6 +95,11 @@ namespace ignition /// \return Vertical velocity in meters per second public: double VerticalVelocity() const; + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Data pointer for private data /// \internal diff --git a/include/gz/sensors/BoundingBoxCameraSensor.hh b/include/gz/sensors/BoundingBoxCameraSensor.hh new file mode 100644 index 00000000..9dc77e18 --- /dev/null +++ b/include/gz/sensors/BoundingBoxCameraSensor.hh @@ -0,0 +1,117 @@ +/* + * 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_SENSORS_BOUNDINGBOXCAMERASENSOR_HH_ +#define IGNITION_SENSORS_BOUNDINGBOXCAMERASENSOR_HH_ + +#include +#include + +#include +#include + +#include "ignition/sensors/CameraSensor.hh" +#include "ignition/sensors/boundingbox_camera/Export.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // forward declarations + class BoundingBoxCameraSensorPrivate; + + /// \brief BoundingBox camera sensor class. + /// + /// This class creates a BoundingBox image from an ignition rendering scene. + /// The scene must be created in advance and given to Manager::Init(). + /// It offers both an ignition-transport interface and a direct C++ API + /// to access the image data. The API works by setting a callback to be + /// called with image data. + class IGNITION_SENSORS_BOUNDINGBOX_CAMERA_VISIBLE + BoundingBoxCameraSensor : public CameraSensor + { + /// \brief constructor + public: BoundingBoxCameraSensor(); + + /// \brief destructor + public: virtual ~BoundingBoxCameraSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successful + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; + + /// \brief Get the rendering BoundingBox camera + /// \return BoundingBox camera pointer + public: virtual rendering::BoundingBoxCameraPtr + BoundingBoxCamera() const; + + /// \brief Callback on new bounding boxes from bounding boxes camera + /// \param[in] _boxes Detected bounding boxes from the camera + public: void OnNewBoundingBoxes( + const std::vector &_boxes); + + /// \brief Set the rendering scene. + /// \param[in] _scene Pointer to the scene + public: virtual void SetScene( + ignition::rendering::ScenePtr _scene) override; + + /// \brief Get image width. + /// \return width of the image + public: virtual unsigned int ImageWidth() const override; + + /// \brief Get image height. + /// \return height of the image + public: virtual unsigned int ImageHeight() const override; + + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + + /// \brief Create a camera in a scene + /// \return True on success. + private: bool CreateCamera(); + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/BrownDistortionModel.hh b/include/gz/sensors/BrownDistortionModel.hh new file mode 100644 index 00000000..388c6d66 --- /dev/null +++ b/include/gz/sensors/BrownDistortionModel.hh @@ -0,0 +1,87 @@ +/* + * 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_SENSORS_BROWNDISTORTIONMODEL_HH_ +#define IGNITION_SENSORS_BROWNDISTORTIONMODEL_HH_ + +#include + +#include "ignition/sensors/Distortion.hh" +#include "ignition/sensors/Export.hh" +#include "ignition/sensors/config.hh" +#include "ignition/utils/ImplPtr.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + // Forward declarations + class BrownDistortionModelPrivate; + + /** \class BrownDistortionModel BrownDistortionModel.hh \ + ignition/sensors/BrownDistortionModel.hh + **/ + /// \brief Brown Distortion Model class + class IGNITION_SENSORS_VISIBLE BrownDistortionModel : public Distortion + { + /// \brief Constructor. + public: BrownDistortionModel(); + + /// \brief Destructor. + public: virtual ~BrownDistortionModel(); + + // Documentation inherited. + public: virtual void Load(const sdf::Camera &_sdf) override; + + /// \brief Get the radial distortion coefficient k1. + /// \return Distortion coefficient k1. + public: double K1() const; + + /// \brief Get the radial distortion coefficient k2. + /// \return Distortion coefficient k2. + public: double K2() const; + + /// \brief Get the radial distortion coefficient k3. + /// \return Distortion coefficient k3. + public: double K3() const; + + /// \brief Get the tangential distortion coefficient p1. + /// \return Distortion coefficient p1. + public: double P1() const; + + /// \brief Get the tangential distortion coefficient p2. + /// \return Distortion coefficient p2. + public: double P2() const; + + /// \brief Get the distortion center. + /// \return Distortion center. + public: math::Vector2d Center() const; + + /// Documentation inherited + public: virtual void Print(std::ostream &_out) const override; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} + +#endif diff --git a/include/gz/sensors/CameraSensor.hh b/include/gz/sensors/CameraSensor.hh index 3f8647fc..b0dde263 100644 --- a/include/gz/sensors/CameraSensor.hh +++ b/include/gz/sensors/CameraSensor.hh @@ -23,9 +23,7 @@ #include -#include #include -#include #ifdef _WIN32 #pragma warning(push) @@ -95,7 +93,8 @@ namespace ignition /// \brief Force the sensor to generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Set a callback to be called when image frame data is /// generated. @@ -139,6 +138,21 @@ namespace ignition /// \return The distance from the 1st camera, in meters. public: double Baseline() const; + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + + /// \brief Check if there are any image subscribers + /// \return True if there are image subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Harmonic + public: bool HasImageConnections() const; + + /// \brief Check if there are any info subscribers + /// \return True if there are info subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Harmonic + public: bool HasInfoConnections() const; + /// \brief Advertise camera info topic. /// \return True if successful. protected: bool AdvertiseInfo(); @@ -157,7 +171,8 @@ namespace ignition /// \brief Publish camera info message. /// \param[in] _now The current time - protected: void PublishInfo(const gz::common::Time &_now); + protected: void PublishInfo( + const std::chrono::steady_clock::duration &_now); /// \brief Create a camera in a scene /// \return True on success. diff --git a/include/gz/sensors/DepthCameraSensor.hh b/include/gz/sensors/DepthCameraSensor.hh index efc07109..4ebb5338 100644 --- a/include/gz/sensors/DepthCameraSensor.hh +++ b/include/gz/sensors/DepthCameraSensor.hh @@ -24,9 +24,7 @@ #include #include -#include #include -#include #ifdef _WIN32 #pragma warning(push) @@ -96,7 +94,8 @@ namespace ignition /// \brief Force the sensor to generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Force the sensor to generate data /// \param[in] _now The current time @@ -158,6 +157,21 @@ namespace ignition /// \return height of the image public: virtual double NearClip() const; + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + + /// \brief Check if there are any depth subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Harmonic + public: bool HasDepthConnections() const; + + /// \brief Check if there are any point subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Harmonic + public: bool HasPointConnections() const; + /// \brief Create a camera in a scene /// \return True on success. private: bool CreateCamera(); diff --git a/include/gz/sensors/Distortion.hh b/include/gz/sensors/Distortion.hh new file mode 100644 index 00000000..852c4bc9 --- /dev/null +++ b/include/gz/sensors/Distortion.hh @@ -0,0 +1,108 @@ +/* + * 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_SENSORS_DISTORTION_HH_ +#define IGNITION_SENSORS_DISTORTION_HH_ + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // Forward declarations + class DistortionPrivate; + + /// \class DistortionFactory Distortion.hh ignition/sensors/Distortion.hh + /// \brief Use this distortion manager for creating and loading distortion + /// models. + class IGNITION_SENSORS_VISIBLE DistortionFactory + { + /// \brief Load a distortion model based on the input sdf parameters and + /// sensor type. + /// \param[in] _sdf Distortion sdf parameters. + /// \param[in] _sensorType Type of sensor. This is currently used to + /// distinguish between image and non image sensors in order to create + /// the appropriate distortion model. + /// \return Pointer to the distortion model created. + public: static DistortionPtr NewDistortionModel(sdf::ElementPtr _sdf, + const std::string &_sensorType = ""); + + /// \brief Load a distortion model based on the input sdf parameters and + /// sensor type. + /// \param[in] _sdf Distortion sdf parameters. + /// \param[in] _sensorType Type of sensor. This is currently used to + /// distinguish between image and non image sensors in order to create + /// the appropriate distortion model. + /// \return Pointer to the distortion model created. + public: static DistortionPtr NewDistortionModel( + const sdf::Camera &_sdf, + const std::string &_sensorType = ""); + }; + + /// \brief Which distortion types we support + enum class IGNITION_SENSORS_VISIBLE DistortionType : int + { + NONE = 0, + CUSTOM = 1, + BROWN = 2 + }; + + /// \class Distortion Distortion.hh ignition/sensors/Distortion.hh + /// \brief Distortion models for sensor output signals. + class IGNITION_SENSORS_VISIBLE Distortion + { + /// \brief Constructor. This should not be called directly unless creating + /// an empty distortion model. Use DistortionFactory::NewDistortionModel + /// to instantiate a new distortion model. + /// \param[in] _type Type of distortion model. + /// \sa DistortionFactory::NewDistortionModel + public: explicit Distortion(DistortionType _type); + + /// \brief Destructor. + public: virtual ~Distortion(); + + /// \brief Load distortion parameters from sdf. + /// \param[in] _sdf SDF Distortion DOM object. + public: virtual void Load(const sdf::Camera &_sdf); + + /// \brief Accessor for DistortionType. + /// \return Type of distortion currently in use. + public: DistortionType Type() const; + + /// \brief Output information about the distortion model. + /// \param[in] _out Output stream + public: virtual void Print(std::ostream &_out) const; + + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/sensors/ForceTorqueSensor.hh b/include/gz/sensors/ForceTorqueSensor.hh new file mode 100644 index 00000000..68b16e29 --- /dev/null +++ b/include/gz/sensors/ForceTorqueSensor.hh @@ -0,0 +1,132 @@ +/* + * 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_SENSORS_FORCETORQUESENSOR_HH_ +#define IGNITION_SENSORS_FORCETORQUESENSOR_HH_ + +#include + +#include + +#include +#include + +#include + +#include +#include + +#include "ignition/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class ForceTorqueSensorPrivate; + + /// \brief Force Torque Sensor Class + /// + /// A force-torque Sensor that reports force and torque applied on a joint. + class IGNITION_SENSORS_FORCE_TORQUE_VISIBLE ForceTorqueSensor + : public Sensor + { + /// \brief constructor + public: ForceTorqueSensor(); + + /// \brief destructor + public: virtual ~ForceTorqueSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; + + /// \brief Get the current joint force. + /// \return The latest measured force. + public: math::Vector3d Force() const; + + /// \brief Set the force vector in sensor frame and where the force is + /// applied on the child (parent-to-child) + /// \param[in] _force force vector in newton. + public: void SetForce(const math::Vector3d &_force); + + /// \brief Get the current joint torque. + /// \return The latest measured torque. + public: math::Vector3d Torque() const; + + /// \brief Set the torque vector in sensor frame and where the torque is + /// applied on the child (parent-to-child) + /// \param[in] _torque torque vector in newton. + public: void SetTorque(const math::Vector3d &_torque); + + /// \brief Set the rotation of the joint parent relative to the sensor + /// frame. + /// \return The current rotation the parent in the sensor frame. + public: math::Quaterniond RotationParentInSensor() const; + + /// \brief Set the rotation of the joint parent relative to the sensor + /// frame. + /// \param[in] _rotParentInSensorRotation of parent in sensor frame. + public: void SetRotationParentInSensor( + const math::Quaterniond &_rotParentInSensor); + + /// \brief Set the rotation of the joint parent relative to the sensor + /// frame. + /// \return The current rotation the child in the sensor frame. + public: math::Quaterniond RotationChildInSensor() const; + + /// \brief Set the rotation of the joint child relative to the sensor + /// frame. + /// \param[in] _rotChildInSensor Rotation of child in sensor frame. + public: void SetRotationChildInSensor( + const math::Quaterniond &_rotChildInSensor); + + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/GpuLidarSensor.hh b/include/gz/sensors/GpuLidarSensor.hh index 140d3f42..c8a58208 100644 --- a/include/gz/sensors/GpuLidarSensor.hh +++ b/include/gz/sensors/GpuLidarSensor.hh @@ -69,7 +69,8 @@ namespace ignition /// \brief Force the sensor to generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Initialize values in the sensor /// \return True on success @@ -120,6 +121,11 @@ namespace ignition /// \return Vertical field of view. public: gz::math::Angle VFOV() const; + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + /// \brief Connect function pointer to internal GpuRays callback /// \return gz::common::Connection pointer public: virtual gz::common::ConnectionPtr ConnectNewLidarFrame( @@ -127,6 +133,12 @@ namespace ignition unsigned int _heighti, unsigned int _channels, const std::string &/*_format*/)> _subscriber) override; + /// \brief Connect function pointer to internal GpuRays callback + /// \return ignition::common::Connection pointer + private: void OnNewLidarFrame(const float *_scan, unsigned int _width, + unsigned int _heighti, unsigned int _channels, + const std::string &_format); + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Data pointer for private data /// \internal diff --git a/include/gz/sensors/ImageBrownDistortionModel.hh b/include/gz/sensors/ImageBrownDistortionModel.hh new file mode 100644 index 00000000..aee103c9 --- /dev/null +++ b/include/gz/sensors/ImageBrownDistortionModel.hh @@ -0,0 +1,76 @@ +/* + * 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_SENSORS_IMAGEBROWNDISTORTIONMODEL_HH_ +#define IGNITION_SENSORS_IMAGEBROWNDISTORTIONMODEL_HH_ + +#include + +// TODO(WilliamLewww): Remove these pragmas once ign-rendering is disabling the +// warnings +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include "ignition/sensors/config.hh" +#include "ignition/sensors/BrownDistortionModel.hh" +#include "ignition/sensors/rendering/Export.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // Forward declarations + class ImageBrownDistortionModelPrivate; + + /** \class ImageBrownDistortionModel ImageBrownDistortionModel.hh \ + ignition/sensors/ImageBrownDistortionModel.hh + **/ + /// \brief Distortion Model class for image sensors + class IGNITION_SENSORS_RENDERING_VISIBLE ImageBrownDistortionModel : + public BrownDistortionModel + { + /// \brief Constructor. + public: ImageBrownDistortionModel(); + + /// \brief Destructor. + public: virtual ~ImageBrownDistortionModel(); + + // Documentation inherited. + public: virtual void Load(const sdf::Camera &_sdf) override; + + // Documentation inherited. + public: virtual void SetCamera(rendering::CameraPtr _camera); + + /// Documentation inherited + public: virtual void Print(std::ostream &_out) const override; + + /// \brief Private data pointer. + private: ImageBrownDistortionModelPrivate *dataPtr = nullptr; + }; + } + } +} + +#endif diff --git a/include/gz/sensors/ImageDistortion.hh b/include/gz/sensors/ImageDistortion.hh new file mode 100644 index 00000000..0e39b8c7 --- /dev/null +++ b/include/gz/sensors/ImageDistortion.hh @@ -0,0 +1,67 @@ +/* + * 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_SENSORS_IMAGEDISTORTION_HH_ +#define IGNITION_SENSORS_IMAGEDISTORTION_HH_ + +#include + +#include + +#include "ignition/sensors/config.hh" +#include "ignition/sensors/SensorTypes.hh" +#include "ignition/sensors/rendering/Export.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // Forward declarations + class DistortionPrivate; + + /// \class ImageDistortionFactory ImageDistortion.hh + /// ignition/sensors/ImageDistortion.hh + /// \brief Use this distortion manager for creating and loading distortion + /// models. + class IGNITION_SENSORS_RENDERING_VISIBLE ImageDistortionFactory + { + /// \brief Load a distortion model based on the input sdf parameters and + /// sensor type. + /// \param[in] _sdf Distortion sdf parameters. + /// \param[in] _sensorType Type of sensor. This is currently used to + /// distinguish between image and non image sensors in order to create + /// the appropriate distortion model. + /// \return Pointer to the distortion model created. + public: static DistortionPtr NewDistortionModel(sdf::ElementPtr _sdf, + const std::string &_sensorType = ""); + + /// \brief Load a distortion model based on the input sdf parameters and + /// sensor type. + /// \param[in] _sdf Distortion sdf parameters. + /// \param[in] _sensorType Type of sensor. This is currently used to + /// distinguish between image and non image sensors in order to create + /// the appropriate distortion model. + /// \return Pointer to the distortion model created. + public: static DistortionPtr NewDistortionModel(const sdf::Camera &_sdf, + const std::string &_sensorType = ""); + }; + } + } +} +#endif diff --git a/include/gz/sensors/ImuSensor.hh b/include/gz/sensors/ImuSensor.hh index 08bc381d..d71645f1 100644 --- a/include/gz/sensors/ImuSensor.hh +++ b/include/gz/sensors/ImuSensor.hh @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -34,9 +33,30 @@ namespace ignition { namespace sensors { - // Inline bracket to help doxygen filtering. + /// Inline bracket to help doxygen filtering. inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // + + /// \brief Reference frames enum + enum class IGNITION_SENSORS_VISIBLE WorldFrameEnumType + { + /// \brief NONE : To be used only when + /// reference orientation tag is empty. + NONE = 0, + + /// \brief ENU (East-North-Up): x - East, y - North, z - Up. + ENU = 1, + + /// \brief NED (North-East-Down): x - North, y - East, z - Down. + NED = 2, + + /// \brief NWU (North-West-Up): x - North, y - West, z - Up. + NWU = 3, + + /// \brief CUSTOM : The frame is described using custom_rpy tag. + CUSTOM = 4 + }; + + /// /// \brief forward declarations class ImuSensorPrivate; @@ -69,7 +89,8 @@ namespace ignition /// \brief Update the sensor and generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Set the angular velocity of the imu /// \param[in] _angularVel Angular velocity of the imu in body frame @@ -111,17 +132,43 @@ namespace ignition public: math::Quaterniond OrientationReference() const; /// \brief Get the orienation of the imu with respect to reference frame - /// \return Orientation in reference frame + /// \return Orientation in reference frame. If orientation is not + /// enabled, this will return the last computed orientation before + /// orientation is disabled or identity Quaternion if orientation has + /// never been enabled. public: math::Quaterniond Orientation() const; /// \brief Set the gravity vector /// \param[in] _gravity gravity vector in meters per second squared. public: void SetGravity(const math::Vector3d &_gravity); + /// \brief Set whether to output orientation. Not all imu's generate + /// orientation values as they use filters to produce orientation + /// estimates. + /// \param[in] _enabled True to publish orientation data, false to leave + /// the message field empty. + public: void SetOrientationEnabled(bool _enabled); + + /// \brief Get whether or not orientation is enabled. + /// \return True if orientation is enabled, false otherwise. + public: bool OrientationEnabled() const; + /// \brief Get the gravity vector /// \return Gravity vectory in meters per second squared. public: math::Vector3d Gravity() const; + /// \brief Specify the rotation offset of the coordinates of the World + /// frame relative to a geo-referenced frame + /// \param[in] _rot rotation offset + /// \param[in] _relativeTo world frame orientation, ENU by default + public: void SetWorldFrameOrientation( + const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo); + + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Data pointer for private data /// \internal diff --git a/include/gz/sensors/Lidar.hh b/include/gz/sensors/Lidar.hh index 9e7cf93f..634851e2 100644 --- a/include/gz/sensors/Lidar.hh +++ b/include/gz/sensors/Lidar.hh @@ -57,7 +57,8 @@ namespace ignition /// \brief Force the sensor to generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Apply noise to the laser buffer, if noise has been /// configured. This should be called before PublishLidarScan if you @@ -67,7 +68,8 @@ namespace ignition /// \brief Publish LaserScan message /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool PublishLidarScan(const common::Time &_now); + public: virtual bool PublishLidarScan( + const std::chrono::steady_clock::duration &_now); /// \brief Load the sensor based on data from an sdf::Sensor object. /// \param[in] _sdf SDF Sensor parameters. @@ -234,6 +236,15 @@ namespace ignition // Documentation inherited public: virtual bool IsActive() const; + /// \brief Check if there are any subscribers for sensor data + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + + /// \brief Get the visibility mask + /// \return Visibility mask + public: uint32_t VisibilityMask() const; + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Just a mutex for thread safety public: mutable std::mutex lidarMutex; diff --git a/include/gz/sensors/LogicalCameraSensor.hh b/include/gz/sensors/LogicalCameraSensor.hh index c68fe74a..e264318a 100644 --- a/include/gz/sensors/LogicalCameraSensor.hh +++ b/include/gz/sensors/LogicalCameraSensor.hh @@ -23,9 +23,7 @@ #include -#include #include -#include #include @@ -80,7 +78,8 @@ namespace ignition /// \brief Force the sensor to generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Get the near distance. This is the distance from the /// frustum's vertex to the closest plane. @@ -107,6 +106,11 @@ namespace ignition /// \return The frustum's aspect ratio. public: double AspectRatio() const; + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + /// \brief Get the latest image. An image is an instance of /// msgs::LogicalCameraImage, which contains a list of detected models. /// \return List of detected models. diff --git a/include/gz/sensors/MagnetometerSensor.hh b/include/gz/sensors/MagnetometerSensor.hh index c1a72b04..84d28626 100644 --- a/include/gz/sensors/MagnetometerSensor.hh +++ b/include/gz/sensors/MagnetometerSensor.hh @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -69,7 +68,8 @@ namespace ignition /// \brief Update the sensor and generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Set the world pose of the sensor /// \param[in] _pose Pose in world frame @@ -91,6 +91,11 @@ namespace ignition /// \return Magnetic field vector in body frame public: math::Vector3d MagneticField() const; + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Data pointer for private data /// \internal diff --git a/include/gz/sensors/Manager.hh b/include/gz/sensors/Manager.hh index 915d9869..8c5671f2 100644 --- a/include/gz/sensors/Manager.hh +++ b/include/gz/sensors/Manager.hh @@ -19,15 +19,16 @@ #include #include +#include #include #include #include #include -#include #include #include #include #include +#include namespace ignition { @@ -61,89 +62,37 @@ namespace ignition /// \return True if successfully initialized, false if not public: bool Init(); - /// \brief Create a sensor from SDF with a known sensor type. - /// - /// This creates sensors by looking at the given sdf element. - /// Sensors created with this API offer an ignition-transport interface. - /// If you need a direct C++ interface to the data, you must get the - /// sensor pointer and cast to the correct type. - /// - /// A tag may have multiple tags. A SensorId will be - /// returned for each plugin that is described in SDF. - /// If there are no tags then one of the plugins shipped with - /// this library will be loaded. For example, a tag with - /// but no will load a CameraSensor from - /// ignition-sensors-camera. + /// \brief Create a sensor from an SDF ovject with a known sensor type. /// \sa Sensor() - /// \param[in] _sdf pointer to the sdf element - /// \return A pointer to the created sensor. nullptr returned on - /// error. - public: template - T *CreateSensor(sdf::Sensor _sdf) + /// \param[in] _sdf An SDF element or DOM object. + /// \tparam SensorType Sensor type + /// \tparam SdfType It may be an `sdf::ElementPtr` containing a sensor or + /// an `sdf::Sensor`. + /// \return A pointer to the created sensor. Null returned on + /// error. The Manager keeps ownership of the pointer's lifetime. + public: template + SensorType *CreateSensor(SdfType _sdf) { - gz::sensors::SensorId id = this->CreateSensor(_sdf); - - if (id != NO_SENSOR) + SensorFactory sensorFactory; + auto sensor = sensorFactory.CreateSensor(_sdf); + if (nullptr == sensor) { - T *result = dynamic_cast(this->Sensor(id)); - - if (!result) - ignerr << "SDF sensor type does not match template type\n"; - - return result; + ignerr << "Failed to create sensor." << std::endl; + return nullptr; } - - ignerr << "Failed to create sensor of type[" - << _sdf.TypeStr() << "]\n"; - return nullptr; - } - - /// \brief Create a sensor from SDF with a known sensor type. - /// - /// This creates sensors by looking at the given sdf element. - /// Sensors created with this API offer an ignition-transport interface. - /// If you need a direct C++ interface to the data, you must get the - /// sensor pointer and cast to the correct type. - /// - /// A tag may have multiple tags. A SensorId will be - /// returned for each plugin that is described in SDF. - /// If there are no tags then one of the plugins shipped with - /// this library will be loaded. For example, a tag with - /// but no will load a CameraSensor from - /// ignition-sensors-camera. - /// \sa Sensor() - /// \param[in] _sdf pointer to the sdf element - /// \return A pointer to the created sensor. nullptr returned on - /// error. - public: template - T *CreateSensor(sdf::ElementPtr _sdf) - { - gz::sensors::SensorId id = this->CreateSensor(_sdf); - - if (id != NO_SENSOR) + auto result = sensor.get(); + if (NO_SENSOR == this->AddSensor(std::move(sensor))) { - T *result = dynamic_cast(this->Sensor(id)); - - if (nullptr == result) - { - ignerr << "Failed to create sensor [" << id << "] of type [" - << _sdf->Get("type") - << "]. SDF sensor type does not match template type." - << std::endl; - } - - return result; + ignerr << "Failed to add sensor." << std::endl; + return nullptr; } - - ignerr << "Failed to create sensor of type [" - << _sdf->Get("type") << "]\n"; - return nullptr; + return result; } /// \brief Create a sensor from SDF without a known sensor type. /// /// This creates sensors by looking at the given sdf element. - /// Sensors created with this API offer an ignition-transport interface. + /// Sensors created with this API offer an gz-transport interface. /// If you need a direct C++ interface to the data, you must get the /// sensor pointer and cast to the correct type. /// @@ -152,17 +101,21 @@ namespace ignition /// If there are no tags then one of the plugins shipped with /// this library will be loaded. For example, a tag with /// but no will load a CameraSensor from - /// ignition-sensors-camera. + /// gz-sensors-camera. /// \sa Sensor() /// \param[in] _sdf pointer to the sdf element /// \return A sensor id that refers to the created sensor. NO_SENSOR /// is returned on erro. - public: gz::sensors::SensorId CreateSensor(sdf::ElementPtr _sdf); + /// \deprecated Sensor registration is deprecated, so it's necessary to + /// provide the specific sensor type to create it. Use the templated + /// `CreateSensor` function. + public: gz::sensors::SensorId IGN_DEPRECATED(6) CreateSensor( + sdf::ElementPtr _sdf); /// \brief Create a sensor from SDF without a known sensor type. /// /// This creates sensors by looking at the given sdf element. - /// Sensors created with this API offer an ignition-transport interface. + /// Sensors created with this API offer an gz-transport interface. /// If you need a direct C++ interface to the data, you must get the /// sensor pointer and cast to the correct type. /// @@ -171,13 +124,23 @@ namespace ignition /// If there are no tags then one of the plugins shipped with /// this library will be loaded. For example, a tag with /// but no will load a CameraSensor from - /// ignition-sensors-camera. + /// gz-sensors-camera. /// \sa Sensor() /// \param[in] _sdf SDF sensor DOM object /// \return A sensor id that refers to the created sensor. NO_SENSOR /// is returned on erro. - public: gz::sensors::SensorId CreateSensor(const sdf::Sensor &_sdf); + /// \deprecated Sensor registration is deprecated, so it's necessary to + /// provide the specific sensor type to create it. Use the templated + /// `CreateSensor` function. + public: gz::sensors::SensorId IGN_DEPRECATED(6) CreateSensor( + const sdf::Sensor &_sdf); + /// \brief Add a sensor for this manager to manage. + /// \sa Sensor() + /// \param[in] _sensor Pointer to the sensor + /// \return A sensor id that refers to the created sensor. NO_SENSOR + /// is returned on error. + public: SensorId AddSensor(std::unique_ptr _sensor); /// \brief Get an instance of a loaded sensor by sensor id /// \param[in] _id Idenitifier of the sensor. @@ -194,17 +157,11 @@ namespace ignition /// \param _time: The current simulated time /// \param _force: If true, all sensors are forced to update. Otherwise /// a sensor will update based on it's Hz rate. - public: void RunOnce(const gz::common::Time &_time, + public: void RunOnce(const std::chrono::steady_clock::duration &_time, bool _force = false); /// \brief Adds colon delimited paths sensor plugins may be - public: void AddPluginPaths(const std::string &_path); - - /// \brief load a plugin and return a shared_ptr - /// \param[in] _filename Sensor plugin file to load. - /// \return Pointer to the new sensor, nullptr on error. - private: gz::sensors::SensorId LoadSensorPlugin( - const std::string &_filename, sdf::ElementPtr _sdf); + public: void IGN_DEPRECATED(6) AddPluginPaths(const std::string &_path); IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief private data pointer diff --git a/include/gz/sensors/NavSatSensor.hh b/include/gz/sensors/NavSatSensor.hh new file mode 100644 index 00000000..d80e5ba4 --- /dev/null +++ b/include/gz/sensors/NavSatSensor.hh @@ -0,0 +1,133 @@ +/* + * 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_SENSORS_NAVSAT_HH_ +#define IGNITION_SENSORS_NAVSAT_HH_ + +#include + +#include +#include + +#include "gz/sensors/config.hh" +#include "gz/sensors/navsat/Export.hh" + +#include "ignition/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class NavSatPrivate; + + /// \brief NavSat Sensor Class + /// + /// A sensor that reports position and velocity readings over + /// Ignition Transport using spherical coordinates (latitude / longitude). + /// + /// By default, it publishes `ignition::msgs::NavSat` messages on the + /// `/.../navsat` topic. + /// + /// This sensor assumes the world is using the East-North-Up (ENU) frame. + class IGNITION_SENSORS_NAVSAT_VISIBLE NavSatSensor : public Sensor + { + /// \brief Constructor + public: NavSatSensor(); + + /// \brief Destructor + public: virtual ~NavSatSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; + + /// \brief Set the latitude of the NavSat + /// \param[in] _latitude Latitude of NavSat + public: void SetLatitude(const math::Angle &_latitude); + + /// \brief Get the latitude of the NavSat, wrapped between +/- 180 + /// degrees. + /// \return Latitude angle. + public: const math::Angle &Latitude() const; + + /// \brief Set the longitude of the NavSat + /// \param[in] _longitude Longitude of NavSat + public: void SetLongitude(const math::Angle &_longitude); + + /// \brief Get the longitude of the NavSat, wrapped between +/- 180 + /// degrees. + /// \return Longitude angle. + public: const math::Angle &Longitude() const; + + /// \brief Set the altitude of the NavSat + /// \param[in] _altitude altitude of NavSat in meters + public: void SetAltitude(double _altitude); + + /// \brief Get NavSat altitude above sea level + /// \return Altitude in meters + public: double Altitude() const; + + /// \brief Set the velocity of the NavSat in ENU world frame. + /// \param[in] _vel NavSat in meters per second. + public: void SetVelocity(const math::Vector3d &_vel); + + /// \brief Get the velocity of the NavSat sensor in the ENU world frame. + /// \return Velocity in meters per second + public: const math::Vector3d &Velocity() const; + + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + + /// \brief Easy short hand for setting the position of the sensor. + /// \param[in] _latitude Latitude angle. + /// \param[in] _longitude Longitude angle. + /// \param[in] _altitude Altitude in meters; defaults to zero. + public: void SetPosition(const math::Angle &_latitude, + const math::Angle &_longitude, double _altitude = 0.0); + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/RgbdCameraSensor.hh b/include/gz/sensors/RgbdCameraSensor.hh index f57c89a7..1484f4bb 100644 --- a/include/gz/sensors/RgbdCameraSensor.hh +++ b/include/gz/sensors/RgbdCameraSensor.hh @@ -22,7 +22,6 @@ #include #include -#include #include "gz/sensors/CameraSensor.hh" #include "gz/sensors/config.hh" @@ -63,6 +62,11 @@ namespace ignition /// \return true if loading was successful public: virtual bool Load(const sdf::Sensor &_sdf) override; + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + /// \brief Initialize values in the sensor /// \return True on success public: virtual bool Init() override; @@ -70,7 +74,8 @@ namespace ignition /// \brief Force the sensor to generate data /// \param[in] _now The current time /// \return true if the update was successful - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Set the rendering scene. /// \param[in] _scene Pointer to the scene @@ -85,6 +90,26 @@ namespace ignition /// \return height of the image public: virtual unsigned int ImageHeight() const override; + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + + /// \brief Check if there are color subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Harmonic + public: bool HasColorConnections() const; + + /// \brief Check if there are depth subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Harmonic + public: bool HasDepthConnections() const; + + /// \brief Check if there are point cloud subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Harmonic + public: bool HasPointConnections() const; + /// \brief Create an RGB camera and a depth camera. /// \return True on success. private: bool CreateCameras(); diff --git a/include/gz/sensors/SegmentationCameraSensor.hh b/include/gz/sensors/SegmentationCameraSensor.hh new file mode 100644 index 00000000..4bed152f --- /dev/null +++ b/include/gz/sensors/SegmentationCameraSensor.hh @@ -0,0 +1,141 @@ +/* + * 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_SENSORS_SEGMENTATIONCAMERASENSOR_HH_ +#define IGNITION_SENSORS_SEGMENTATIONCAMERASENSOR_HH_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ignition/sensors/CameraSensor.hh" +#include "ignition/sensors/Export.hh" +#include "ignition/sensors/Sensor.hh" + +#include "ignition/sensors/segmentation_camera/Export.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // forward declarations + class SegmentationCameraSensorPrivate; + + /// \brief Segmentation camera sensor class. + /// + /// This class creates segmentation images from an ignition rendering scene. + /// The scene must be created in advance and given to Manager::Init(). + /// It offers both an ignition-transport interface and a direct C++ API + /// to access the image data. The API works by setting a callback to be + /// called with image data. + class IGNITION_SENSORS_SEGMENTATION_CAMERA_VISIBLE + SegmentationCameraSensor : public CameraSensor + { + /// \brief constructor + public: SegmentationCameraSensor(); + + /// \brief destructor + public: virtual ~SegmentationCameraSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successful + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; + + /// \brief Get the rendering segmentation camera + /// \return Segmentation camera pointer + public: rendering::SegmentationCameraPtr SegmentationCamera() const; + + /// \brief Segmentation data callback used to get the data from the sensor + /// \param[in] _data pointer to the data from the sensor + /// \param[in] _width width of the segmentation image + /// \param[in] _height height of the segmentation image + /// \param[in] _channels num of channels + /// \param[in] _format string with the format + public: void OnNewSegmentationFrame(const uint8_t * _data, + unsigned int _width, unsigned int _height, unsigned int _channels, + const std::string &_format); + + /// \brief Set a callback to be called when image frame data is + /// generated. + /// \param[in] _callback This callback will be called every time the + /// camera produces image data. The Update function will be blocked + /// while the callbacks are executed. + /// \remark Do not block inside of the callback. + /// \return A connection pointer that must remain in scope. When the + /// connection pointer falls out of scope, the connection is broken. + public: ignition::common::ConnectionPtr ConnectImageCallback( + std::function _callback); + + /// \brief Set the rendering scene. + /// \param[in] _scene Pointer to the scene + public: virtual void SetScene( + ignition::rendering::ScenePtr _scene) override; + + /// \brief Get image width. + /// \return width of the image + public: virtual unsigned int ImageWidth() const override; + + /// \brief Get image height. + /// \return height of the image + public: virtual unsigned int ImageHeight() const override; + + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + + /// \brief Create a camera in a scene + /// \return True on success. + private: bool CreateCamera(); + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/Sensor.hh b/include/gz/sensors/Sensor.hh index 50b00cf0..66703f9f 100644 --- a/include/gz/sensors/Sensor.hh +++ b/include/gz/sensors/Sensor.hh @@ -32,7 +32,6 @@ #include #include -#include #include #include #include @@ -76,6 +75,9 @@ namespace ignition public: virtual bool Load(sdf::ElementPtr _sdf); /// \brief Initialize values in the sensor + /// This will set the next update time to zero. This is particularly + /// useful if simulation time has jumped backward, for example during + /// a seek backward in a log file. public: virtual bool Init(); /// \brief Force the sensor to generate data @@ -91,10 +93,11 @@ namespace ignition /// \param[in] _now The current time /// \return true if the update was successfull /// \sa SetUpdateRate() - public: virtual bool Update(const common::Time &_now) = 0; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) = 0; /// \brief Return the next time the sensor will generate data - public: common::Time NextUpdateTime() const; + public: std::chrono::steady_clock::duration NextDataUpdateTime() const; /// \brief Update the sensor. /// @@ -105,11 +108,12 @@ namespace ignition /// \param[in] _force Force the update to happen even if it's not time /// \return True if the update was triggered (_force was true or _now /// >= next_update_time) and the sensor's - /// bool Sensor::Update(const common::Time &_now) function returned true. + /// bool Sensor::Update(std::chrono::steady_clock::time_point) + /// function returned true. /// False otherwise. /// \remarks If forced the NextUpdateTime() will be unchanged. - /// \sa virtual bool Update(const common::Time &_name) = 0 - public: bool Update(const common::Time &_now, const bool _force); + public: bool Update( + const std::chrono::steady_clock::duration &_now, const bool _force); /// \brief Get the update rate of the sensor. /// @@ -203,6 +207,19 @@ namespace ignition public: void PublishMetrics( const std::chrono::duration &_now); + /// \brief Get whether the sensor is enabled or not + /// \return True if the sensor is active, false otherwise. + /// \sa SetActive + public: bool IsActive() const; + + /// \brief Enable or disable the sensor. Disabled sensors will not + /// generate or publish data unless Update is called with the + /// '_force' argument set to true. + /// \param[in] _active True to set the sensor to be active, + /// false to disable the sensor. + /// \sa IsActive + public: void SetActive(bool _active); + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \internal /// \brief Data pointer for private data @@ -212,5 +229,4 @@ namespace ignition } } } - #endif diff --git a/include/gz/sensors/SensorFactory.hh b/include/gz/sensors/SensorFactory.hh index ea3d11b7..2089fe5a 100644 --- a/include/gz/sensors/SensorFactory.hh +++ b/include/gz/sensors/SensorFactory.hh @@ -23,7 +23,6 @@ #include #include -#include #include #include @@ -41,32 +40,35 @@ namespace ignition class SensorFactoryPrivate; /// \brief Base sensor plugin interface + /// \deprecated Sensor plugins are deprecated. Instantiate sensor objects + /// instead. class IGNITION_SENSORS_VISIBLE SensorPlugin { - /// \brief Allows using shorter APIS in common::PluginLoader - public: IGN_COMMON_SPECIALIZE_INTERFACE(gz::sensors::SensorPlugin) - /// \brief Instantiate new sensor /// \return New sensor - public: virtual Sensor *New() = 0; + public: virtual Sensor IGN_DEPRECATED(6) * New() = 0; }; /// \brief Templated class for instantiating sensors of the specified type /// \tparam Type of sensor being instantiated. + /// \deprecated Sensor plugins are deprecated. Instantiate sensor objects + /// instead. template class SensorTypePlugin : public SensorPlugin { // Documentation inherited - public: SensorType *New() override + public: SensorType IGN_DEPRECATED(6) * New() override { return new SensorType(); }; }; /// \brief A factory class for creating sensors - /// This class wll load a sensor plugin based on the given sensor type and - /// instantiates a sensor object - /// + /// This class instantiates sensor objects based on the sensor type and + /// makes sure they're initialized correctly. + // After removing plugin functionality, the sensor factory class doesn't + // hold any internal state. Consider converting the functionality in this + // class to helper functions. class IGNITION_SENSORS_VISIBLE SensorFactory { /// \brief Constructor @@ -78,79 +80,103 @@ namespace ignition /// \brief Create a sensor from a SDF DOM object with a known sensor type. /// /// This creates sensors by looking at the given SDF DOM object. - /// Sensors created with this API offer an ignition-transport interface. + /// Sensors created with this API offer an gz-transport interface. /// If you need a direct C++ interface to the data, you must get the /// sensor pointer and cast to the correct type. /// /// \sa Sensor() /// \param[in] _sdf SDF Sensor DOM object. - /// \return A pointer to the created sensor. nullptr returned on - /// error. - public: template - std::unique_ptr CreateSensor(const sdf::Sensor &_sdf) + /// \tparam SensorType Sensor type + /// \return A pointer to the created sensor. Null returned on error. + public: template + std::unique_ptr CreateSensor(const sdf::Sensor &_sdf) { - auto sensor = SensorFactory::CreateSensor(_sdf); + auto sensor = std::make_unique(); - if (sensor) + if (nullptr == sensor) { - std::unique_ptr result( - dynamic_cast(sensor.release())); + ignerr << "Failed to create sensor [" << _sdf.Name() + << "] of type[" << _sdf.TypeStr() << "]" << std::endl; + return nullptr; + } - if (!result) - ignerr << "SDF sensor type does not match template type\n"; + if (!sensor->Load(_sdf)) + { + ignerr << "Failed to load sensor [" << _sdf.Name() + << "] of type[" << _sdf.TypeStr() << "]" << std::endl; + return nullptr; + } - return result; + if (!sensor->Init()) + { + ignerr << "Failed to initialize sensor [" << _sdf.Name() + << "] of type[" << _sdf.TypeStr() << "]" << std::endl; + return nullptr; } - ignerr << "Failed to create sensor of type[" - << _sdf.TypeStr() << "]\n"; - return nullptr; + return sensor; } - /// \brief Create a sensor from SDF with a known sensor type. - /// - /// This creates sensors by looking at the given sdf element. - /// Sensors created with this API offer an ignition-transport interface. - /// If you need a direct C++ interface to the data, you must get the - /// sensor pointer and cast to the correct type. - /// + /// \brief Create a sensor from an SDF element with a known sensor type. /// \sa Sensor() /// \param[in] _sdf pointer to the sdf element - /// \return A pointer to the created sensor. nullptr returned on + /// \tparam SensorType Sensor type + /// \return A pointer to the created sensor. Null returned on /// error. - public: template - std::unique_ptr CreateSensor(sdf::ElementPtr _sdf) + public: template + std::unique_ptr CreateSensor(sdf::ElementPtr _sdf) { - auto sensor = SensorFactory::CreateSensor(_sdf); + if (nullptr == _sdf) + { + ignerr << "Failed to create sensor, received null SDF " + << "pointer." << std::endl; + return nullptr; + } + + auto sensor = std::make_unique(); + + auto type = _sdf->Get("type"); + auto name = _sdf->Get("name"); - if (sensor) + if (nullptr == sensor) { - std::unique_ptr result( - dynamic_cast(sensor.release())); + ignerr << "Failed to create sensor [" << name + << "] of type[" << type << "]" << std::endl; + return nullptr; + } - if (!result) - ignerr << "SDF sensor type does not match template type\n"; + if (!sensor->Load(_sdf)) + { + ignerr << "Failed to load sensor [" << name + << "] of type[" << type << "]" << std::endl; + return nullptr; + } - return result; + if (!sensor->Init()) + { + ignerr << "Failed to initialize sensor [" << name + << "] of type[" << type << "]" << std::endl; + return nullptr; } - ignerr << "Failed to create sensor of type[" - << _sdf->Get("type") << "]\n"; - return nullptr; + return sensor; } /// \brief Create a sensor from SDF without a known sensor type. /// /// This creates sensors by looking at the given sdf element. - /// Sensors created with this API offer an ignition-transport interface. + /// Sensors created with this API offer an gz-transport interface. /// If you need a direct C++ interface to the data, you must get the /// sensor pointer and cast to the correct type. /// /// \sa Sensor() /// \param[in] _sdf pointer to the sdf element - /// \return A sensor id that refers to the created sensor. Null is - /// is returned on error. - public: std::unique_ptr CreateSensor(sdf::ElementPtr _sdf); + /// \return Null, as the function is deprecated. + /// \deprecated Sensor registration is deprecated, so it's necessary to + /// provide the specific sensor type to create it. Use the templated + /// `CreateSensor` function. + public: std::unique_ptr IGN_DEPRECATED(6) CreateSensor( + sdf::ElementPtr _sdf); /// \brief Create a sensor from an SDF Sensor DOM object without a known /// sensor type. @@ -165,29 +191,22 @@ namespace ignition /// \param[in] _sdf SDF Sensor DOM object. /// \return A sensor id that refers to the created sensor. Null is /// is returned on error. - public: std::unique_ptr CreateSensor(const sdf::Sensor &_sdf); + /// \deprecated Sensor registration is deprecated, so it's necessary to + /// provide the specific sensor type to create it. Use the templated + /// `CreateSensor` function. + public: std::unique_ptr IGN_DEPRECATED(6) CreateSensor( + const sdf::Sensor &_sdf); /// \brief Add additional path to search for sensor plugins /// \param[in] _path Search path - public: void AddPluginPaths(const std::string &_path); - - /// \brief load a plugin and return a pointer - /// \param[in] _filename Sensor plugin file to load. - /// \return Pointer to the new sensor, nullptr on error. - private: std::shared_ptr LoadSensorPlugin( - const std::string &_filename); + /// \deprecated Sensor plugins aren't supported anymore. + public: void IGN_DEPRECATED(6) AddPluginPaths(const std::string &_path); IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief private data pointer private: std::unique_ptr dataPtr; IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING }; - - /// \brief Sensor registration macro - #define IGN_SENSORS_REGISTER_SENSOR(classname) \ - IGN_COMMON_REGISTER_SINGLE_PLUGIN(\ - gz::sensors::SensorTypePlugin, \ - gz::sensors::SensorPlugin) } } } diff --git a/include/gz/sensors/SensorTypes.hh b/include/gz/sensors/SensorTypes.hh index b75d0897..48c52f28 100644 --- a/include/gz/sensors/SensorTypes.hh +++ b/include/gz/sensors/SensorTypes.hh @@ -42,6 +42,9 @@ namespace ignition class GaussianNoiseModel; class ImageGaussianNoiseModel; class Noise; + class Distortion; + class BrownDistortionModel; + class ImageBrownDistortionModel; class Sensor; /// \def SensorPtr @@ -68,6 +71,19 @@ namespace ignition typedef std::shared_ptr ImageGaussianNoiseModelPtr; + /// \def DistortionPtr + /// \brief Shared pointer to Distortion + typedef std::shared_ptr DistortionPtr; + + /// \def DistortionPtr + /// \brief Shared pointer to Distortion + typedef std::shared_ptr BrownDistortionModelPtr; + + /// \def DistortionPtr + /// \brief Shared pointer to Distortion + typedef std::shared_ptr + ImageBrownDistortionModelPtr; + /// \def Sensor_V /// \brief Vector of Sensor shared pointers typedef std::vector Sensor_V; @@ -149,6 +165,46 @@ namespace ignition /// \sa Lidar LIDAR_NOISE = 14, + /// \brief Force body-frame X axis noise in N + /// \sa ForceTorqueSensor + FORCE_X_NOISE_N = 15, + + /// \brief Force body-frame Y axis noise in N + /// \sa ForceTorqueSensor + FORCE_Y_NOISE_N = 16, + + /// \brief Force body-frame Z axis noise in N + /// \sa ForceTorqueSensor + FORCE_Z_NOISE_N = 17, + + /// \brief Torque body-frame X axis noise in Nm + /// \sa ForceTorqueSensor + TORQUE_X_NOISE_N_M = 18, + + /// \brief Torque body-frame Y axis noise in Nm + /// \sa ForceTorqueSensor + TORQUE_Y_NOISE_N_M = 19, + + /// \brief Torque body-frame Z axis noise in Nm + /// \sa ForceTorqueSensor + TORQUE_Z_NOISE_N_M = 20, + + /// \brief Noise streams for the NavSat position sensor + /// \sa NavSat + NAVSAT_HORIZONTAL_POSITION_NOISE = 21, + + /// \brief Noise streams for the NavSat position sensor + /// \sa NavSat + NAVSAT_VERTICAL_POSITION_NOISE = 22, + + /// \brief Noise streams for the NavSat velocity sensor + /// \sa NavSat + NAVSAT_HORIZONTAL_VELOCITY_NOISE = 23, + + /// \brief Noise streams for the NavSat velocity sensor + /// \sa NavSat + NAVSAT_VERTICAL_VELOCITY_NOISE = 24, + /// \internal /// \brief Indicator used to create an iterator over the enum. Do not /// use this. @@ -156,6 +212,31 @@ namespace ignition }; /// \} + + /// \def SensorDistortionType + /// \brief Eumeration of all sensor noise types + enum SensorDistortionType + { + /// \internal + /// \brief Indicator used to create an iterator over the enum. Do not + /// use this. + SENSOR_DISTORTION_TYPE_BEGIN = 0, + + /// \brief Noise streams for the Camera sensor + /// \sa CameraSensor + NO_DISTORTION = SENSOR_DISTORTION_TYPE_BEGIN, + + /// \brief Noise streams for the Camera sensor + /// \sa CameraSensor + CAMERA_DISTORTION = 1, + + /// \internal + /// \brief Indicator used to create an iterator over the enum. Do not + /// use this. + SENSOR_DISTORTION_TYPE_END + }; + /// \} + /// \brief SensorCategory is used to categorize sensors. This is used to /// put sensors into different threads. enum SensorCategory diff --git a/include/gz/sensors/ThermalCameraSensor.hh b/include/gz/sensors/ThermalCameraSensor.hh index 7b1d550d..32017528 100644 --- a/include/gz/sensors/ThermalCameraSensor.hh +++ b/include/gz/sensors/ThermalCameraSensor.hh @@ -24,9 +24,7 @@ #include #include -#include #include -#include #ifdef _WIN32 #pragma warning(push) @@ -96,7 +94,8 @@ namespace ignition /// \brief Force the sensor to generate data /// \param[in] _now The current time /// \return true if the update was successfull - public: virtual bool Update(const common::Time &_now) override; + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; /// \brief Force the sensor to generate data /// \param[in] _now The current time @@ -162,7 +161,12 @@ namespace ignition /// \param[in] resolution Temperature linear resolution public: virtual void SetLinearResolution(float _resolution); - /// \brief Create a camera in a scene + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + + /// \brief Create a camera in a scene /// \return True on success. private: bool CreateCamera(); diff --git a/include/gz/sensors/Util.hh b/include/gz/sensors/Util.hh new file mode 100644 index 00000000..e9d2db43 --- /dev/null +++ b/include/gz/sensors/Util.hh @@ -0,0 +1,68 @@ +/* + * 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_SENSORS_UTIL_HH_ +#define IGNITION_SENSORS_UTIL_HH_ + +#include + +#include +#include + +#include "ignition/sensors/config.hh" +#include "ignition/sensors/Export.hh" + +namespace ignition +{ +namespace sensors +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { +// +/// \brief Get the name of a sensor's custom type from SDF. +/// +/// Given an SDF tag as follows: +/// +/// +/// +/// This function returns `sensor_type`. +/// +/// It will return an empty string if the element is malformed. For example, +/// if it misses the `ignition:type` attribute or is not of `type="custom"`. +/// +/// \param[in] _sdf Sensor SDF object. +/// \return _sensorType Name of sensor type. +std::string IGNITION_SENSORS_VISIBLE customType(const sdf::Sensor &_sdf); // NOLINT + +/// \brief Get the name of a sensor's custom type from SDF. +/// +/// Given an SDF tag as follows: +/// +/// +/// +/// This function returns `sensor_type`. +/// +/// It will return an empty string if the element is malformed. For example, +/// if it misses the `ignition:type` attribute or is not of `type="custom"`. +/// +/// \param[in] _sdf Sensor SDF object. +/// \return _sensorType Name of sensor type. +std::string IGNITION_SENSORS_VISIBLE customType(sdf::ElementPtr _sdf); // NOLINT +} +} +} + +#endif diff --git a/include/ignition/sensors/BoundingBoxCameraSensor.hh b/include/ignition/sensors/BoundingBoxCameraSensor.hh index 9dc77e18..672fbd86 100644 --- a/include/ignition/sensors/BoundingBoxCameraSensor.hh +++ b/include/ignition/sensors/BoundingBoxCameraSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. @@ -13,105 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_SENSORS_BOUNDINGBOXCAMERASENSOR_HH_ -#define IGNITION_SENSORS_BOUNDINGBOXCAMERASENSOR_HH_ - -#include -#include - -#include -#include - -#include "ignition/sensors/CameraSensor.hh" -#include "ignition/sensors/boundingbox_camera/Export.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // forward declarations - class BoundingBoxCameraSensorPrivate; - - /// \brief BoundingBox camera sensor class. - /// - /// This class creates a BoundingBox image from an ignition rendering scene. - /// The scene must be created in advance and given to Manager::Init(). - /// It offers both an ignition-transport interface and a direct C++ API - /// to access the image data. The API works by setting a callback to be - /// called with image data. - class IGNITION_SENSORS_BOUNDINGBOX_CAMERA_VISIBLE - BoundingBoxCameraSensor : public CameraSensor - { - /// \brief constructor - public: BoundingBoxCameraSensor(); - - /// \brief destructor - public: virtual ~BoundingBoxCameraSensor(); - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(sdf::ElementPtr _sdf) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Force the sensor to generate data - /// \param[in] _now The current time - /// \return true if the update was successful - public: virtual bool Update( - const std::chrono::steady_clock::duration &_now) override; - - /// \brief Get the rendering BoundingBox camera - /// \return BoundingBox camera pointer - public: virtual rendering::BoundingBoxCameraPtr - BoundingBoxCamera() const; - - /// \brief Callback on new bounding boxes from bounding boxes camera - /// \param[in] _boxes Detected bounding boxes from the camera - public: void OnNewBoundingBoxes( - const std::vector &_boxes); - - /// \brief Set the rendering scene. - /// \param[in] _scene Pointer to the scene - public: virtual void SetScene( - ignition::rendering::ScenePtr _scene) override; - - /// \brief Get image width. - /// \return width of the image - public: virtual unsigned int ImageWidth() const override; - - /// \brief Get image height. - /// \return height of the image - public: virtual unsigned int ImageHeight() const override; - - /// \brief Check if there are any subscribers - /// \return True if there are subscribers, false otherwise - /// \todo(iche033) Make this function virtual on Garden - public: bool HasConnections() const; - - /// \brief Create a camera in a scene - /// \return True on success. - private: bool CreateCamera(); - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/BrownDistortionModel.hh b/include/ignition/sensors/BrownDistortionModel.hh index 388c6d66..ed617035 100644 --- a/include/ignition/sensors/BrownDistortionModel.hh +++ b/include/ignition/sensors/BrownDistortionModel.hh @@ -13,75 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_SENSORS_BROWNDISTORTIONMODEL_HH_ -#define IGNITION_SENSORS_BROWNDISTORTIONMODEL_HH_ - -#include - -#include "ignition/sensors/Distortion.hh" -#include "ignition/sensors/Export.hh" -#include "ignition/sensors/config.hh" -#include "ignition/utils/ImplPtr.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - // Forward declarations - class BrownDistortionModelPrivate; - - /** \class BrownDistortionModel BrownDistortionModel.hh \ - ignition/sensors/BrownDistortionModel.hh - **/ - /// \brief Brown Distortion Model class - class IGNITION_SENSORS_VISIBLE BrownDistortionModel : public Distortion - { - /// \brief Constructor. - public: BrownDistortionModel(); - - /// \brief Destructor. - public: virtual ~BrownDistortionModel(); - - // Documentation inherited. - public: virtual void Load(const sdf::Camera &_sdf) override; - - /// \brief Get the radial distortion coefficient k1. - /// \return Distortion coefficient k1. - public: double K1() const; - - /// \brief Get the radial distortion coefficient k2. - /// \return Distortion coefficient k2. - public: double K2() const; - - /// \brief Get the radial distortion coefficient k3. - /// \return Distortion coefficient k3. - public: double K3() const; - - /// \brief Get the tangential distortion coefficient p1. - /// \return Distortion coefficient p1. - public: double P1() const; - - /// \brief Get the tangential distortion coefficient p2. - /// \return Distortion coefficient p2. - public: double P2() const; - - /// \brief Get the distortion center. - /// \return Distortion center. - public: math::Vector2d Center() const; - - /// Documentation inherited - public: virtual void Print(std::ostream &_out) const override; - - /// \brief Private data pointer. - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/Distortion.hh b/include/ignition/sensors/Distortion.hh index 852c4bc9..defb0c1f 100644 --- a/include/ignition/sensors/Distortion.hh +++ b/include/ignition/sensors/Distortion.hh @@ -13,96 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_SENSORS_DISTORTION_HH_ -#define IGNITION_SENSORS_DISTORTION_HH_ - -#include -#include -#include - -#include -#include +#include #include -#include - -#include - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // Forward declarations - class DistortionPrivate; - - /// \class DistortionFactory Distortion.hh ignition/sensors/Distortion.hh - /// \brief Use this distortion manager for creating and loading distortion - /// models. - class IGNITION_SENSORS_VISIBLE DistortionFactory - { - /// \brief Load a distortion model based on the input sdf parameters and - /// sensor type. - /// \param[in] _sdf Distortion sdf parameters. - /// \param[in] _sensorType Type of sensor. This is currently used to - /// distinguish between image and non image sensors in order to create - /// the appropriate distortion model. - /// \return Pointer to the distortion model created. - public: static DistortionPtr NewDistortionModel(sdf::ElementPtr _sdf, - const std::string &_sensorType = ""); - - /// \brief Load a distortion model based on the input sdf parameters and - /// sensor type. - /// \param[in] _sdf Distortion sdf parameters. - /// \param[in] _sensorType Type of sensor. This is currently used to - /// distinguish between image and non image sensors in order to create - /// the appropriate distortion model. - /// \return Pointer to the distortion model created. - public: static DistortionPtr NewDistortionModel( - const sdf::Camera &_sdf, - const std::string &_sensorType = ""); - }; - - /// \brief Which distortion types we support - enum class IGNITION_SENSORS_VISIBLE DistortionType : int - { - NONE = 0, - CUSTOM = 1, - BROWN = 2 - }; - - /// \class Distortion Distortion.hh ignition/sensors/Distortion.hh - /// \brief Distortion models for sensor output signals. - class IGNITION_SENSORS_VISIBLE Distortion - { - /// \brief Constructor. This should not be called directly unless creating - /// an empty distortion model. Use DistortionFactory::NewDistortionModel - /// to instantiate a new distortion model. - /// \param[in] _type Type of distortion model. - /// \sa DistortionFactory::NewDistortionModel - public: explicit Distortion(DistortionType _type); - - /// \brief Destructor. - public: virtual ~Distortion(); - - /// \brief Load distortion parameters from sdf. - /// \param[in] _sdf SDF Distortion DOM object. - public: virtual void Load(const sdf::Camera &_sdf); - - /// \brief Accessor for DistortionType. - /// \return Type of distortion currently in use. - public: DistortionType Type() const; - - /// \brief Output information about the distortion model. - /// \param[in] _out Output stream - public: virtual void Print(std::ostream &_out) const; - - /// \brief Private data pointer - IGN_UTILS_IMPL_PTR(dataPtr) - }; - } - } -} -#endif diff --git a/include/ignition/sensors/ForceTorqueSensor.hh b/include/ignition/sensors/ForceTorqueSensor.hh index 68b16e29..fbf6f74e 100644 --- a/include/ignition/sensors/ForceTorqueSensor.hh +++ b/include/ignition/sensors/ForceTorqueSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. @@ -13,120 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ - -#ifndef IGNITION_SENSORS_FORCETORQUESENSOR_HH_ -#define IGNITION_SENSORS_FORCETORQUESENSOR_HH_ - -#include - -#include - -#include -#include - -#include + */ +#include #include -#include - -#include "ignition/sensors/Sensor.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - /// \brief forward declarations - class ForceTorqueSensorPrivate; - - /// \brief Force Torque Sensor Class - /// - /// A force-torque Sensor that reports force and torque applied on a joint. - class IGNITION_SENSORS_FORCE_TORQUE_VISIBLE ForceTorqueSensor - : public Sensor - { - /// \brief constructor - public: ForceTorqueSensor(); - - /// \brief destructor - public: virtual ~ForceTorqueSensor(); - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(sdf::ElementPtr _sdf) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Update the sensor and generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual bool Update( - const std::chrono::steady_clock::duration &_now) override; - - /// \brief Get the current joint force. - /// \return The latest measured force. - public: math::Vector3d Force() const; - - /// \brief Set the force vector in sensor frame and where the force is - /// applied on the child (parent-to-child) - /// \param[in] _force force vector in newton. - public: void SetForce(const math::Vector3d &_force); - - /// \brief Get the current joint torque. - /// \return The latest measured torque. - public: math::Vector3d Torque() const; - - /// \brief Set the torque vector in sensor frame and where the torque is - /// applied on the child (parent-to-child) - /// \param[in] _torque torque vector in newton. - public: void SetTorque(const math::Vector3d &_torque); - - /// \brief Set the rotation of the joint parent relative to the sensor - /// frame. - /// \return The current rotation the parent in the sensor frame. - public: math::Quaterniond RotationParentInSensor() const; - - /// \brief Set the rotation of the joint parent relative to the sensor - /// frame. - /// \param[in] _rotParentInSensorRotation of parent in sensor frame. - public: void SetRotationParentInSensor( - const math::Quaterniond &_rotParentInSensor); - - /// \brief Set the rotation of the joint parent relative to the sensor - /// frame. - /// \return The current rotation the child in the sensor frame. - public: math::Quaterniond RotationChildInSensor() const; - - /// \brief Set the rotation of the joint child relative to the sensor - /// frame. - /// \param[in] _rotChildInSensor Rotation of child in sensor frame. - public: void SetRotationChildInSensor( - const math::Quaterniond &_rotChildInSensor); - - /// \brief Check if there are any subscribers - /// \return True if there are subscribers, false otherwise - /// \todo(iche033) Make this function virtual on Garden - public: bool HasConnections() const; - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif diff --git a/include/ignition/sensors/ImageBrownDistortionModel.hh b/include/ignition/sensors/ImageBrownDistortionModel.hh index aee103c9..10ce3f0e 100644 --- a/include/ignition/sensors/ImageBrownDistortionModel.hh +++ b/include/ignition/sensors/ImageBrownDistortionModel.hh @@ -13,64 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_SENSORS_IMAGEBROWNDISTORTIONMODEL_HH_ -#define IGNITION_SENSORS_IMAGEBROWNDISTORTIONMODEL_HH_ - -#include - -// TODO(WilliamLewww): Remove these pragmas once ign-rendering is disabling the -// warnings -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable: 4251) -#endif -#include -#ifdef _WIN32 -#pragma warning(pop) -#endif - -#include "ignition/sensors/config.hh" -#include "ignition/sensors/BrownDistortionModel.hh" -#include "ignition/sensors/rendering/Export.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // Forward declarations - class ImageBrownDistortionModelPrivate; - - /** \class ImageBrownDistortionModel ImageBrownDistortionModel.hh \ - ignition/sensors/ImageBrownDistortionModel.hh - **/ - /// \brief Distortion Model class for image sensors - class IGNITION_SENSORS_RENDERING_VISIBLE ImageBrownDistortionModel : - public BrownDistortionModel - { - /// \brief Constructor. - public: ImageBrownDistortionModel(); - - /// \brief Destructor. - public: virtual ~ImageBrownDistortionModel(); - - // Documentation inherited. - public: virtual void Load(const sdf::Camera &_sdf) override; - - // Documentation inherited. - public: virtual void SetCamera(rendering::CameraPtr _camera); - - /// Documentation inherited - public: virtual void Print(std::ostream &_out) const override; - - /// \brief Private data pointer. - private: ImageBrownDistortionModelPrivate *dataPtr = nullptr; - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/ImageDistortion.hh b/include/ignition/sensors/ImageDistortion.hh index 0e39b8c7..22a3c6c4 100644 --- a/include/ignition/sensors/ImageDistortion.hh +++ b/include/ignition/sensors/ImageDistortion.hh @@ -13,55 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_SENSORS_IMAGEDISTORTION_HH_ -#define IGNITION_SENSORS_IMAGEDISTORTION_HH_ - -#include - -#include - -#include "ignition/sensors/config.hh" -#include "ignition/sensors/SensorTypes.hh" -#include "ignition/sensors/rendering/Export.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // Forward declarations - class DistortionPrivate; - - /// \class ImageDistortionFactory ImageDistortion.hh - /// ignition/sensors/ImageDistortion.hh - /// \brief Use this distortion manager for creating and loading distortion - /// models. - class IGNITION_SENSORS_RENDERING_VISIBLE ImageDistortionFactory - { - /// \brief Load a distortion model based on the input sdf parameters and - /// sensor type. - /// \param[in] _sdf Distortion sdf parameters. - /// \param[in] _sensorType Type of sensor. This is currently used to - /// distinguish between image and non image sensors in order to create - /// the appropriate distortion model. - /// \return Pointer to the distortion model created. - public: static DistortionPtr NewDistortionModel(sdf::ElementPtr _sdf, - const std::string &_sensorType = ""); - - /// \brief Load a distortion model based on the input sdf parameters and - /// sensor type. - /// \param[in] _sdf Distortion sdf parameters. - /// \param[in] _sensorType Type of sensor. This is currently used to - /// distinguish between image and non image sensors in order to create - /// the appropriate distortion model. - /// \return Pointer to the distortion model created. - public: static DistortionPtr NewDistortionModel(const sdf::Camera &_sdf, - const std::string &_sensorType = ""); - }; - } - } -} -#endif +#include +#include diff --git a/include/ignition/sensors/NavSatSensor.hh b/include/ignition/sensors/NavSatSensor.hh index 714ad2b3..94b21b79 100644 --- a/include/ignition/sensors/NavSatSensor.hh +++ b/include/ignition/sensors/NavSatSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. @@ -13,121 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_NAVSAT_HH_ -#define IGNITION_SENSORS_NAVSAT_HH_ + */ -#include - -#include -#include - -#include "ignition/sensors/config.hh" -#include "ignition/sensors/navsat/Export.hh" - -#include "ignition/sensors/Sensor.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // - /// \brief forward declarations - class NavSatPrivate; - - /// \brief NavSat Sensor Class - /// - /// A sensor that reports position and velocity readings over - /// Ignition Transport using spherical coordinates (latitude / longitude). - /// - /// By default, it publishes `ignition::msgs::NavSat` messages on the - /// `/.../navsat` topic. - /// - /// This sensor assumes the world is using the East-North-Up (ENU) frame. - class IGNITION_SENSORS_NAVSAT_VISIBLE NavSatSensor : public Sensor - { - /// \brief Constructor - public: NavSatSensor(); - - /// \brief Destructor - public: virtual ~NavSatSensor(); - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(sdf::ElementPtr _sdf) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Update the sensor and generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual bool Update( - const std::chrono::steady_clock::duration &_now) override; - - /// \brief Set the latitude of the NavSat - /// \param[in] _latitude Latitude of NavSat - public: void SetLatitude(const math::Angle &_latitude); - - /// \brief Get the latitude of the NavSat, wrapped between +/- 180 - /// degrees. - /// \return Latitude angle. - public: const math::Angle &Latitude() const; - - /// \brief Set the longitude of the NavSat - /// \param[in] _longitude Longitude of NavSat - public: void SetLongitude(const math::Angle &_longitude); - - /// \brief Get the longitude of the NavSat, wrapped between +/- 180 - /// degrees. - /// \return Longitude angle. - public: const math::Angle &Longitude() const; - - /// \brief Set the altitude of the NavSat - /// \param[in] _altitude altitude of NavSat in meters - public: void SetAltitude(double _altitude); - - /// \brief Get NavSat altitude above sea level - /// \return Altitude in meters - public: double Altitude() const; - - /// \brief Set the velocity of the NavSat in ENU world frame. - /// \param[in] _vel NavSat in meters per second. - public: void SetVelocity(const math::Vector3d &_vel); - - /// \brief Get the velocity of the NavSat sensor in the ENU world frame. - /// \return Velocity in meters per second - public: const math::Vector3d &Velocity() const; - - /// \brief Check if there are any subscribers - /// \return True if there are subscribers, false otherwise - /// \todo(iche033) Make this function virtual on Garden - public: bool HasConnections() const; - - /// \brief Easy short hand for setting the position of the sensor. - /// \param[in] _latitude Latitude angle. - /// \param[in] _longitude Longitude angle. - /// \param[in] _altitude Altitude in meters; defaults to zero. - public: void SetPosition(const math::Angle &_latitude, - const math::Angle &_longitude, double _altitude = 0.0); - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/SegmentationCameraSensor.hh b/include/ignition/sensors/SegmentationCameraSensor.hh index 4bed152f..5eddea5f 100644 --- a/include/ignition/sensors/SegmentationCameraSensor.hh +++ b/include/ignition/sensors/SegmentationCameraSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. @@ -13,129 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ -#ifndef IGNITION_SENSORS_SEGMENTATIONCAMERASENSOR_HH_ -#define IGNITION_SENSORS_SEGMENTATIONCAMERASENSOR_HH_ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ignition/sensors/CameraSensor.hh" -#include "ignition/sensors/Export.hh" -#include "ignition/sensors/Sensor.hh" - -#include "ignition/sensors/segmentation_camera/Export.hh" - -namespace ignition -{ - namespace sensors - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { - // forward declarations - class SegmentationCameraSensorPrivate; - - /// \brief Segmentation camera sensor class. - /// - /// This class creates segmentation images from an ignition rendering scene. - /// The scene must be created in advance and given to Manager::Init(). - /// It offers both an ignition-transport interface and a direct C++ API - /// to access the image data. The API works by setting a callback to be - /// called with image data. - class IGNITION_SENSORS_SEGMENTATION_CAMERA_VISIBLE - SegmentationCameraSensor : public CameraSensor - { - /// \brief constructor - public: SegmentationCameraSensor(); - - /// \brief destructor - public: virtual ~SegmentationCameraSensor(); - - /// \brief Load the sensor based on data from an sdf::Sensor object. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(const sdf::Sensor &_sdf) override; - - /// \brief Load the sensor with SDF parameters. - /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful - public: virtual bool Load(sdf::ElementPtr _sdf) override; - - /// \brief Initialize values in the sensor - /// \return True on success - public: virtual bool Init() override; - - /// \brief Force the sensor to generate data - /// \param[in] _now The current time - /// \return true if the update was successful - public: virtual bool Update( - const std::chrono::steady_clock::duration &_now) override; - - /// \brief Get the rendering segmentation camera - /// \return Segmentation camera pointer - public: rendering::SegmentationCameraPtr SegmentationCamera() const; - - /// \brief Segmentation data callback used to get the data from the sensor - /// \param[in] _data pointer to the data from the sensor - /// \param[in] _width width of the segmentation image - /// \param[in] _height height of the segmentation image - /// \param[in] _channels num of channels - /// \param[in] _format string with the format - public: void OnNewSegmentationFrame(const uint8_t * _data, - unsigned int _width, unsigned int _height, unsigned int _channels, - const std::string &_format); - - /// \brief Set a callback to be called when image frame data is - /// generated. - /// \param[in] _callback This callback will be called every time the - /// camera produces image data. The Update function will be blocked - /// while the callbacks are executed. - /// \remark Do not block inside of the callback. - /// \return A connection pointer that must remain in scope. When the - /// connection pointer falls out of scope, the connection is broken. - public: ignition::common::ConnectionPtr ConnectImageCallback( - std::function _callback); - - /// \brief Set the rendering scene. - /// \param[in] _scene Pointer to the scene - public: virtual void SetScene( - ignition::rendering::ScenePtr _scene) override; - - /// \brief Get image width. - /// \return width of the image - public: virtual unsigned int ImageWidth() const override; - - /// \brief Get image height. - /// \return height of the image - public: virtual unsigned int ImageHeight() const override; - - /// \brief Check if there are any subscribers - /// \return True if there are subscribers, false otherwise - /// \todo(iche033) Make this function virtual on Garden - public: bool HasConnections() const; - - /// \brief Create a camera in a scene - /// \return True on success. - private: bool CreateCamera(); - - IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING - /// \brief Data pointer for private data - /// \internal - private: std::unique_ptr dataPtr; - IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING - }; - } - } -} - -#endif +#include +#include diff --git a/include/ignition/sensors/Util.hh b/include/ignition/sensors/Util.hh index e9d2db43..d0685347 100644 --- a/include/ignition/sensors/Util.hh +++ b/include/ignition/sensors/Util.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. @@ -13,56 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_SENSORS_UTIL_HH_ -#define IGNITION_SENSORS_UTIL_HH_ + */ -#include - -#include -#include - -#include "ignition/sensors/config.hh" -#include "ignition/sensors/Export.hh" - -namespace ignition -{ -namespace sensors -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { -// -/// \brief Get the name of a sensor's custom type from SDF. -/// -/// Given an SDF tag as follows: -/// -/// -/// -/// This function returns `sensor_type`. -/// -/// It will return an empty string if the element is malformed. For example, -/// if it misses the `ignition:type` attribute or is not of `type="custom"`. -/// -/// \param[in] _sdf Sensor SDF object. -/// \return _sensorType Name of sensor type. -std::string IGNITION_SENSORS_VISIBLE customType(const sdf::Sensor &_sdf); // NOLINT - -/// \brief Get the name of a sensor's custom type from SDF. -/// -/// Given an SDF tag as follows: -/// -/// -/// -/// This function returns `sensor_type`. -/// -/// It will return an empty string if the element is malformed. For example, -/// if it misses the `ignition:type` attribute or is not of `type="custom"`. -/// -/// \param[in] _sdf Sensor SDF object. -/// \return _sensorType Name of sensor type. -std::string IGNITION_SENSORS_VISIBLE customType(sdf::ElementPtr _sdf); // NOLINT -} -} -} - -#endif +#include +#include diff --git a/include/ignition/sensors/boundingbox_camera/Export.hh b/include/ignition/sensors/boundingbox_camera/Export.hh new file mode 100644 index 00000000..be727903 --- /dev/null +++ b/include/ignition/sensors/boundingbox_camera/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/force_torque/Export.hh b/include/ignition/sensors/force_torque/Export.hh new file mode 100644 index 00000000..e6e0aa98 --- /dev/null +++ b/include/ignition/sensors/force_torque/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/navsat/Export.hh b/include/ignition/sensors/navsat/Export.hh new file mode 100644 index 00000000..33b96ddb --- /dev/null +++ b/include/ignition/sensors/navsat/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/sensors/segmentation_camera/Export.hh b/include/ignition/sensors/segmentation_camera/Export.hh new file mode 100644 index 00000000..4e3ca411 --- /dev/null +++ b/include/ignition/sensors/segmentation_camera/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 From 1f75c5f0dbe244a0c6728f47fe103bf6b4813997 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 3 Feb 2023 20:08:45 -0800 Subject: [PATCH 05/26] fix links Signed-off-by: Ian Chen --- tutorials/thermal_camera.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tutorials/thermal_camera.md b/tutorials/thermal_camera.md index df5f79fe..e5298daf 100644 --- a/tutorials/thermal_camera.md +++ b/tutorials/thermal_camera.md @@ -251,9 +251,9 @@ You should see something similar to this: @image html files/thermal_camera/thermal_camera_demo.png -Taking a look at the [SDF file](https://github.com/gazebosim/gz-gazebo/blob/e647570f25f962d63af75cf669ff72731d57bd5e/examples/worlds/thermal_camera.sdf) for this example shows that the box was assigned a temperature of 285 Kelvin. +Taking a look at the [SDF file](https://github.com/gazebosim/gz-sim/blob/e647570f25f962d63af75cf669ff72731d57bd5e/examples/worlds/thermal_camera.sdf) for this example shows that the box was assigned a temperature of 285 Kelvin. -If we take a look at the Rescue Randy and Samsung J8 [fuel models](https://app.gazebosim.org/dashboard), we see that they have the following temperature range (the SDF file we are using with these models has an [ambient temperature of 300 Kelvin](https://github.com/gazebosim/gz-gazebo/blob/e647570f25f962d63af75cf669ff72731d57bd5e/examples/worlds/thermal_camera.sdf#L135-L144)): +If we take a look at the Rescue Randy and Samsung J8 [fuel models](https://app.gazebosim.org/dashboard), we see that they have the following temperature range (the SDF file we are using with these models has an [ambient temperature of 300 Kelvin](https://github.com/gazebosim/gz-sim/blob/e647570f25f962d63af75cf669ff72731d57bd5e/examples/worlds/thermal_camera.sdf#L135-L144)): * Rescue Randy: 300 Kelvin to 310 Kelvin * Samsung J8: 298.75 Kelvin to 300 Kelvin @@ -359,7 +359,7 @@ The thermal camera has a few limitations: A more realistic implementation would have the camera display temperature fluctuations in the closer object if the temperature difference between the two objects is large enough, and if the closer object isn't too thick. - More information about thermal camera behavior can be found [here](https://www.flir.com/discover/cores-components/can-thermal-imaging-see-through-walls/). * There's a precision loss when the thermal camera converts temperature readings to gray scale output. -To help quantify the magnitude of this precision loss, running the conversion code above on [this SDF file](https://github.com/gazebosim/gz-gazebo/blob/990e4f240bbb3246a0e1d0c89b74e0ef8f109b4b/examples/worlds/thermal_camera.sdf) results in the following processed temperatures for each object in the camera's image: +To help quantify the magnitude of this precision loss, running the conversion code above on [this SDF file](https://github.com/gazebosim/gz-sim/blob/990e4f240bbb3246a0e1d0c89b74e0ef8f109b4b/examples/worlds/thermal_camera.sdf) results in the following processed temperatures for each object in the camera's image: - sphere: listed in the SDF file as 600 Kelvin, processed from the thermal camera image topic as 598.81 Kelvin - box: listed in the SDF file as 200 Kelvin, processed from the thermal camera image topic as 200.46 Kelvin - cylinder: listed in the SDF file as 400 Kelvin, processed from the thermal camera image topic as 400.92 Kelvin From 7872955bc1d98b62e26379f6b34e54280033083f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 8 Feb 2023 16:32:01 -0800 Subject: [PATCH 06/26] set custom projection values from sdf Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- src/CameraSensor.cc | 18 +++ test/integration/camera.cc | 216 +++++++++++++++++++++++++++++---- test/sdf/camera_intrinsics.sdf | 2 +- test/sdf/camera_projection.sdf | 43 +++++-- 5 files changed, 245 insertions(+), 36 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index af7e1963..58c98756 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,7 +89,7 @@ set(GZ_MSGS_VER ${gz-msgs9_VERSION_MAJOR}) #-------------------------------------- # Find SDFormat -gz_find_package(sdformat13 REQUIRED) +gz_find_package(sdformat13 13.3 REQUIRED) set(SDF_VER ${sdformat13_VERSION_MAJOR}) #============================================================================ diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index f908dbc7..db9cf5a4 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -356,6 +356,24 @@ bool CameraSensor::CreateCamera() cameraSdf->SetLensProjectionCx(intrinsicMatrix(0, 2)); cameraSdf->SetLensProjectionCy(intrinsicMatrix(1, 2)); } + // set custom projection matrix based on projection param specified in sdf + else + { + // tx and ty are not used + double fx = cameraSdf->LensProjectionFx(); + double fy = cameraSdf->LensProjectionFy(); + double cx = cameraSdf->LensProjectionCx(); + double cy = cameraSdf->LensProjectionCy(); + double s = 0; + + auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix( + this->dataPtr->camera->ImageWidth(), + this->dataPtr->camera->ImageHeight(), + fx, fy, cx, cy, s, + this->dataPtr->camera->NearClipPlane(), + this->dataPtr->camera->FarClipPlane()); + this->dataPtr->camera->SetProjectionMatrix(projectionMatrix); + } // Populate camera info topic this->PopulateInfo(cameraSdf); diff --git a/test/integration/camera.cc b/test/integration/camera.cc index db3d33a6..43c5ea1d 100644 --- a/test/integration/camera.cc +++ b/test/integration/camera.cc @@ -489,13 +489,17 @@ void CameraSensorTest::CameraProjection(const std::string &_renderEngine) auto linkPtr = modelPtr->GetElement("link"); ASSERT_TRUE(linkPtr->HasElement("sensor")); - // Camera sensor without intrinsics tag - auto cameraWithoutIntrinsicsTag = linkPtr->GetElement("sensor"); + // Camera sensor without projection tag + auto cameraWithoutProjectionsTag = linkPtr->GetElement("sensor"); - // Camera sensor with intrinsics tag - auto cameraWithIntrinsicsTag = + // Camera sensor with projection tag + auto cameraWithProjectionsTag = linkPtr->GetElement("sensor")->GetNextElement(); + // Camera sensor with different projection tag + auto cameraWithDiffProjectionsTag = + cameraWithProjectionsTag->GetNextElement(); + // Setup gz-rendering with an empty scene auto *engine = gz::rendering::engine(_renderEngine); if (!engine) @@ -506,27 +510,60 @@ void CameraSensorTest::CameraProjection(const std::string &_renderEngine) } gz::rendering::ScenePtr scene = engine->CreateScene("scene"); + scene->SetAmbientLight(1.0, 1.0, 1.0); + + gz::rendering::VisualPtr root = scene->RootVisual(); + ASSERT_NE(nullptr, root); + + // create blue material + gz::rendering::MaterialPtr blue = scene->CreateMaterial(); + blue->SetAmbient(0.0, 0.0, 0.3); + blue->SetDiffuse(0.0, 0.0, 0.8); + blue->SetSpecular(0.5, 0.5, 0.5); + + // create box visual + gz::rendering::VisualPtr box = scene->CreateVisual("box"); + ASSERT_NE(nullptr, box); + box->AddGeometry(scene->CreateBox()); + box->SetOrigin(0.0, 0.0, 0.0); + box->SetLocalPosition(gz::math::Vector3d(4.0, 1, 0.5)); + box->SetLocalRotation(0, 0, 0); + box->SetMaterial(blue); + root->AddChild(box); // Do the test gz::sensors::Manager mgr; auto *sensor1 = - mgr.CreateSensor(cameraWithoutIntrinsicsTag); + mgr.CreateSensor(cameraWithoutProjectionsTag); auto *sensor2 = - mgr.CreateSensor(cameraWithIntrinsicsTag); + mgr.CreateSensor(cameraWithProjectionsTag); + auto *sensor3 = + mgr.CreateSensor(cameraWithDiffProjectionsTag); + ASSERT_NE(sensor1, nullptr); ASSERT_NE(sensor2, nullptr); + ASSERT_NE(sensor3, nullptr); + std::string imgTopic1 = "/camera1/image"; + std::string imgTopic2 = "/camera2/image"; + std::string imgTopic3 = "/camera3/image"; sensor1->SetScene(scene); sensor2->SetScene(scene); + sensor3->SetScene(scene); std::string infoTopic1 = "/camera1/camera_info"; std::string infoTopic2 = "/camera2/camera_info"; + std::string infoTopic3 = "/camera3/camera_info"; WaitForMessageTestHelper helper1("/camera1/image"); WaitForMessageTestHelper helper2(infoTopic1); WaitForMessageTestHelper helper3("/camera2/image"); WaitForMessageTestHelper helper4(infoTopic2); + WaitForMessageTestHelper helper5(imgTopic3); + WaitForMessageTestHelper helper6(infoTopic3); + EXPECT_TRUE(sensor1->HasConnections()); EXPECT_TRUE(sensor2->HasConnections()); + EXPECT_TRUE(sensor3->HasConnections()); // Update once to create image mgr.RunOnce(std::chrono::steady_clock::duration::zero()); @@ -535,28 +572,72 @@ void CameraSensorTest::CameraProjection(const std::string &_renderEngine) EXPECT_TRUE(helper2.WaitForMessage()) << helper2; EXPECT_TRUE(helper3.WaitForMessage()) << helper3; EXPECT_TRUE(helper4.WaitForMessage()) << helper4; + EXPECT_TRUE(helper5.WaitForMessage()) << helper5; + EXPECT_TRUE(helper6.WaitForMessage()) << helper6; // Subscribe to the camera info topic - gz::msgs::CameraInfo camera1Info, camera2Info; - unsigned int camera1Counter = 0; - unsigned int camera2Counter = 0; + gz::msgs::CameraInfo camera1Info, camera2Info, camera3Info; + unsigned int camera1Counter = 0u; + unsigned int camera2Counter = 0u; + unsigned int camera3Counter = 0u; std::function camera1InfoCallback = - [&camera1Info, &camera1Counter](const gz::msgs::CameraInfo& _msg) { - camera1Info = _msg; - camera1Counter++; + [&camera1Info, &camera1Counter](const gz::msgs::CameraInfo& _msg) + { + camera1Info = _msg; + camera1Counter++; }; - std::function camera2InfoCallback = - [&camera2Info, &camera2Counter](const gz::msgs::CameraInfo& _msg) { - camera2Info = _msg; - camera2Counter++; + [&camera2Info, &camera2Counter](const gz::msgs::CameraInfo& _msg) + { + camera2Info = _msg; + camera2Counter++; + }; + std::function camera3InfoCallback = + [&camera3Info, &camera3Counter](const gz::msgs::CameraInfo& _msg) + { + camera3Info = _msg; + camera3Counter++; + }; + + unsigned int height = 1000u; + unsigned int width = 1000u; + unsigned int bpp = 3u; + unsigned int imgBufferSize = width * height * bpp; + unsigned char* img1 = new unsigned char[imgBufferSize]; + unsigned char* img2 = new unsigned char[imgBufferSize]; + unsigned char* img3 = new unsigned char[imgBufferSize]; + unsigned int camera1DataCounter = 0u; + unsigned int camera2DataCounter = 0u; + unsigned int camera3DataCounter = 0u; + std::function camera1Callback = + [&img1, &camera1DataCounter, &imgBufferSize](const gz::msgs::Image & _msg) + { + memcpy(img1, _msg.data().c_str(), imgBufferSize); + camera1DataCounter++; + }; + + std::function camera2Callback = + [&img2, &camera2DataCounter, &imgBufferSize](const gz::msgs::Image & _msg) + { + memcpy(img2, _msg.data().c_str(), imgBufferSize); + camera2DataCounter++; + }; + std::function camera3Callback = + [&img3, &camera3DataCounter, &imgBufferSize](const gz::msgs::Image & _msg) + { + memcpy(img3, _msg.data().c_str(), imgBufferSize); + camera3DataCounter++; }; // Subscribe to the camera topic gz::transport::Node node; node.Subscribe(infoTopic1, camera1InfoCallback); node.Subscribe(infoTopic2, camera2InfoCallback); + node.Subscribe(infoTopic3, camera3InfoCallback); + node.Subscribe(imgTopic1, camera1Callback); + node.Subscribe(imgTopic2, camera2Callback); + node.Subscribe(imgTopic3, camera3Callback); // Wait for a few camera frames mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); @@ -568,32 +649,115 @@ void CameraSensorTest::CameraProjection(const std::string &_renderEngine) while (!done && sleep++ < maxSleep) { std::lock_guard lock(g_mutex); - done = (camera1Counter > 0 && camera2Counter > 0); + done = (camera1Counter > 0u && camera2Counter > 0u && camera3Counter > 0u && + camera1DataCounter > 0u && camera2DataCounter > 0u && + camera3DataCounter > 0u); std::this_thread::sleep_for(std::chrono::milliseconds(100)); } // Image size, focal length and optical center // Camera sensor without projection tag - double error = 1e-1; - EXPECT_EQ(camera1Info.width(), 1000u); - EXPECT_EQ(camera1Info.height(), 1000u); - EXPECT_NEAR(camera1Info.projection().p(0), 863.22975158691406, error); - EXPECT_NEAR(camera1Info.projection().p(5), 863.22975158691406, error); + // account for error converting gl projection values back to + // cv projection values + double error = 1; + EXPECT_EQ(camera1Info.width(), width); + EXPECT_EQ(camera1Info.height(), width); + EXPECT_NEAR(camera1Info.projection().p(0), 866.23, error); + EXPECT_NEAR(camera1Info.projection().p(5), 866.23, error); EXPECT_DOUBLE_EQ(camera1Info.projection().p(2), 500); EXPECT_DOUBLE_EQ(camera1Info.projection().p(6), 500); EXPECT_DOUBLE_EQ(camera1Info.projection().p(3), 0); EXPECT_DOUBLE_EQ(camera1Info.projection().p(7), 0); // Camera sensor with projection tag - EXPECT_EQ(camera2Info.width(), 1000u); - EXPECT_EQ(camera2Info.height(), 1000u); + EXPECT_EQ(camera2Info.width(), height); + EXPECT_EQ(camera2Info.height(), height); EXPECT_DOUBLE_EQ(camera2Info.projection().p(0), 866.23); - EXPECT_DOUBLE_EQ(camera2Info.projection().p(5), 966.23); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(5), 866.23); EXPECT_DOUBLE_EQ(camera2Info.projection().p(2), 500); - EXPECT_DOUBLE_EQ(camera2Info.projection().p(6), 400); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(6), 500); EXPECT_DOUBLE_EQ(camera2Info.projection().p(3), 300); EXPECT_DOUBLE_EQ(camera2Info.projection().p(7), 200); + // Camera sensor with different projection tag + EXPECT_EQ(camera3Info.width(), width); + EXPECT_EQ(camera3Info.height(), height); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(0), 900); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(5), 900); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(2), 501); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(6), 501); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(3), 0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(7), 0); + + unsigned int r1Sum = 0u; + unsigned int g1Sum = 0u; + unsigned int b1Sum = 0u; + unsigned int r2Sum = 0u; + unsigned int g2Sum = 0u; + unsigned int b2Sum = 0u; + unsigned int r3Sum = 0u; + unsigned int g3Sum = 0u; + unsigned int b3Sum = 0u; + unsigned int step = width * bpp; + + gz::common::Image i1; + i1.SetFromData(img1, width, height, + gz::common::Image::RGB_INT8); + i1.SavePNG("img1.png"); + + // get sum of each color channel + // all cameras should just see blue colors + for (unsigned int i = 0u; i < height; ++i) + { + for (unsigned int j = 0u; j < step; j+=bpp) + { + unsigned int idx = i * step + j; + unsigned int r1 = img1[idx]; + unsigned int g1 = img1[idx+1]; + unsigned int b1 = img1[idx+2]; + r1Sum += r1; + g1Sum += g1; + b1Sum += b1; + + unsigned int r2 = img2[idx]; + unsigned int g2 = img2[idx+1]; + unsigned int b2 = img2[idx+2]; + r2Sum += r2; + g2Sum += g2; + b2Sum += b2; + + unsigned int r3 = img3[idx]; + unsigned int g3 = img3[idx+1]; + unsigned int b3 = img3[idx+2]; + r3Sum += r3; + g3Sum += g3; + b3Sum += b3; + } + } + + // images from camera1 without projections params and + // camera2 with default projection params should be the same + EXPECT_EQ(0u, r1Sum); + EXPECT_EQ(0u, g1Sum); + EXPECT_LT(0u, b1Sum); + EXPECT_EQ(r1Sum, r2Sum); + EXPECT_EQ(g1Sum, g2Sum); + EXPECT_EQ(b1Sum, b2Sum); + + // images from camera2 and camera3 that have different projections params + // should be different. Both cameras should still see the blue box but + // sum of blue pixels should be slightly different + EXPECT_EQ(0u, r3Sum); + EXPECT_EQ(0u, g3Sum); + EXPECT_LT(0u, b3Sum); + EXPECT_EQ(r2Sum, r3Sum); + EXPECT_EQ(g2Sum, g3Sum); + EXPECT_NE(b2Sum, b3Sum); + + delete [] img1; + delete [] img2; + delete [] img3; + // Clean up engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); diff --git a/test/sdf/camera_intrinsics.sdf b/test/sdf/camera_intrinsics.sdf index 3b715ac5..85557b52 100644 --- a/test/sdf/camera_intrinsics.sdf +++ b/test/sdf/camera_intrinsics.sdf @@ -47,7 +47,7 @@ 10 - base_camera + base_camera /camera3/image 1.0472 diff --git a/test/sdf/camera_projection.sdf b/test/sdf/camera_projection.sdf index 7415640a..80a0a271 100644 --- a/test/sdf/camera_projection.sdf +++ b/test/sdf/camera_projection.sdf @@ -4,40 +4,67 @@ 10 - base_camera + base_camera /camera1/image - 1.05 + 1.0472 1000 1000 - L8 + R8G8B8 10 - base_camera + base_camera /camera2/image - 1.05 + 1.0472 1000 1000 - L8 + R8G8B8 866.23 - 966.23 + 866.23 500 - 400 + 500 300 200 + + 10 + base_camera + /camera3/image + + 1.0472 + + 1000 + 1000 + R8G8B8 + + + 0.1 + 100 + + + + 900 + 900 + 501 + 501 + 0 + 0 + + + + From 5b9d8046916834a9f3895a364437019e424bd2f4 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 8 Feb 2023 17:12:08 -0800 Subject: [PATCH 07/26] remove debug img Signed-off-by: Ian Chen --- test/integration/camera.cc | 5 ----- 1 file changed, 5 deletions(-) diff --git a/test/integration/camera.cc b/test/integration/camera.cc index 43c5ea1d..4bb09948 100644 --- a/test/integration/camera.cc +++ b/test/integration/camera.cc @@ -700,11 +700,6 @@ void CameraSensorTest::CameraProjection(const std::string &_renderEngine) unsigned int b3Sum = 0u; unsigned int step = width * bpp; - gz::common::Image i1; - i1.SetFromData(img1, width, height, - gz::common::Image::RGB_INT8); - i1.SavePNG("img1.png"); - // get sum of each color channel // all cameras should just see blue colors for (unsigned int i = 0u; i < height; ++i) From 98288bdd3f54792d4ff89b6b8895687f0ffb168c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 8 Feb 2023 17:26:07 -0800 Subject: [PATCH 08/26] CMakel tweak Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 58c98756..dc227425 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,7 +89,7 @@ set(GZ_MSGS_VER ${gz-msgs9_VERSION_MAJOR}) #-------------------------------------- # Find SDFormat -gz_find_package(sdformat13 13.3 REQUIRED) +gz_find_package(sdformat13 VERSION 13.3 REQUIRED) set(SDF_VER ${sdformat13_VERSION_MAJOR}) #============================================================================ From 0d7a5ff4e92cbe643cf81d2d0db03bd6f56ba771 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 10 Feb 2023 12:09:47 -0800 Subject: [PATCH 09/26] floating point values Signed-off-by: Ian Chen --- test/integration/camera.cc | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/test/integration/camera.cc b/test/integration/camera.cc index 4bb09948..d9d80a26 100644 --- a/test/integration/camera.cc +++ b/test/integration/camera.cc @@ -659,35 +659,35 @@ void CameraSensorTest::CameraProjection(const std::string &_renderEngine) // Camera sensor without projection tag // account for error converting gl projection values back to // cv projection values - double error = 1; + double error = 1.0; EXPECT_EQ(camera1Info.width(), width); EXPECT_EQ(camera1Info.height(), width); EXPECT_NEAR(camera1Info.projection().p(0), 866.23, error); EXPECT_NEAR(camera1Info.projection().p(5), 866.23, error); - EXPECT_DOUBLE_EQ(camera1Info.projection().p(2), 500); - EXPECT_DOUBLE_EQ(camera1Info.projection().p(6), 500); - EXPECT_DOUBLE_EQ(camera1Info.projection().p(3), 0); - EXPECT_DOUBLE_EQ(camera1Info.projection().p(7), 0); + EXPECT_DOUBLE_EQ(camera1Info.projection().p(2), 500.0); + EXPECT_DOUBLE_EQ(camera1Info.projection().p(6), 500.0); + EXPECT_DOUBLE_EQ(camera1Info.projection().p(3), 0.0); + EXPECT_DOUBLE_EQ(camera1Info.projection().p(7), 0.0); // Camera sensor with projection tag EXPECT_EQ(camera2Info.width(), height); EXPECT_EQ(camera2Info.height(), height); EXPECT_DOUBLE_EQ(camera2Info.projection().p(0), 866.23); EXPECT_DOUBLE_EQ(camera2Info.projection().p(5), 866.23); - EXPECT_DOUBLE_EQ(camera2Info.projection().p(2), 500); - EXPECT_DOUBLE_EQ(camera2Info.projection().p(6), 500); - EXPECT_DOUBLE_EQ(camera2Info.projection().p(3), 300); - EXPECT_DOUBLE_EQ(camera2Info.projection().p(7), 200); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(2), 500.0); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(6), 500.0); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(3), 300.0); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(7), 200.0); // Camera sensor with different projection tag EXPECT_EQ(camera3Info.width(), width); EXPECT_EQ(camera3Info.height(), height); - EXPECT_DOUBLE_EQ(camera3Info.projection().p(0), 900); - EXPECT_DOUBLE_EQ(camera3Info.projection().p(5), 900); - EXPECT_DOUBLE_EQ(camera3Info.projection().p(2), 501); - EXPECT_DOUBLE_EQ(camera3Info.projection().p(6), 501); - EXPECT_DOUBLE_EQ(camera3Info.projection().p(3), 0); - EXPECT_DOUBLE_EQ(camera3Info.projection().p(7), 0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(0), 900.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(5), 900.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(2), 501.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(6), 501.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(3), 0.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(7), 0.0); unsigned int r1Sum = 0u; unsigned int g1Sum = 0u; From b13bf0d474be0101bd0dd47b16aff8888d15b97c Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 13 Feb 2023 15:41:58 -0800 Subject: [PATCH 10/26] 6.7.0 prerelease (#320) Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 4 ++-- Changelog.md | 59 +++++++++++++++++++++++++++++++++++--------------- 2 files changed, 43 insertions(+), 20 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7fdb3346..ee11e054 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-sensors6 VERSION 6.6.0) +project(ignition-sensors6 VERSION 6.7.0) #============================================================================ # Find ignition-cmake @@ -15,7 +15,7 @@ find_package(ignition-cmake2 2.13 REQUIRED) #============================================================================ ign_configure_project( REPLACE_IGNITION_INCLUDE_PATH gz/sensors - VERSION_SUFFIX + VERSION_SUFFIX pre1 ) #============================================================================ diff --git a/Changelog.md b/Changelog.md index bdf3f6b1..bf1de492 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,16 +1,39 @@ -## Ignition Sensors 6 +## Gazebo Sensors 6 -### Ignition Sensors 6.6.0 (2022-06-17) +### Gazebo Sensors 6.7.0 (2023-02-13) + +1. Disable thermal camera test on MacOS. + * [Pull request #243](https://github.com/gazebosim/gz-sensors/pull/243) + +1. Add optional optical frame id to camera sensors. + * [Pull request #259](https://github.com/gazebosim/gz-sensors/pull/259) + +1. Add support for 16 bit image format. + * [Pull request #276](https://github.com/gazebosim/gz-sensors/pull/276) + +1. Fix navsat frame id. + * [Pull request #298](https://github.com/gazebosim/gz-sensors/pull/298) + +1. CameraInfo is now published when there's a CameraSensor subscriber. + * [Pull request #308](https://github.com/gazebosim/gz-sensors/pull/308) + +1. Add HasInfoConnections() method to expose infoPub.HasConnections() to + child of CameraSensor class. + * [Pull request #310](https://github.com/gazebosim/gz-sensors/pull/310) + +1. Forward port 3.5.0. + +### Gazebo Sensors 6.6.0 (2022-06-17) 1. Add BoundingBox Sensor * [Pull request #136](https://github.com/gazebosim/gz-sensors/pull/136) -### Ignition Sensors 6.5.0 (2022-05-24) +### Gazebo Sensors 6.5.0 (2022-05-24) 1. Add HasConnections function * [Pull request #222](https://github.com/ignitionrobotics/ign-sensors/pull/222) -### Ignition Sensors 6.4.0 (2022-05-13) +### Gazebo Sensors 6.4.0 (2022-05-13) 1. Set lidar visibility mask * [Pull request #224](https://github.com/ignitionrobotics/ign-sensors/pull/224) @@ -24,7 +47,7 @@ 1. Fix `` not working for GpuLidarSensor * [Pull request #218](https://github.com/ignitionrobotics/ign-sensors/pull/218) -### Ignition Sensors 6.3.0 (2022-04-04) +### Gazebo Sensors 6.3.0 (2022-04-04) 1. IMU custom_rpy parent_frame should be set to 'world' * [Pull request #212](https://github.com/ignitionrobotics/ign-sensors/pull/212) @@ -35,7 +58,7 @@ 1. Check if noise or distortion render pass is null * [Pull request #211](https://github.com/ignitionrobotics/ign-sensors/pull/211) -### Ignition Sensors 6.2.0 (2022-03-29) +### Gazebo Sensors 6.2.0 (2022-03-29) 1. Distortion Camera Sensor * [Pull request #192](https://github.com/ignitionrobotics/ign-sensors/pull/192) @@ -61,7 +84,7 @@ 1. Fix compiler warnings (CMP0072 and copy elision) * [Pull request #188](https://github.com/ignitionrobotics/ign-sensors/pull/188) -### Ignition Sensors 6.1.0 (2022-01-04) +### Gazebo Sensors 6.1.0 (2022-01-04) 1. Add NavSat (GPS) sensor * [Pull request #177](https://github.com/ignitionrobotics/ign-sensors/pull/177) @@ -72,7 +95,7 @@ 1. IMU ``custom_rpy`` tag parsing added * [Pull request #178](https://github.com/ignitionrobotics/ign-sensors/pull/178) -### Ignition Sensors 6.0.1 (2021-11-12) +### Gazebo Sensors 6.0.1 (2021-11-12) 1. Disable GPU lidar tests on macOS * [Pull request #163](https://github.com/ignitionrobotics/ign-sensors/pull/163) @@ -83,7 +106,7 @@ 1. Destroy rendering sensors when sensor is removed. * [Pull request #169](https://github.com/ignitionrobotics/ign-sensors/pull/169) -### Ignition Sensors 6.0.0 (2021-09-30) +### Gazebo Sensors 6.0.0 (2021-09-30) 1. Trivial tutorial typo correction in Custom Sensors tutorial * [Pull request #160](https://github.com/ignitionrobotics/ign-sensors/pull/160) @@ -112,11 +135,11 @@ 1. Joint Force-Torque Sensor * [Pull request #144](https://github.com/ignitionrobotics/ign-sensors/pull/144) -## Ignition Sensors 5 +## Gazebo Sensors 5 -### Ignition Sensors 5.X.X +### Gazebo Sensors 5.X.X -### Ignition Sensors 5.1.0 (2021-10-15) +### Gazebo Sensors 5.1.0 (2021-10-15) 1. Depend on ign-msgs 7.2 and libSDFormat 11.3 * [Pull request #154](https://github.com/ignitionrobotics/ign-sensors/pull/154) @@ -145,7 +168,7 @@ 1. 👩‍🌾 Disable tests that consistently fail on macOS * [Pull request #121](https://github.com/ignitionrobotics/ign-sensors/pull/121) -### Ignition Sensors 5.0.0 (2021-03-30) +### Gazebo Sensors 5.0.0 (2021-03-30) 1. Bump in edifice: ign-common4 * [Pull request #85](https://github.com/ignitionrobotics/ign-sensors/pull/85) @@ -162,11 +185,11 @@ 1. Documentation updates * [Pull request #116](https://github.com/ignitionrobotics/ign-sensors/pull/116) -## Ignition Sensors 4 +## Gazebo Sensors 4 -### Ignition Sensors 4.X.X +### Gazebo Sensors 4.X.X -### Ignition Sensors 4.2.0 (2021-07-12) +### Gazebo Sensors 4.2.0 (2021-07-12) 1. Add API for enabling / disabling IMU orientation * [Pull request #142](https://github.com/ignitionrobotics/ign-sensors/pull/142) @@ -201,7 +224,7 @@ 1. Removed issue & PR templates * [Pull request #99](https://github.com/ignitionrobotics/ign-sensors/pull/99) -### Ignition Sensors 4.1.0 (2021-02-10) +### Gazebo Sensors 4.1.0 (2021-02-10) 1. Added issue and PR templates. * [Pull request 91](https://github.com/ignitionrobotics/ign-sensors/pull/91) @@ -214,7 +237,7 @@ 1. All features up to version 3.2.0. -### Ignition Sensors 4.0.0 (2020-09-30) +### Gazebo Sensors 4.0.0 (2020-09-30) 1. Fix link in README.md * [Pull request 51](https://github.com/ignitionrobotics/ign-sensors/pull/51) From 898570dc7fc874de1dc2a3895317b46ef950646f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 15 Feb 2023 23:37:03 +0100 Subject: [PATCH 11/26] Added Camera Info topic support for cameras (#285) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Added Camera Info topic support for cameras --------- Signed-off-by: ahcorde Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Ian Chen --- src/CameraSensor.cc | 23 ++++++++++++++--------- src/Camera_TEST.cc | 19 ++++++++++++------- 2 files changed, 26 insertions(+), 16 deletions(-) diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index e340c74d..e2cfa049 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -257,6 +257,11 @@ bool CameraSensor::Load(const sdf::Sensor &_sdf) if (this->Topic().empty()) this->SetTopic("/camera"); + if (!_sdf.CameraSensor()->CameraInfoTopic().empty()) + { + this->dataPtr->infoTopic = _sdf.CameraSensor()->CameraInfoTopic(); + } + this->dataPtr->pub = this->dataPtr->node.Advertise( this->Topic()); @@ -491,17 +496,17 @@ std::string CameraSensor::InfoTopic() const ////////////////////////////////////////////////// bool CameraSensor::AdvertiseInfo() { - // TODO(anyone) Make info topic configurable from SDF - // Info topic must be at same level as image topic - auto parts = common::Split(this->Topic(), '/'); - parts.pop_back(); - - for (const auto &part : parts) + if (this->dataPtr->infoTopic.empty()) { - if (!part.empty()) - this->dataPtr->infoTopic += "/" + part; + auto parts = common::Split(this->Topic(), '/'); + parts.pop_back(); + for (const auto &part : parts) + { + if (!part.empty()) + this->dataPtr->infoTopic += "/" + part; + } + this->dataPtr->infoTopic += "/camera_info"; } - this->dataPtr->infoTopic += "/camera_info"; return this->AdvertiseInfo(this->dataPtr->infoTopic); } diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 3c077019..912b22a3 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -48,7 +48,8 @@ sdf::ElementPtr cameraToBadSdf() sdf::ElementPtr CameraToSdf(const std::string &_type, const std::string &_name, double _updateRate, - const std::string &_topic, bool _alwaysOn, bool _visualize) + const std::string &_topic, const std::string &_cameraInfoTopic, + bool _alwaysOn, bool _visualize) { std::ostringstream stream; stream @@ -58,6 +59,7 @@ sdf::ElementPtr CameraToSdf(const std::string &_type, << " " << " " << " " << _topic << "" + << " " << _cameraInfoTopic << "" << " "<< _updateRate <<"" << " "<< _alwaysOn <<"" << " " << _visualize << "" @@ -146,8 +148,8 @@ TEST(Camera_TEST, CreateCamera) { gz::sensors::Manager mgr; - sdf::ElementPtr camSdf = CameraToSdf("camera", "my_camera", 60.0, "/cam", - true, true); + sdf::ElementPtr camSdf = CameraToSdf("camera", "my_camera", 60.0, + "/cam", "my_camera/camera_info", true, true); // Create a CameraSensor gz::sensors::CameraSensor *cam = @@ -190,8 +192,9 @@ TEST(Camera_TEST, Topic) // Default topic { const std::string topic; - auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, - visualize); + const std::string cameraInfoTopic; + auto cameraSdf = CameraToSdf(type, name, updateRate, topic, cameraInfoTopic, + alwaysOn, visualize); auto sensorId = mgr.CreateSensor(cameraSdf); EXPECT_NE(gz::sensors::NO_SENSOR, sensorId); @@ -203,12 +206,13 @@ TEST(Camera_TEST, Topic) ASSERT_NE(nullptr, camera); EXPECT_EQ("/camera", camera->Topic()); + EXPECT_EQ("/camera_info", camera->InfoTopic()); } // Convert to valid topic { const std::string topic = "/topic with spaces/@~characters//"; - auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, + auto cameraSdf = CameraToSdf(type, name, updateRate, topic, "", alwaysOn, visualize); auto sensorId = mgr.CreateSensor(cameraSdf); @@ -221,12 +225,13 @@ TEST(Camera_TEST, Topic) ASSERT_NE(nullptr, camera); EXPECT_EQ("/topic_with_spaces/characters", camera->Topic()); + EXPECT_EQ("/topic_with_spaces/camera_info", camera->InfoTopic()); } // Invalid topic { const std::string topic = "@@@"; - auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, + auto cameraSdf = CameraToSdf(type, name, updateRate, topic, "", alwaysOn, visualize); auto sensorId = mgr.CreateSensor(cameraSdf); From d2dff1a8da469257b9e38c18aaaac4ad5760945a Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 16 Feb 2023 15:03:29 -0800 Subject: [PATCH 12/26] 6.7.0 release (#323) Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ee11e054..b274d160 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(ignition-cmake2 2.13 REQUIRED) #============================================================================ ign_configure_project( REPLACE_IGNITION_INCLUDE_PATH gz/sensors - VERSION_SUFFIX pre1 + VERSION_SUFFIX ) #============================================================================ From e03feb1bd8ebf6a19adf86bafd51c6ec5b35c085 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 23 Feb 2023 02:57:47 -0800 Subject: [PATCH 13/26] clean up rendering resources (#324) Signed-off-by: Ian Chen --- src/RenderingSensor.cc | 2 +- test/integration/boundingbox_camera.cc | 8 ++++++++ test/integration/depth_camera.cc | 12 ++++++------ test/integration/gpu_lidar_sensor.cc | 24 +++++++++++++++++++++--- test/integration/rgbd_camera.cc | 10 ++++++---- test/integration/segmentation_camera.cc | 4 ++++ test/integration/thermal_camera.cc | 16 ++++++++-------- 7 files changed, 54 insertions(+), 22 deletions(-) diff --git a/src/RenderingSensor.cc b/src/RenderingSensor.cc index ad9cc32e..e02e1232 100644 --- a/src/RenderingSensor.cc +++ b/src/RenderingSensor.cc @@ -47,7 +47,7 @@ RenderingSensor::RenderingSensor() : ////////////////////////////////////////////////// RenderingSensor::~RenderingSensor() { - if (!this->dataPtr->scene) + if (!this->dataPtr->scene || !this->dataPtr->scene->IsInitialized()) return; for (auto &s : this->dataPtr->sensors) { diff --git a/test/integration/boundingbox_camera.cc b/test/integration/boundingbox_camera.cc index badb7610..e6108fcd 100644 --- a/test/integration/boundingbox_camera.cc +++ b/test/integration/boundingbox_camera.cc @@ -344,7 +344,11 @@ void BoundingBoxCameraSensorTest::BoxesWithBuiltinSDF( g_mutex.unlock(); + // Clean up rendering ptrs + camera.reset(); + // Clean up + mgr.Remove(sensor->Id()); engine->DestroyScene(scene); rendering::unloadEngine(engine->Name()); } @@ -465,7 +469,11 @@ void BoundingBoxCameraSensorTest::Boxes3DWithBuiltinSDF( g_mutex.unlock(); + // Clean up rendering ptrs + camera.reset(); + // Clean up + mgr.Remove(sensor->Id()); engine->DestroyScene(scene); rendering::unloadEngine(engine->Name()); } diff --git a/test/integration/depth_camera.cc b/test/integration/depth_camera.cc index ba4c6ca9..4f0c217a 100644 --- a/test/integration/depth_camera.cc +++ b/test/integration/depth_camera.cc @@ -222,6 +222,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( box->SetLocalRotation(0, 0, 0); box->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); box->SetMaterial(blue); + scene->DestroyMaterial(blue); root->AddChild(box); // do the test @@ -336,11 +337,9 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( EXPECT_EQ(9, infoMsg.rectification_matrix().size()); // Check that for a box really close it returns -inf - root->RemoveChild(box); gz::math::Vector3d boxPositionNear( unitBoxSize * 0.5 + near_ * 0.5, 0.0, 0.0); box->SetLocalPosition(boxPositionNear); - root->AddChild(box); mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; @@ -372,11 +371,9 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( g_mutex.unlock(); // Check that for a box really far it returns inf - root->RemoveChild(box); gz::math::Vector3d boxPositionFar( unitBoxSize * 0.5 + far_ * 1.5, 0.0, 0.0); box->SetLocalPosition(boxPositionFar); - root->AddChild(box); mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; @@ -408,11 +405,9 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( // Check that the depth values for a box do not warp. - root->RemoveChild(box); gz::math::Vector3d boxPositionFillFrame( unitBoxSize * 0.5 + 0.2, 0.0, 0.0); box->SetLocalPosition(boxPositionFillFrame); - root->AddChild(box); mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; @@ -497,7 +492,12 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( g_mutex.unlock(); g_pcMutex.unlock(); + // clean up rendering ptrs + blue.reset(); + box.reset(); + // Clean up + mgr.Remove(depthSensor->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } diff --git a/test/integration/gpu_lidar_sensor.cc b/test/integration/gpu_lidar_sensor.cc index 4edca6d3..570a15da 100644 --- a/test/integration/gpu_lidar_sensor.cc +++ b/test/integration/gpu_lidar_sensor.cc @@ -278,6 +278,7 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) // Clean up c.reset(); + mgr.Remove(sensor->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } @@ -416,8 +417,11 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) EXPECT_FALSE(pointMsgs.back().is_dense()); EXPECT_EQ(32u * horzSamples * vertSamples, pointMsgs.back().data().size()); + // Clean up rendering ptrs + visualBox1.reset(); + // Clean up - // + mgr.Remove(sensor->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } @@ -573,7 +577,14 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) for (unsigned int i = 0; i < sensor1->RayCount(); ++i) EXPECT_DOUBLE_EQ(sensor2->Range(i), gz::math::INF_D); + // Clean up rendering ptrs + visualBox1.reset(); + visualBox2.reset(); + visualBox3.reset(); + // Clean up + mgr.Remove(sensor1->Id()); + mgr.Remove(sensor2->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } @@ -693,7 +704,11 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) } } + // Clean up rendering ptrs + visualBox1.reset(); + // Clean up + mgr.Remove(sensor->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } @@ -819,8 +834,12 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) EXPECT_DOUBLE_EQ(sensor2->Range(last), gz::math::INF_D); #endif + // Clean up rendering ptrs + visualBox1.reset(); + // Clean up - // + mgr.Remove(sensor1->Id()); + mgr.Remove(sensor2->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } @@ -859,7 +878,6 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) // Create a GpuLidarSensor gz::sensors::Manager mgr; - // Default topic { const std::string topic; diff --git a/test/integration/rgbd_camera.cc b/test/integration/rgbd_camera.cc index a0e01ef6..cd319104 100644 --- a/test/integration/rgbd_camera.cc +++ b/test/integration/rgbd_camera.cc @@ -242,6 +242,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( box->SetLocalRotation(0, 0, 0); box->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); box->SetMaterial(blue); + scene->DestroyMaterial(blue); root->AddChild(box); // do the test @@ -529,11 +530,9 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( g_imgMutex.unlock(); // Check that for a box really close it returns -inf - root->RemoveChild(box); math::Vector3d boxPositionNear( unitBoxSize * 0.5 + near_ * 0.5, 0.0, 0.0); box->SetLocalPosition(boxPositionNear); - root->AddChild(box); mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; sleep < 300 && @@ -635,11 +634,9 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( g_pcMutex.unlock(); // Check that for a box really far it returns inf - root->RemoveChild(box); math::Vector3d boxPositionFar( unitBoxSize * 0.5 + far_ * 1.5, 0.0, 0.0); box->SetLocalPosition(boxPositionFar); - root->AddChild(box); mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; sleep < 300 && @@ -740,7 +737,12 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( g_imgMutex.unlock(); g_pcMutex.unlock(); + // Clean up rendering ptrs + box.reset(); + blue.reset(); + // Clean up + mgr.Remove(rgbdSensor->Id()); engine->DestroyScene(scene); rendering::unloadEngine(engine->Name()); } diff --git a/test/integration/segmentation_camera.cc b/test/integration/segmentation_camera.cc index 41771dd1..7f5cad1d 100644 --- a/test/integration/segmentation_camera.cc +++ b/test/integration/segmentation_camera.cc @@ -352,7 +352,11 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( } } + // Clean up rendering ptrs + camera.reset(); + // Clean up + mgr.Remove(sensor->Id()); engine->DestroyScene(scene); ignition::rendering::unloadEngine(engine->Name()); } diff --git a/test/integration/thermal_camera.cc b/test/integration/thermal_camera.cc index a87f26d8..18eec315 100644 --- a/test/integration/thermal_camera.cc +++ b/test/integration/thermal_camera.cc @@ -282,11 +282,9 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( EXPECT_EQ(9, infoMsg.rectification_matrix().size()); // Check that for a box really close it returns box temperature - root->RemoveChild(box); gz::math::Vector3d boxPositionNear( unitBoxSize * 0.5 + near_ * 0.5, 0.0, 0.0); box->SetLocalPosition(boxPositionNear); - root->AddChild(box); mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; @@ -316,11 +314,9 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( g_mutex.unlock(); // Check that for a box really far it returns ambient temperature - root->RemoveChild(box); gz::math::Vector3d boxPositionFar( unitBoxSize * 0.5 + far_ * 1.5, 0.0, 0.0); box->SetLocalPosition(boxPositionFar); - root->AddChild(box); mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; @@ -352,7 +348,11 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( delete [] g_thermalBuffer; g_thermalBuffer = nullptr; + // Clean up rendering ptrs + box.reset(); + // Clean up + mgr.Remove(thermalSensor->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } @@ -543,11 +543,9 @@ void ThermalCameraSensorTest::Images8BitWithBuiltinSDF( EXPECT_EQ(9, infoMsg.rectification_matrix().size()); // Check that for a box really close it returns box temperature - root->RemoveChild(box); gz::math::Vector3d boxPositionNear( unitBoxSize * 0.5 + near_ * 0.5, 0.0, 0.0); box->SetLocalPosition(boxPositionNear); - root->AddChild(box); mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; @@ -578,11 +576,9 @@ void ThermalCameraSensorTest::Images8BitWithBuiltinSDF( g_mutex.unlock(); // Check that for a box really far it returns ambient temperature - root->RemoveChild(box); ignition::math::Vector3d boxPositionFar( unitBoxSize * 0.5 + far_ * 1.5, 0.0, 0.0); box->SetLocalPosition(boxPositionFar); - root->AddChild(box); mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; @@ -614,7 +610,11 @@ void ThermalCameraSensorTest::Images8BitWithBuiltinSDF( delete [] g_thermalBuffer8Bit; g_thermalBuffer8Bit = nullptr; + // Clean up rendering ptrs + box.reset(); + // Clean up + mgr.Remove(thermalSensor->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } From 0bab3507e69e61194cf9dc79f267622d661bdf1f Mon Sep 17 00:00:00 2001 From: Valentina Vasco Date: Sat, 25 Feb 2023 01:35:48 +0100 Subject: [PATCH 14/26] Added trigger to BoundingBoxCamera (#322) This adds the possibility to trigger a BoundingBoxCamera. --------- Signed-off-by: Valentina Vasco Signed-off-by: Ian Chen Co-authored-by: Ian Chen --- include/gz/sensors/BoundingBoxCameraSensor.hh | 4 + include/gz/sensors/CameraSensor.hh | 5 + src/BoundingBoxCameraSensor.cc | 55 +++++ src/CameraSensor.cc | 12 +- test/integration/CMakeLists.txt | 1 + .../triggered_boundingbox_camera.cc | 230 ++++++++++++++++++ ...ered_boundingbox_camera_sensor_builtin.sdf | 24 ++ 7 files changed, 323 insertions(+), 8 deletions(-) create mode 100644 test/integration/triggered_boundingbox_camera.cc create mode 100644 test/sdf/triggered_boundingbox_camera_sensor_builtin.sdf diff --git a/include/gz/sensors/BoundingBoxCameraSensor.hh b/include/gz/sensors/BoundingBoxCameraSensor.hh index 80fb6686..4fe47496 100644 --- a/include/gz/sensors/BoundingBoxCameraSensor.hh +++ b/include/gz/sensors/BoundingBoxCameraSensor.hh @@ -103,6 +103,10 @@ namespace gz /// \return True on success. private: bool CreateCamera(); + /// \brief Callback for triggered subscription + /// \param[in] _msg Boolean message + private: void OnTrigger(const gz::msgs::Boolean &/*_msg*/); + GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Data pointer for private data /// \internal diff --git a/include/gz/sensors/CameraSensor.hh b/include/gz/sensors/CameraSensor.hh index 78ec0985..aae0ad45 100644 --- a/include/gz/sensors/CameraSensor.hh +++ b/include/gz/sensors/CameraSensor.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -175,6 +176,10 @@ namespace gz /// \param[in] _scene Pointer to the new scene. private: void OnSceneChange(gz::rendering::ScenePtr /*_scene*/); + /// \brief Callback for triggered subscription + /// \param[in] _msg Boolean message + private: void OnTrigger(const gz::msgs::Boolean &/*_msg*/); + GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Data pointer for private data /// \internal diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index 5ea79b8c..a7a26d35 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -85,6 +85,15 @@ class gz::sensors::BoundingBoxCameraSensorPrivate /// \brief Just a mutex for thread safety public: std::mutex mutex; + /// \brief True if camera is triggered by a topic + public: bool isTriggeredCamera{false}; + + /// \brief True if camera has been triggered by a topic + public: bool isTriggered{false}; + + /// \brief Topic for camera trigger + public: std::string triggerTopic{""}; + /// \brief BoundingBoxes type public: rendering::BoundingBoxType type {rendering::BoundingBoxType::BBT_VISIBLEBOX2D}; @@ -217,6 +226,33 @@ bool BoundingBoxCameraSensor::Load(const sdf::Sensor &_sdf) gzdbg << "Bounding boxes for [" << this->Name() << "] advertised on [" << topicBoundingBoxes << std::endl; + if (_sdf.CameraSensor()->Triggered()) + { + if (!_sdf.CameraSensor()->TriggerTopic().empty()) + { + this->dataPtr->triggerTopic = _sdf.CameraSensor()->TriggerTopic(); + } + else + { + this->dataPtr->triggerTopic = + transport::TopicUtils::AsValidTopic(this->dataPtr->triggerTopic); + + if (this->dataPtr->triggerTopic.empty()) + { + gzerr << "Invalid trigger topic name [" << + this->dataPtr->triggerTopic << "]" << std::endl; + return false; + } + } + + this->dataPtr->node.Subscribe(this->dataPtr->triggerTopic, + &BoundingBoxCameraSensor::OnTrigger, this); + + gzdbg << "Camera trigger messages for [" << this->Name() << "] subscribed" + << " on [" << this->dataPtr->triggerTopic << "]" << std::endl; + this->dataPtr->isTriggeredCamera = true; + } + if (!this->AdvertiseInfo()) return false; @@ -390,6 +426,13 @@ bool BoundingBoxCameraSensor::Update( this->PublishInfo(_now); } + // render only if necessary + if (this->dataPtr->isTriggeredCamera && + !this->dataPtr->isTriggered) + { + return true; + } + // don't render if there are no subscribers nor saving if (!this->dataPtr->imagePublisher.HasConnections() && !this->dataPtr->boxesPublisher.HasConnections() && @@ -528,6 +571,11 @@ bool BoundingBoxCameraSensor::Update( ++this->dataPtr->saveCounter; } + if (this->dataPtr->isTriggeredCamera) + { + return this->dataPtr->isTriggered = false; + } + return true; } @@ -543,6 +591,13 @@ unsigned int BoundingBoxCameraSensor::ImageWidth() const return this->dataPtr->rgbCamera->ImageWidth(); } +////////////////////////////////////////////////// +void BoundingBoxCameraSensor::OnTrigger(const gz::msgs::Boolean &/*_msg*/) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->isTriggered = true; +} + ////////////////////////////////////////////////// void BoundingBoxCameraSensorPrivate::SaveImage() { diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index db9cf5a4..1d0ca19b 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -49,10 +49,6 @@ using namespace sensors; /// \brief Private data for CameraSensor class gz::sensors::CameraSensorPrivate { - /// \brief Callback for triggered subscription - /// \param[in] _msg Boolean message - public: void OnTrigger(const msgs::Boolean &_msg); - /// \brief Save an image /// \param[in] _data the image data to be saved /// \param[in] _width width of image in pixels @@ -459,7 +455,7 @@ bool CameraSensor::Load(const sdf::Sensor &_sdf) } this->dataPtr->node.Subscribe(this->dataPtr->triggerTopic, - &CameraSensorPrivate::OnTrigger, this->dataPtr.get()); + &CameraSensor::OnTrigger, this); gzdbg << "Camera trigger messages for [" << this->Name() << "] subscribed" << " on [" << this->dataPtr->triggerTopic << "]" << std::endl; @@ -657,10 +653,10 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) } ////////////////////////////////////////////////// -void CameraSensorPrivate::OnTrigger(const gz::msgs::Boolean &/*_msg*/) +void CameraSensor::OnTrigger(const gz::msgs::Boolean &/*_msg*/) { - std::lock_guard lock(this->mutex); - this->isTriggered = true; + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->isTriggered = true; } ////////////////////////////////////////////////// diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index aaa5b908..ba4a1575 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -11,6 +11,7 @@ set(dri_tests thermal_camera.cc triggered_camera.cc wide_angle_camera.cc + triggered_boundingbox_camera.cc ) set(tests diff --git a/test/integration/triggered_boundingbox_camera.cc b/test/integration/triggered_boundingbox_camera.cc new file mode 100644 index 00000000..079fea51 --- /dev/null +++ b/test/integration/triggered_boundingbox_camera.cc @@ -0,0 +1,230 @@ +/* + * Copyright (C) 2023 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 + +// TODO(louise) Remove these pragmas once gz-rendering is disabling the +// warnings +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4251) +#endif +#include +#include +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include "test_config.hh" // NOLINT(build/include) +#include "TransportTestTools.hh" + +using namespace std::chrono_literals; + +class TriggeredBoundingBoxCameraTest: public testing::Test, + public testing::WithParamInterface +{ + // Documentation inherited + protected: void SetUp() override + { + // Disable Ogre tests on windows. See + // https://github.com/gazebosim/gz-sensors/issues/284 +#ifdef _WIN32 + if (strcmp(GetParam(), "ogre") == 0) + { + GTEST_SKIP() << "Ogre tests disabled on windows. See #284."; + } +#endif + gz::common::Console::SetVerbosity(4); + } + + // Create a BoundingBox Camera sensor from a SDF and gets a boxes message + public: void BoxesWithBuiltinSDF(const std::string &_renderEngine); + +}; + +/// \brief mutex for thread safety +std::mutex g_mutex; + +/// \brief bounding boxes from the camera +std::vector g_boxes; + +/// \brief callback to receive 2d boxes from the camera +void OnNewBoundingBoxes(const gz::msgs::AnnotatedAxisAligned2DBox_V &_boxes) +{ + g_mutex.lock(); + g_boxes.clear(); + + int size = _boxes.annotated_box_size(); + for (int i = 0; i < size; ++i) + { + auto annotated_box = _boxes.annotated_box(i); + g_boxes.push_back(annotated_box); + } + g_mutex.unlock(); +} + +/// \brief Build a scene with 2 visible boxes +void BuildScene(gz::rendering::ScenePtr _scene) +{ + gz::math::Vector3d box1Position(1, -1, 0); + gz::math::Vector3d box2Position(1, 1, 0); + + gz::rendering::VisualPtr root = _scene->RootVisual(); + + gz::rendering::VisualPtr box1 = _scene->CreateVisual(); + box1->AddGeometry(_scene->CreateBox()); + box1->SetOrigin(0.0, 0.0, 0.0); + box1->SetLocalPosition(box1Position); + box1->SetLocalRotation(0, 0, 0); + box1->SetUserData("label", 1); + root->AddChild(box1); + + gz::rendering::VisualPtr box2 = _scene->CreateVisual(); + box2->AddGeometry(_scene->CreateBox()); + box2->SetOrigin(0.0, 0.0, 0.0); + box2->SetLocalPosition(box2Position); + box2->SetLocalRotation(0, 0, 0); + box2->SetUserData("label", 2); + root->AddChild(box2); +} + +void TriggeredBoundingBoxCameraTest::BoxesWithBuiltinSDF( + const std::string &_renderEngine) +{ + std::string path = gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "triggered_boundingbox_camera_sensor_builtin.sdf"); + sdf::SDFPtr doc(new sdf::SDF()); + sdf::init(doc); + ASSERT_TRUE(sdf::readFile(path, doc)); + ASSERT_NE(nullptr, doc->Root()); + ASSERT_TRUE(doc->Root()->HasElement("model")); + auto modelPtr = doc->Root()->GetElement("model"); + ASSERT_TRUE(modelPtr->HasElement("link")); + auto linkPtr = modelPtr->GetElement("link"); + ASSERT_TRUE(linkPtr->HasElement("sensor")); + auto sensorPtr = linkPtr->GetElement("sensor"); + + // Skip unsupported engines + if (_renderEngine != "ogre2") + { + gzdbg << "Engine '" << _renderEngine + << "' doesn't support bounding box cameras" << std::endl; + return; + } + + // Setup gz-rendering with an empty scene + auto *engine = gz::rendering::engine(_renderEngine); + if (!engine) + { + gzdbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); + ASSERT_NE(nullptr, scene); + BuildScene(scene); + + gz::sensors::Manager mgr; + + sdf::Sensor sdfSensor; + sdfSensor.Load(sensorPtr); + + std::string type = sdfSensor.TypeStr(); + EXPECT_EQ(type, "boundingbox_camera"); + + gz::sensors::BoundingBoxCameraSensor *sensor = + mgr.CreateSensor(sdfSensor); + ASSERT_NE(sensor, nullptr); + EXPECT_FALSE(sensor->HasConnections()); + sensor->SetScene(scene); + + EXPECT_EQ(320u, sensor->ImageWidth()); + EXPECT_EQ(240u, sensor->ImageHeight()); + EXPECT_EQ(true, sdfSensor.CameraSensor()->Triggered()); + + // subscribe to the BoundingBox camera topic + gz::transport::Node node; + std::string boxTopic = + "/test/integration/TriggeredBBCameraPlugin_imagesWithBuiltinSDF"; + node.Subscribe(boxTopic, &OnNewBoundingBoxes); + + // we should not have image before trigger + { + std::string imageTopic = + "/test/integration/TriggeredBBCameraPlugin_imagesWithBuiltinSDF_image"; + WaitForMessageTestHelper helper(imageTopic); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + EXPECT_FALSE(helper.WaitForMessage(1s)) << helper; + g_mutex.lock(); + EXPECT_EQ(g_boxes.size(), size_t(0)); + g_mutex.unlock(); + } + + // trigger camera through topic + std::string triggerTopic = + "/test/integration/TriggeredBBCameraPlugin_imagesWithBuiltinSDF/trigger"; + + auto pub = node.Advertise(triggerTopic); + gz::msgs::Boolean msg; + msg.set_data(true); + pub.Publish(msg); + + // we should receive images and boxes after trigger + { + std::string imageTopic = + "/test/integration/TriggeredBBCameraPlugin_imagesWithBuiltinSDF_image"; + WaitForMessageTestHelper helper(imageTopic); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + EXPECT_TRUE(helper.WaitForMessage(10s)) << helper; + g_mutex.lock(); + EXPECT_EQ(g_boxes.size(), size_t(2)); + g_mutex.unlock(); + } + + // Clean up + engine->DestroyScene(scene); + gz::rendering::unloadEngine(engine->Name()); +} + +////////////////////////////////////////////////// +TEST_P(TriggeredBoundingBoxCameraTest, BoxesWithBuiltinSDF) +{ + BoxesWithBuiltinSDF(GetParam()); +} + +INSTANTIATE_TEST_SUITE_P(BoundingBoxCameraSensor, + TriggeredBoundingBoxCameraTest, + RENDER_ENGINE_VALUES, gz::rendering::PrintToStringParam()); + +////////////////////////////////////////////////// +int main(int argc, char **argv) +{ + gz::common::Console::SetVerbosity(4); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/sdf/triggered_boundingbox_camera_sensor_builtin.sdf b/test/sdf/triggered_boundingbox_camera_sensor_builtin.sdf new file mode 100644 index 00000000..83ae00fd --- /dev/null +++ b/test/sdf/triggered_boundingbox_camera_sensor_builtin.sdf @@ -0,0 +1,24 @@ + + + + + + 10 + /test/integration/TriggeredBBCameraPlugin_imagesWithBuiltinSDF + + true + /test/integration/TriggeredBBCameraPlugin_imagesWithBuiltinSDF/trigger + 1.05 + + 320 + 240 + + + 0.1 + 10.0 + + + + + + \ No newline at end of file From 6c242474b0ffc7892d1d9a591012e65cd0d40431 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 27 Feb 2023 13:35:58 -0800 Subject: [PATCH 15/26] Fix Camera info test (#326) * fix camera info test --------- Signed-off-by: Ian Chen --- src/Camera_TEST.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 912b22a3..d90402c5 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -59,11 +59,12 @@ sdf::ElementPtr CameraToSdf(const std::string &_type, << " " << " " << " " << _topic << "" - << " " << _cameraInfoTopic << "" << " "<< _updateRate <<"" << " "<< _alwaysOn <<"" << " " << _visualize << "" << " " + << " " << _cameraInfoTopic + << "" << " .75" << " " << " 640" @@ -160,7 +161,7 @@ TEST(Camera_TEST, CreateCamera) // Check topics EXPECT_EQ("/cam", cam->Topic()); - EXPECT_EQ("/camera_info", cam->InfoTopic()); + EXPECT_EQ("my_camera/camera_info", cam->InfoTopic()); // however camera is not loaded because a rendering scene is missing so // updates will not be successful and image size will be 0 From 2d52be7a0523555b1217270766f15023443d45da Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 28 Feb 2023 13:25:04 -0800 Subject: [PATCH 16/26] Fix flaky triggered bounding box camera test (#329) Signed-off-by: Ian Chen --- src/BoundingBoxCameraSensor.cc | 9 ++++++--- test/integration/triggered_boundingbox_camera.cc | 7 ++++--- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index a7a26d35..0adbfee7 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -427,10 +427,13 @@ bool BoundingBoxCameraSensor::Update( } // render only if necessary - if (this->dataPtr->isTriggeredCamera && - !this->dataPtr->isTriggered) { - return true; + std::lock_guard lock(this->dataPtr->mutex); + if (this->dataPtr->isTriggeredCamera && + !this->dataPtr->isTriggered) + { + return true; + } } // don't render if there are no subscribers nor saving diff --git a/test/integration/triggered_boundingbox_camera.cc b/test/integration/triggered_boundingbox_camera.cc index 079fea51..c6efa08f 100644 --- a/test/integration/triggered_boundingbox_camera.cc +++ b/test/integration/triggered_boundingbox_camera.cc @@ -193,12 +193,13 @@ void TriggeredBoundingBoxCameraTest::BoxesWithBuiltinSDF( gz::msgs::Boolean msg; msg.set_data(true); pub.Publish(msg); + // sleep to wait for trigger msg to be received before calling mgr.RunOnce + std::this_thread::sleep_for(2s); // we should receive images and boxes after trigger { - std::string imageTopic = - "/test/integration/TriggeredBBCameraPlugin_imagesWithBuiltinSDF_image"; - WaitForMessageTestHelper helper(imageTopic); + WaitForMessageTestHelper + helper(boxTopic); mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); EXPECT_TRUE(helper.WaitForMessage(10s)) << helper; g_mutex.lock(); From 07d3e0f8c1efb4e9e03f04d4baa1b3084477aaaf Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 2 Mar 2023 14:15:51 -0800 Subject: [PATCH 17/26] Fix links in Changelog (#330) Signed-off-by: Ian Chen --- Changelog.md | 84 ++++++++++++++++++++++++++-------------------------- 1 file changed, 42 insertions(+), 42 deletions(-) diff --git a/Changelog.md b/Changelog.md index c8e4b932..3ca00260 100644 --- a/Changelog.md +++ b/Changelog.md @@ -102,29 +102,29 @@ ### Gazebo Sensors 3.0.0 (2019-12-10) 1. Add support for sdformat frame semantics - * [BitBucket pull request 104](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/104) + * [BitBucket pull request 104](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/104) 1. Remove deprecations in ign-sensors3 - * [BitBucket pull request 103](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/103) + * [BitBucket pull request 103](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/103) 1. Break out image noise classes - * [BitBucket pull request 102](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/102) + * [BitBucket pull request 102](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/102) 1. Depend on ign-transport8, ign-msgs5, sdformat9 - * [BitBucket pull request 101](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/101) - * [BitBucket pull request 105](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/105) + * [BitBucket pull request 101](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/101) + * [BitBucket pull request 105](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/105) 1. Add Thermal Camera Sensor - * [BitBucket pull request 100](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/100) + * [BitBucket pull request 100](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/100) 1. Updating exports and includes - * [BitBucket pull request 98](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/98) + * [BitBucket pull request 98](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/98) 1. Removed deprecations from Manager. - * [BitBucket pull request 99](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/99) + * [BitBucket pull request 99](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/99) 1. Depend on ign-rendering3 - * [BitBucket pull request 88](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/88) + * [BitBucket pull request 88](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/88) ## Gazebo Sensors 2 @@ -150,123 +150,123 @@ ### Gazebo Sensors 2.8.0 (2020-03-04) 1. Added sequence numbers to sensor data messages. - * [BitBucket pull request 112](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/112) + * [BitBucket pull request 112](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/112) ### Gazebo Sensors 2.7.0 (2019-12-16) 1. Add clipping for depth camera on rgbd camera sensor (requires sdformat 8.7.0) - * [BitBucket pull request 107](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/107) + * [BitBucket pull request 107](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/107) ### Gazebo Sensors 2.6.1 (2019-09-13) 1. Fix IMU noise model dt - * [BitBucket pull request 94](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/94) + * [BitBucket pull request 94](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/94) ### Gazebo Sensors 2.6.0 (2019-08-27) 1. Update depth and rgbd camera sensor to output point cloud data generated by ign-rendering DepthCamera - * [BitBucket pull request 91](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/91) + * [BitBucket pull request 91](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/91) ### Gazebo Sensors 2.5.1 (2019-08-12) 1. Add intensity and ring fields to GpuLidarSensor point cloud msg - * [BitBucket pull request 89](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/89) + * [BitBucket pull request 89](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/89) ### Gazebo Sensors 2.5.0 1. Add `IGN_PROFILER_ENABLE` cmake option for enabling the ign-common profiler. - * [BitBucket pull request 82](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/82) + * [BitBucket pull request 82](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/82) 1. Deduplicate `frame_ids` from sensor message headers - * [BitBucket pull request 83](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/83) + * [BitBucket pull request 83](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/83) 1. Baseline for stereo cameras - * [BitBucket pull request 84](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/84) + * [BitBucket pull request 84](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/84) ### Gazebo Sensors 2.4.0 (2019-07-17) 1. Support manual scene updates for rendering sensors - * [BitBucket pull request 81](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/81) + * [BitBucket pull request 81](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/81) ### Gazebo Sensors 2.3.0 (2019-07-16) 1. The GpuLidar and Rgbd sensors publish point cloud data using `msgs::PointCloudPacked`. - * [BitBucket pull request 78](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/78) + * [BitBucket pull request 78](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/78) ### Gazebo Sensors 2.2.0 (2019-06-27) 1. Update the GPU Lidar to use the sensor's name as the `frame_id`. - * [BitBucket pull request 74](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/74) + * [BitBucket pull request 74](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/74) 1. Fix camera_info topic to be on the same level as image and depth_image for RGBD Camera. - * [BitBucket pull request 73](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/73) + * [BitBucket pull request 73](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/73) ### Gazebo Sensors 2.1.0 (2019-06-18) 1. Adds an RGBD camera sensor that combines a CameraSensor and DepthCameraSensor, and also outputs a pointcloud. - * [BitBucket pull request 70](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/70) + * [BitBucket pull request 70](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/70) 1. Create and publish on `camera_info` topics for the Camera and DepthCamera sensors. - * [BitBucket pull request 67](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/67) + * [BitBucket pull request 67](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/67) ### Gazebo Sensors 2.0.0 (2019-05-21) 1. Zero update rate, virtual SetParent and fix gpu_lidar - * [BitBucket pull request 66](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/66) + * [BitBucket pull request 66](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/66) 1. Add `frame_id` to sensor messages - * [BitBucket pull request 63](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/63) + * [BitBucket pull request 63](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/63) 1. Restore `pixel_format` in message and add deprecation comment. - * [BitBucket pull request 62](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/62) - * [BitBucket pull request 65](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/65) + * [BitBucket pull request 62](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/62) + * [BitBucket pull request 65](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/65) 1. Added noise to camera and lidar sensors. - * [BitBucket pull request 60](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/60) - * [BitBucket pull request 61](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/61) + * [BitBucket pull request 60](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/60) + * [BitBucket pull request 61](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/61) 1. Add support for loading a Lidar sensor from an SDF Sensor DOM object. - * [BitBucket pull request 59](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/59) + * [BitBucket pull request 59](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/59) 1. Add support for loading an IMU sensor from an SDF Sensor DOM object. - * [BitBucket pull request 58](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/58) + * [BitBucket pull request 58](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/58) 1. Add support for loading a camera and depth camera sensor from an SDF Sensor DOM object. - * [BitBucket pull request 57](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/57) + * [BitBucket pull request 57](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/57) 1. Add support for loading an air pressure sensor from an SDF Sensor DOM object. - * [BitBucket pull request 56](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/56) + * [BitBucket pull request 56](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/56) 1. Add support for loading an altimeter sensor from an SDF Sensor DOM object. - * [BitBucket pull request 55](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/55) + * [BitBucket pull request 55](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/55) 1. Noise factory uses `sdf::Noise` objects, Magnetometer sensor utilizes noise parameters. - * [BitBucket pull request 54](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/54) + * [BitBucket pull request 54](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/54) 1. Add support for loading a magnetometer sensor from an SDF Sensor DOM object. - * [BitBucket pull request 53](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/53) + * [BitBucket pull request 53](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/53) 1. Add magnetometer - * [BitBucket pull request 47](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/47) + * [BitBucket pull request 47](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/47) 1. Add IMU - * [BitBucket pull request 44](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/44) + * [BitBucket pull request 44](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/44) 1. Add altimeter - * [BitBucket pull request 43](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/43) + * [BitBucket pull request 43](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/43) 1. Create component for rendering sensor classes - * [BitBucket pull request 42](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/42) + * [BitBucket pull request 42](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/42) 1. Upgrade to gz-rendering2 - * [BitBucket pull request 45](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/45) + * [BitBucket pull request 45](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/45) 1. Upgrade to gz-msgs4 and gz-transport7 - * [BitBucket pull request 51](https://osrf-migration.github.io/ignition-gh-pages/#!/gazebosim/gz-sensors/pull-requests/51) + * [BitBucket pull request 51](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/51) ### Gazebo Sensors 1.X.X (2019-XX-XX) From 632804a2546ccabf0e3775c5d12df9017ca1f462 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 20 Mar 2023 23:46:27 -0700 Subject: [PATCH 18/26] Rename COPYING to LICENSE (#334) The LICENSE file contained a copy of the stanze used at the top of source code files, while the actual license was in the COPYING file. So remove the stanza and put the actual Apache 2.0 license text in LICENSE. Similar to gazebosim/gz-math#521. Signed-off-by: Steve Peters --- COPYING | 178 ----------------------------------------------------- LICENSE | 185 ++++++++++++++++++++++++++++++++++++++++++++++++++++---- 2 files changed, 174 insertions(+), 189 deletions(-) delete mode 100644 COPYING diff --git a/COPYING b/COPYING deleted file mode 100644 index 4909afd0..00000000 --- a/COPYING +++ /dev/null @@ -1,178 +0,0 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - diff --git a/LICENSE b/LICENSE index e3d1f5e4..4909afd0 100644 --- a/LICENSE +++ b/LICENSE @@ -1,15 +1,178 @@ -Software License Agreement (Apache License) + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ -Copyright 2017 Open Source Robotics Foundation + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION -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 + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS - 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. From cb182404eae1672a33620a860d6bad007dd02e00 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 24 Mar 2023 10:16:10 -0700 Subject: [PATCH 19/26] CI workflow: use checkout v3 (#335) Version v2 of the actions/checkout workflow is deprecated, so switch to v3. Part of gazebo-tooling/release-tools#862. Signed-off-by: Steve Peters --- .github/workflows/ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index e36175c3..744bfcff 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -8,7 +8,7 @@ jobs: name: Ubuntu Bionic CI steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@bionic @@ -19,7 +19,7 @@ jobs: name: Ubuntu Focal CI steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@focal From d412d9d820ddce84d1363ef28a6247c7711c96be Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 7 Apr 2023 14:59:57 -0700 Subject: [PATCH 20/26] destroy sensor in destructor (#340) Signed-off-by: Ian Chen --- src/CameraSensor.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index ff422656..afef5875 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -386,6 +386,10 @@ CameraSensor::CameraSensor() ////////////////////////////////////////////////// CameraSensor::~CameraSensor() { + if (this->Scene() && this->dataPtr->camera) + { + this->Scene()->DestroySensor(this->dataPtr->camera); + } } ////////////////////////////////////////////////// From ab0659bdc1ee87cf815550de31b1608c1a96f40c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 13 Apr 2023 11:55:16 -0700 Subject: [PATCH 21/26] prepare for 7.2.0 release (#341) Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- Changelog.md | 32 ++++++++++++++++++++++++++++++++ 2 files changed, 33 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b2c1e4f8..65983edc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(gz-sensors7 VERSION 7.1.0) +project(gz-sensors7 VERSION 7.2.0) #============================================================================ # Find gz-cmake diff --git a/Changelog.md b/Changelog.md index e60fcae1..dc2ba3bf 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,38 @@ ### Gazebo Sensors 7.X.X +### Gazebo Sensors 7.1.0 (2023-04-13) + +1. Cleanup resources in CameraSensor destructor + * [Pull request #340](https://github.com/gazebosim/gz-sensors/pull/340) + +1. CI workflow: use checkout v3 + * [Pull request #335](https://github.com/gazebosim/gz-sensors/pull/335) + +1. Rename COPYING to LICENSE + * [Pull request #334](https://github.com/gazebosim/gz-sensors/pull/334) + +1. Fix links in Changelog + * [Pull request #330](https://github.com/gazebosim/gz-sensors/pull/330) + +1. Fix flaky triggered bounding box camera test + * [Pull request #329](https://github.com/gazebosim/gz-sensors/pull/329) + +1. Fix Camera info test + * [Pull request #326](https://github.com/gazebosim/gz-sensors/pull/326) + +1. Added trigger to BoundingBoxCamera + * [Pull request #322](https://github.com/gazebosim/gz-sensors/pull/322) + +1. clean up rendering resources + * [Pull request #324](https://github.com/gazebosim/gz-sensors/pull/324) + +1. Added Camera Info topic support for cameras + * [Pull request #285](https://github.com/gazebosim/gz-sensors/pull/285) + +1. ign -> gz Migrate Ignition Headers : gz-sensors + * [Pull request #260](https://github.com/gazebosim/gz-sensors/pull/260) + ### Gazebo Sensors 7.1.0 (2023-02-09) 1. Added airspeed sensor From 7f864e5ee0cf8a662943356f4c0772b017827d3a Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 3 May 2023 09:50:06 -0500 Subject: [PATCH 22/26] Update version number in Changelog (#344) Signed-off-by: Addisu Z. Taddese --- Changelog.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Changelog.md b/Changelog.md index dc2ba3bf..dc0d1bfc 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,7 +2,7 @@ ### Gazebo Sensors 7.X.X -### Gazebo Sensors 7.1.0 (2023-04-13) +### Gazebo Sensors 7.2.0 (2023-04-13) 1. Cleanup resources in CameraSensor destructor * [Pull request #340](https://github.com/gazebosim/gz-sensors/pull/340) From 49d9aaa3dfc4d03f3709cafc6ef0602adf001c1e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 3 May 2023 11:42:56 -0700 Subject: [PATCH 23/26] Generate default trigger topic name if empty (#343) Signed-off-by: Ian Chen --- src/BoundingBoxCameraSensor.cc | 3 +- src/CameraSensor.cc | 3 +- .../triggered_boundingbox_camera.cc | 90 +++++++++++++++++++ test/integration/triggered_camera.cc | 77 ++++++++++++++++ ...oundingbox_camera_sensor_topic_builtin.sdf | 23 +++++ .../triggered_camera_sensor_topic_builtin.sdf | 23 +++++ 6 files changed, 217 insertions(+), 2 deletions(-) create mode 100644 test/sdf/triggered_boundingbox_camera_sensor_topic_builtin.sdf create mode 100644 test/sdf/triggered_camera_sensor_topic_builtin.sdf diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index 0adbfee7..777dc14a 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -235,7 +235,8 @@ bool BoundingBoxCameraSensor::Load(const sdf::Sensor &_sdf) else { this->dataPtr->triggerTopic = - transport::TopicUtils::AsValidTopic(this->dataPtr->triggerTopic); + transport::TopicUtils::AsValidTopic( + this->Topic() + "/trigger"); if (this->dataPtr->triggerTopic.empty()) { diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index afef5875..eeedd4f7 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -454,7 +454,8 @@ bool CameraSensor::Load(const sdf::Sensor &_sdf) else { this->dataPtr->triggerTopic = - transport::TopicUtils::AsValidTopic(this->dataPtr->triggerTopic); + transport::TopicUtils::AsValidTopic( + this->Topic() + "/trigger"); if (this->dataPtr->triggerTopic.empty()) { diff --git a/test/integration/triggered_boundingbox_camera.cc b/test/integration/triggered_boundingbox_camera.cc index 424e7e51..2cc37558 100644 --- a/test/integration/triggered_boundingbox_camera.cc +++ b/test/integration/triggered_boundingbox_camera.cc @@ -64,6 +64,8 @@ class TriggeredBoundingBoxCameraTest: public testing::Test, // Create a BoundingBox Camera sensor from a SDF and gets a boxes message public: void BoxesWithBuiltinSDF(const std::string &_renderEngine); + // Create a BoundingBox Camera sensor from a SDF with empty trigger topic + public: void EmptyTriggerTopic(const std::string &_renderEngine); }; /// \brief mutex for thread safety @@ -213,12 +215,100 @@ void TriggeredBoundingBoxCameraTest::BoxesWithBuiltinSDF( gz::rendering::unloadEngine(engine->Name()); } +void TriggeredBoundingBoxCameraTest::EmptyTriggerTopic( + const std::string &_renderEngine) +{ + std::string path = gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "triggered_boundingbox_camera_sensor_topic_builtin.sdf"); + sdf::SDFPtr doc(new sdf::SDF()); + sdf::init(doc); + ASSERT_TRUE(sdf::readFile(path, doc)); + ASSERT_NE(nullptr, doc->Root()); + ASSERT_TRUE(doc->Root()->HasElement("model")); + auto modelPtr = doc->Root()->GetElement("model"); + ASSERT_TRUE(modelPtr->HasElement("link")); + auto linkPtr = modelPtr->GetElement("link"); + ASSERT_TRUE(linkPtr->HasElement("sensor")); + auto sensorPtr = linkPtr->GetElement("sensor"); + + // Skip unsupported engines + if (_renderEngine != "ogre2") + { + gzdbg << "Engine '" << _renderEngine + << "' doesn't support bounding box cameras" << std::endl; + return; + } + + // Setup gz-rendering with an empty scene + auto *engine = gz::rendering::engine(_renderEngine); + if (!engine) + { + gzdbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); + ASSERT_NE(nullptr, scene); + BuildScene(scene); + + gz::sensors::Manager mgr; + + sdf::Sensor sdfSensor; + sdfSensor.Load(sensorPtr); + + std::string type = sdfSensor.TypeStr(); + EXPECT_EQ(type, "boundingbox_camera"); + + gz::sensors::BoundingBoxCameraSensor *sensor = + mgr.CreateSensor(sdfSensor); + ASSERT_NE(sensor, nullptr); + EXPECT_FALSE(sensor->HasConnections()); + sensor->SetScene(scene); + + // trigger camera through topic + std::string triggerTopic = + "/test/integration/triggered_bbcamera/trigger"; + + gz::transport::Node node; + auto pub = node.Advertise(triggerTopic); + gz::msgs::Boolean msg; + msg.set_data(true); + pub.Publish(msg); + // sleep to wait for trigger msg to be received before calling mgr.RunOnce + std::this_thread::sleep_for(2s); + + // we should receive images and boxes after trigger + { + std::string boxTopic = + "/test/integration/triggered_bbcamera"; + WaitForMessageTestHelper + helper(boxTopic); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + EXPECT_TRUE(helper.WaitForMessage(10s)) << helper; + g_mutex.lock(); + EXPECT_EQ(g_boxes.size(), size_t(2)); + g_mutex.unlock(); + } + + // Clean up + mgr.Remove(sensor->Id()); + engine->DestroyScene(scene); + gz::rendering::unloadEngine(engine->Name()); +} + ////////////////////////////////////////////////// TEST_P(TriggeredBoundingBoxCameraTest, BoxesWithBuiltinSDF) { BoxesWithBuiltinSDF(GetParam()); } +////////////////////////////////////////////////// +TEST_P(TriggeredBoundingBoxCameraTest, EmptyTriggerTopic) +{ + EmptyTriggerTopic(GetParam()); +} + INSTANTIATE_TEST_SUITE_P(BoundingBoxCameraSensor, TriggeredBoundingBoxCameraTest, RENDER_ENGINE_VALUES, gz::rendering::PrintToStringParam()); diff --git a/test/integration/triggered_camera.cc b/test/integration/triggered_camera.cc index 31d86be6..8aa2eec1 100644 --- a/test/integration/triggered_camera.cc +++ b/test/integration/triggered_camera.cc @@ -63,6 +63,9 @@ class TriggeredCameraTest: public testing::Test, // Create a Camera sensor from a SDF and gets a image message public: void ImagesWithBuiltinSDF(const std::string &_renderEngine); + + // Create a Camera sensor from a SDF with empty trigger topic + public: void EmptyTriggerTopic(const std::string &_renderEngine); }; void TriggeredCameraTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) @@ -128,6 +131,9 @@ void TriggeredCameraTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) msg.set_data(true); pub.Publish(msg); + // sleep to wait for trigger msg to be received before calling mgr.RunOnce + std::this_thread::sleep_for(2s); + // check camera image after trigger { std::string imageTopic = @@ -154,6 +160,70 @@ void TriggeredCameraTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) gz::rendering::unloadEngine(engine->Name()); } +void TriggeredCameraTest::EmptyTriggerTopic(const std::string &_renderEngine) +{ + std::string path = gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "triggered_camera_sensor_topic_builtin.sdf"); + sdf::SDFPtr doc(new sdf::SDF()); + sdf::init(doc); + ASSERT_TRUE(sdf::readFile(path, doc)); + ASSERT_NE(nullptr, doc->Root()); + ASSERT_TRUE(doc->Root()->HasElement("model")); + auto modelPtr = doc->Root()->GetElement("model"); + ASSERT_TRUE(modelPtr->HasElement("link")); + auto linkPtr = modelPtr->GetElement("link"); + ASSERT_TRUE(linkPtr->HasElement("sensor")); + auto sensorPtr = linkPtr->GetElement("sensor"); + + // Setup gz-rendering with an empty scene + auto *engine = gz::rendering::engine(_renderEngine); + if (!engine) + { + gzdbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); + + gz::sensors::Manager mgr; + + gz::sensors::CameraSensor *sensor = + mgr.CreateSensor(sensorPtr); + ASSERT_NE(sensor, nullptr); + EXPECT_FALSE(sensor->HasConnections()); + sensor->SetScene(scene); + + sdf::Sensor sdfSensor; + sdfSensor.Load(sensorPtr); + EXPECT_EQ(true, sdfSensor.CameraSensor()->Triggered()); + + // trigger camera through default generated topic + gz::transport::Node triggerNode; + std::string triggerTopic = + "/test/integration/triggered_camera/trigger"; + + auto pub = triggerNode.Advertise(triggerTopic); + gz::msgs::Boolean msg; + msg.set_data(true); + pub.Publish(msg); + + // check camera image after trigger + { + std::string imageTopic = + "/test/integration/triggered_camera"; + WaitForMessageTestHelper helper(imageTopic); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + EXPECT_TRUE(helper.WaitForMessage(10s)) << helper; + } + + // Clean up + auto sensorId = sensor->Id(); + EXPECT_TRUE(mgr.Remove(sensorId)); + engine->DestroyScene(scene); + gz::rendering::unloadEngine(engine->Name()); +} + ////////////////////////////////////////////////// TEST_P(TriggeredCameraTest, ImagesWithBuiltinSDF) { @@ -161,5 +231,12 @@ TEST_P(TriggeredCameraTest, ImagesWithBuiltinSDF) ImagesWithBuiltinSDF(GetParam()); } +////////////////////////////////////////////////// +TEST_P(TriggeredCameraTest, EmptyTriggerTopic) +{ + gz::common::Console::SetVerbosity(4); + EmptyTriggerTopic(GetParam()); +} + INSTANTIATE_TEST_SUITE_P(CameraSensor, TriggeredCameraTest, RENDER_ENGINE_VALUES, gz::rendering::PrintToStringParam()); diff --git a/test/sdf/triggered_boundingbox_camera_sensor_topic_builtin.sdf b/test/sdf/triggered_boundingbox_camera_sensor_topic_builtin.sdf new file mode 100644 index 00000000..cf6a1448 --- /dev/null +++ b/test/sdf/triggered_boundingbox_camera_sensor_topic_builtin.sdf @@ -0,0 +1,23 @@ + + + + + + 10 + /test/integration/triggered_bbcamera + + true + 1.05 + + 320 + 240 + + + 0.1 + 10.0 + + + + + + diff --git a/test/sdf/triggered_camera_sensor_topic_builtin.sdf b/test/sdf/triggered_camera_sensor_topic_builtin.sdf new file mode 100644 index 00000000..84a9943a --- /dev/null +++ b/test/sdf/triggered_camera_sensor_topic_builtin.sdf @@ -0,0 +1,23 @@ + + + + + + 10 + /test/integration/triggered_camera + + true + 1.05 + + 256 + 257 + + + 0.1 + 10.0 + + + + + + From f88ac6076500b535dfb2f2501daee936b7b93670 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 17 May 2023 10:03:18 -0700 Subject: [PATCH 24/26] Fix flaky trigger camera test (#346) Signed-off-by: Ian Chen --- test/integration/triggered_camera.cc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/test/integration/triggered_camera.cc b/test/integration/triggered_camera.cc index 8aa2eec1..873fe486 100644 --- a/test/integration/triggered_camera.cc +++ b/test/integration/triggered_camera.cc @@ -208,6 +208,9 @@ void TriggeredCameraTest::EmptyTriggerTopic(const std::string &_renderEngine) msg.set_data(true); pub.Publish(msg); + // sleep to wait for trigger msg to be received before calling mgr.RunOnce + std::this_thread::sleep_for(2s); + // check camera image after trigger { std::string imageTopic = From 60dac3da4deac58692ec6a7edb80c37019e651e5 Mon Sep 17 00:00:00 2001 From: Tejal Ashwini Barnwal <64950661+tejalbarnwal@users.noreply.github.com> Date: Mon, 22 May 2023 13:17:03 +0530 Subject: [PATCH 25/26] Add support for bayer images to camera sensor (#336) Signed-off-by: tejalbarnwal --- src/CameraSensor.cc | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index eeedd4f7..977c7bdc 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -278,6 +278,18 @@ bool CameraSensor::CreateCamera() case sdf::PixelFormatType::L_INT16: this->dataPtr->camera->SetImageFormat(rendering::PF_L16); break; + case sdf::PixelFormatType::BAYER_RGGB8: + this->dataPtr->camera->SetImageFormat(rendering::PF_BAYER_RGGB8); + break; + case sdf::PixelFormatType::BAYER_BGGR8: + this->dataPtr->camera->SetImageFormat(rendering::PF_BAYER_BGGR8); + break; + case sdf::PixelFormatType::BAYER_GBRG8: + this->dataPtr->camera->SetImageFormat(rendering::PF_BAYER_GBRG8); + break; + case sdf::PixelFormatType::BAYER_GRBG8: + this->dataPtr->camera->SetImageFormat(rendering::PF_BAYER_GRBG8); + break; default: gzerr << "Unsupported pixel format [" << static_cast(pixelFormat) << "]\n"; @@ -605,6 +617,22 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) format = common::Image::L_INT16; msgsPixelFormat = msgs::PixelFormatType::L_INT16; break; + case rendering::PF_BAYER_RGGB8: + format = common::Image::BAYER_RGGB8; + msgsPixelFormat = msgs::PixelFormatType::BAYER_RGGB8; + break; + case rendering::PF_BAYER_BGGR8: + format = common::Image::BAYER_BGGR8; + msgsPixelFormat = msgs::PixelFormatType::BAYER_BGGR8; + break; + case rendering::PF_BAYER_GBRG8: + format = common::Image::BAYER_GBRG8; + msgsPixelFormat = msgs::PixelFormatType::BAYER_GBRG8; + break; + case rendering::PF_BAYER_GRBG8: + format = common::Image::BAYER_GRBG8; + msgsPixelFormat = msgs::PixelFormatType::BAYER_GRBG8; + break; default: gzerr << "Unsupported pixel format [" << this->dataPtr->camera->ImageFormat() << "]\n"; From 68c183921a585e569fc414ef5ab12da74ffe2777 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 6 Jun 2023 12:08:20 -0700 Subject: [PATCH 26/26] Fix dvl integration test (#348) --------- Signed-off-by: Ian Chen --- src/Manager.cc | 1 + test/integration/dvl.cc | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/src/Manager.cc b/src/Manager.cc index 84052c0f..a68c5149 100644 --- a/src/Manager.cc +++ b/src/Manager.cc @@ -42,6 +42,7 @@ Manager::Manager() : ////////////////////////////////////////////////// Manager::~Manager() { + this->dataPtr->sensors.clear(); } ////////////////////////////////////////////////// diff --git a/test/integration/dvl.cc b/test/integration/dvl.cc index bf697acd..ab12ba73 100644 --- a/test/integration/dvl.cc +++ b/test/integration/dvl.cc @@ -342,6 +342,8 @@ TEST_P(DopplerVelocityLogTest, BottomTrackingWhileStatic) 4 * config.trackingNoise)); } EXPECT_EQ(0, message.status()); + + this->manager.Remove(sensor->Id()); } ///////////////////////////////////////////////// @@ -431,6 +433,8 @@ TEST_P(DopplerVelocityLogTest, WaterMassTrackingWhileStatic) EXPECT_EQ(velocityReference, message.beams(i).velocity().reference()); } EXPECT_EQ(0, message.status()); + + this->manager.Remove(sensor->Id()); } ///////////////////////////////////////////////// @@ -512,6 +516,8 @@ TEST_P(DopplerVelocityLogTest, BottomTrackingWhileInMotion) EXPECT_EQ(velocityReference, message.beams(i).velocity().reference()); } EXPECT_EQ(0, message.status()); + + this->manager.Remove(sensor->Id()); } INSTANTIATE_TEST_SUITE_P(DopplerVelocityLogTests, DopplerVelocityLogTest,