From ccedda379c61ce6a8f48fccb58a79b39093cae11 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 4 Feb 2021 17:58:25 -0800 Subject: [PATCH 01/18] Use ign-plugin (alternative to #38) Signed-off-by: Louise Poubel --- Migration.md | 8 +++++ include/ignition/sensors/CameraSensor.hh | 1 - include/ignition/sensors/DepthCameraSensor.hh | 1 - .../ignition/sensors/LogicalCameraSensor.hh | 1 - include/ignition/sensors/SensorFactory.hh | 10 ------ .../ignition/sensors/ThermalCameraSensor.hh | 1 - src/AirPressureSensor.cc | 5 ++- src/AltimeterSensor.cc | 5 ++- src/CMakeLists.txt | 24 ++++++++++++++ src/CameraSensor.cc | 5 ++- src/DepthCameraSensor.cc | 6 +++- src/GpuLidarSensor.cc | 5 ++- src/ImuSensor.cc | 5 ++- src/Lidar.cc | 5 ++- src/LogicalCameraSensor.cc | 9 +++--- src/MagnetometerSensor.cc | 5 ++- src/Manager.cc | 2 -- src/RgbdCameraSensor.cc | 5 ++- src/SensorFactory.cc | 31 ++++++++++++------- src/ThermalCameraSensor.cc | 6 +++- test/integration/CMakeLists.txt | 2 ++ 21 files changed, 100 insertions(+), 42 deletions(-) diff --git a/Migration.md b/Migration.md index 1ca8c577..e9230a53 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,14 @@ Deprecated code produces compile-time warnings. These warning serve as notification to users that their code should be upgraded. The next major release will remove the deprecated code. +## Ignition Sensors 4.X to 5.X + +* Plugins now use Ignition Plugin instead of Ignition Common's plugin framework. + * Macro `IGN_SENSORS_REGISTER_SENSOR` has been removed. Use `IGNITION_ADD_PLUGIN` instead. + + ***Deprecation*** IGN_SENSORS_REGISTER_SENSOR(SensorName) + + ***Replacement*** IGNITION_ADD_PLUGIN(ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) + * Use `#include ` + * Privately link against `ignition-plugin${IGN_PLUGIN_VER}::register` ## Ignition Sensors 3.X to 4.X diff --git a/include/ignition/sensors/CameraSensor.hh b/include/ignition/sensors/CameraSensor.hh index 53423575..ae84124e 100644 --- a/include/ignition/sensors/CameraSensor.hh +++ b/include/ignition/sensors/CameraSensor.hh @@ -23,7 +23,6 @@ #include -#include #include #include diff --git a/include/ignition/sensors/DepthCameraSensor.hh b/include/ignition/sensors/DepthCameraSensor.hh index 59d6b64e..dc25b3f3 100644 --- a/include/ignition/sensors/DepthCameraSensor.hh +++ b/include/ignition/sensors/DepthCameraSensor.hh @@ -24,7 +24,6 @@ #include #include -#include #include #include diff --git a/include/ignition/sensors/LogicalCameraSensor.hh b/include/ignition/sensors/LogicalCameraSensor.hh index 338e587f..c07f0d41 100644 --- a/include/ignition/sensors/LogicalCameraSensor.hh +++ b/include/ignition/sensors/LogicalCameraSensor.hh @@ -23,7 +23,6 @@ #include -#include #include #include diff --git a/include/ignition/sensors/SensorFactory.hh b/include/ignition/sensors/SensorFactory.hh index 32004a21..188836fb 100644 --- a/include/ignition/sensors/SensorFactory.hh +++ b/include/ignition/sensors/SensorFactory.hh @@ -23,7 +23,6 @@ #include #include -#include #include #include @@ -43,9 +42,6 @@ namespace ignition /// \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; @@ -182,12 +178,6 @@ namespace ignition 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) } } } diff --git a/include/ignition/sensors/ThermalCameraSensor.hh b/include/ignition/sensors/ThermalCameraSensor.hh index fddb2ca9..ece4ddca 100644 --- a/include/ignition/sensors/ThermalCameraSensor.hh +++ b/include/ignition/sensors/ThermalCameraSensor.hh @@ -24,7 +24,6 @@ #include #include -#include #include #include diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc index 9982e9e3..f0f56523 100644 --- a/src/AirPressureSensor.cc +++ b/src/AirPressureSensor.cc @@ -26,6 +26,7 @@ #endif #include +#include #include #include "ignition/sensors/GaussianNoiseModel.hh" @@ -224,4 +225,6 @@ double AirPressureSensor::ReferenceAltitude() const return this->dataPtr->referenceAltitude; } -IGN_SENSORS_REGISTER_SENSOR(AirPressureSensor) +IGNITION_ADD_PLUGIN( + ignition::sensors::SensorTypePlugin, + ignition::sensors::SensorPlugin) diff --git a/src/AltimeterSensor.cc b/src/AltimeterSensor.cc index 6391380c..18040102 100644 --- a/src/AltimeterSensor.cc +++ b/src/AltimeterSensor.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include "ignition/sensors/AltimeterSensor.hh" @@ -219,4 +220,6 @@ double AltimeterSensor::VerticalVelocity() const return this->dataPtr->verticalVelocity; } -IGN_SENSORS_REGISTER_SENSOR(AltimeterSensor) +IGNITION_ADD_PLUGIN( + ignition::sensors::SensorTypePlugin, + ignition::sensors::SensorPlugin) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 97a38024..1efdf35d 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -43,6 +43,7 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} sdformat${SDF_VER}::sdformat${SDF_VER} PRIVATE ignition-common${IGN_COMMON_VER}::profiler + ignition-plugin${IGN_PLUGIN_VER}::loader ) target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PUBLIC DepthPoints_EXPORTS) @@ -62,6 +63,7 @@ target_link_libraries(${camera_target} ${rendering_target} PRIVATE ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} + ignition-plugin${IGN_PLUGIN_VER}::register ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ) @@ -72,6 +74,7 @@ target_link_libraries(${depth_camera_target} PRIVATE ${camera_target} ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} + ignition-plugin${IGN_PLUGIN_VER}::register ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ) @@ -83,6 +86,7 @@ target_link_libraries(${lidar_target} ${rendering_target} PRIVATE ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} + ignition-plugin${IGN_PLUGIN_VER}::register ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ) @@ -92,6 +96,7 @@ target_compile_definitions(${gpu_lidar_target} PUBLIC GpuLidarSensor_EXPORTS) target_link_libraries(${gpu_lidar_target} PRIVATE ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} + ignition-plugin${IGN_PLUGIN_VER}::register ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ${lidar_target} ) @@ -102,20 +107,37 @@ target_compile_definitions(${logical_camera_target} PUBLIC LogicalCameraSensor_E target_link_libraries(${logical_camera_target} PRIVATE ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} + ignition-plugin${IGN_PLUGIN_VER}::register ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ) set(magnetometer_sources MagnetometerSensor.cc) ign_add_component(magnetometer SOURCES ${magnetometer_sources} GET_TARGET_NAME magnetometer_target) +target_link_libraries(${magnetometer_target} + PRIVATE + ignition-plugin${IGN_PLUGIN_VER}::register +) set(imu_sources ImuSensor.cc) ign_add_component(imu SOURCES ${imu_sources} GET_TARGET_NAME imu_target) +target_link_libraries(${imu_target} + PRIVATE + ignition-plugin${IGN_PLUGIN_VER}::register +) set(altimeter_sources AltimeterSensor.cc) ign_add_component(altimeter SOURCES ${altimeter_sources} GET_TARGET_NAME altimeter_target) +target_link_libraries(${altimeter_target} + PRIVATE + ignition-plugin${IGN_PLUGIN_VER}::register +) set(air_pressure_sources AirPressureSensor.cc) ign_add_component(air_pressure SOURCES ${air_pressure_sources} GET_TARGET_NAME air_pressure_target) +target_link_libraries(${air_pressure_target} + PRIVATE + ignition-plugin${IGN_PLUGIN_VER}::register +) set(rgbd_camera_sources RgbdCameraSensor.cc) ign_add_component(rgbd_camera SOURCES ${rgbd_camera_sources} GET_TARGET_NAME rgbd_camera_target) @@ -124,6 +146,7 @@ target_link_libraries(${rgbd_camera_target} PRIVATE ${camera_target} ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} + ignition-plugin${IGN_PLUGIN_VER}::register ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ) @@ -133,6 +156,7 @@ target_link_libraries(${thermal_camera_target} PRIVATE ${camera_target} ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} + ignition-plugin${IGN_PLUGIN_VER}::register ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ) diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 846cf34f..3ecb9c3a 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -33,6 +33,7 @@ #include #include #include +#include #include #include "ignition/sensors/CameraSensor.hh" @@ -647,4 +648,6 @@ double CameraSensor::Baseline() const return this->dataPtr->baseline; } -IGN_SENSORS_REGISTER_SENSOR(CameraSensor) +IGNITION_ADD_PLUGIN( + ignition::sensors::SensorTypePlugin, + ignition::sensors::SensorPlugin) diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 561e7b78..921d9240 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -34,6 +34,8 @@ #include #include +#include + #include #include "ignition/sensors/DepthCameraSensor.hh" @@ -632,4 +634,6 @@ double DepthCameraSensor::NearClip() const return this->dataPtr->near; } -IGN_SENSORS_REGISTER_SENSOR(DepthCameraSensor) +IGNITION_ADD_PLUGIN( + ignition::sensors::SensorTypePlugin, + ignition::sensors::SensorPlugin) diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc index e72f3868..4ac7c69f 100644 --- a/src/GpuLidarSensor.cc +++ b/src/GpuLidarSensor.cc @@ -28,6 +28,7 @@ #include #include #include +#include #include #include "ignition/sensors/GpuLidarSensor.hh" @@ -372,4 +373,6 @@ void GpuLidarSensorPrivate::FillPointCloudMsg() } } -IGN_SENSORS_REGISTER_SENSOR(GpuLidarSensor) +IGNITION_ADD_PLUGIN( + ignition::sensors::SensorTypePlugin, + ignition::sensors::SensorPlugin) diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index ff31c3e6..c08bfde4 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -25,6 +25,7 @@ #endif #include +#include #include #include "ignition/sensors/ImuSensor.hh" @@ -302,4 +303,6 @@ math::Quaterniond ImuSensor::Orientation() const return this->dataPtr->orientation; } -IGN_SENSORS_REGISTER_SENSOR(ImuSensor) +IGNITION_ADD_PLUGIN( + ignition::sensors::SensorTypePlugin, + ignition::sensors::SensorPlugin) diff --git a/src/Lidar.cc b/src/Lidar.cc index 3dbd0ba1..89996ae6 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -448,4 +449,6 @@ bool Lidar::IsActive() const return true; } -IGN_SENSORS_REGISTER_SENSOR(Lidar) +IGNITION_ADD_PLUGIN( + ignition::sensors::SensorTypePlugin, + ignition::sensors::SensorPlugin) diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc index 75294581..c9b79791 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -19,11 +19,10 @@ #include #include - -#include - #include #include +#include +#include #include "ignition/sensors/SensorFactory.hh" #include "ignition/sensors/LogicalCameraSensor.hh" @@ -209,4 +208,6 @@ msgs::LogicalCameraImage LogicalCameraSensor::Image() const return this->dataPtr->msg; } -IGN_SENSORS_REGISTER_SENSOR(LogicalCameraSensor) +IGNITION_ADD_PLUGIN( + ignition::sensors::SensorTypePlugin, + ignition::sensors::SensorPlugin) diff --git a/src/MagnetometerSensor.cc b/src/MagnetometerSensor.cc index 236b3165..f82089d5 100644 --- a/src/MagnetometerSensor.cc +++ b/src/MagnetometerSensor.cc @@ -26,6 +26,7 @@ #endif #include +#include #include #include @@ -238,4 +239,6 @@ math::Vector3d MagnetometerSensor::MagneticField() const return this->dataPtr->localField; } -IGN_SENSORS_REGISTER_SENSOR(MagnetometerSensor) +IGNITION_ADD_PLUGIN( + ignition::sensors::SensorTypePlugin, + ignition::sensors::SensorPlugin) diff --git a/src/Manager.cc b/src/Manager.cc index 9f337279..8d616f1d 100644 --- a/src/Manager.cc +++ b/src/Manager.cc @@ -18,8 +18,6 @@ #include "ignition/sensors/Manager.hh" #include #include -#include -#include #include #include #include diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 90389d8b..3b1856dc 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -42,6 +42,7 @@ #pragma warning(pop) #endif +#include #include #include @@ -609,4 +610,6 @@ unsigned int RgbdCameraSensor::ImageHeight() const return this->dataPtr->depthCamera->ImageHeight(); } -IGN_SENSORS_REGISTER_SENSOR(RgbdCameraSensor) +IGNITION_ADD_PLUGIN( + ignition::sensors::SensorTypePlugin, + ignition::sensors::SensorPlugin) diff --git a/src/SensorFactory.cc b/src/SensorFactory.cc index 5e9cfc58..99e2de41 100644 --- a/src/SensorFactory.cc +++ b/src/SensorFactory.cc @@ -15,9 +15,9 @@ * */ -#include #include #include +#include #include "ignition/sensors/config.hh" #include "ignition/sensors/SensorFactory.hh" @@ -33,9 +33,6 @@ class ignition::sensors::SensorFactoryPrivate /// \brief Stores paths to search for on file system public: ignition::common::SystemPaths systemPaths; - - /// \brief For loading plugins - public: ignition::common::PluginLoader pluginLoader; }; using namespace ignition; @@ -75,21 +72,31 @@ std::shared_ptr SensorFactory::LoadSensorPlugin( return std::shared_ptr(); } - auto pluginNames = this->dataPtr->pluginLoader.LoadLibrary(fullPath); + plugin::Loader pluginLoader; + auto pluginNames = pluginLoader.LoadLib(fullPath); + if (pluginNames.empty()) { ignerr << "Unable to load sensor plugin file for [" << fullPath << "]\n"; return std::shared_ptr(); } - // Assume the first plugin is the one we're interested in - std::string pluginName = *(pluginNames.begin()); - - common::PluginPtr pluginPtr = - this->dataPtr->pluginLoader.Instantiate(pluginName); + // Go over all plugin names and get the first one that implements the + // ignition::sensors::SensorPlugin interface + plugin::PluginPtr plugin; + std::shared_ptr sensorPlugin{nullptr}; + for (auto pluginName : pluginNames) + { + plugin = pluginLoader.Instantiate(pluginName); + if (!plugin) + continue; + + sensorPlugin = + plugin->QueryInterfaceSharedPtr(); + if (sensorPlugin) + break; + } - auto sensorPlugin = pluginPtr->QueryInterfaceSharedPtr< - ignition::sensors::SensorPlugin>(); return sensorPlugin; } diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index e6573691..1773b0a1 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -36,6 +36,8 @@ #include #include +#include + #include #include "ignition/sensors/ThermalCameraSensor.hh" @@ -593,4 +595,6 @@ bool ThermalCameraSensorPrivate::SaveImage(const uint16_t *_data, return true; } -IGN_SENSORS_REGISTER_SENSOR(ThermalCameraSensor) +IGNITION_ADD_PLUGIN( + ignition::sensors::SensorTypePlugin, + ignition::sensors::SensorPlugin) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 2c5de9fe..fe376c93 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -39,6 +39,7 @@ if (DRI_TESTS) ${PROJECT_LIBRARY_TARGET_NAME}-gpu_lidar ${PROJECT_LIBRARY_TARGET_NAME}-rgbd_camera ${PROJECT_LIBRARY_TARGET_NAME}-thermal_camera + ignition-plugin${IGN_PLUGIN_VER}::core ) endif() @@ -51,6 +52,7 @@ ign_build_tests(TYPE INTEGRATION ${PROJECT_LIBRARY_TARGET_NAME}-logical_camera ${PROJECT_LIBRARY_TARGET_NAME}-magnetometer ${PROJECT_LIBRARY_TARGET_NAME}-imu + ignition-plugin${IGN_PLUGIN_VER}::core ) foreach(plugin_test ${dri_tests} ${tests}) From daeb18dbb1290f0c2e04beaac838dfffefcb0048 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 5 Feb 2021 21:36:29 -0800 Subject: [PATCH 02/18] Hack to match a sensor type when multiple sensors are available Signed-off-by: Louise Poubel --- include/ignition/sensors/SensorFactory.hh | 3 +- src/RgbdCameraSensor.cc | 3 +- src/SensorFactory.cc | 100 ++++++++++++++++++---- src/ThermalCameraSensor.cc | 3 +- test/integration/camera_plugin.cc | 3 +- 5 files changed, 91 insertions(+), 21 deletions(-) diff --git a/include/ignition/sensors/SensorFactory.hh b/include/ignition/sensors/SensorFactory.hh index 188836fb..0b24c7e2 100644 --- a/include/ignition/sensors/SensorFactory.hh +++ b/include/ignition/sensors/SensorFactory.hh @@ -169,9 +169,10 @@ namespace ignition /// \brief load a plugin and return a pointer /// \param[in] _filename Sensor plugin file to load. + /// \param[in] _type Sensor type from SDF /// \return Pointer to the new sensor, nullptr on error. private: std::shared_ptr LoadSensorPlugin( - const std::string &_filename); + const std::string &_filename, const std::string &_type); IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief private data pointer diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 3b1856dc..94cde9f5 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -42,7 +42,8 @@ #pragma warning(pop) #endif -#include +// This plugin loads another plugin, so it shouldn't include Register.hh +#include #include #include diff --git a/src/SensorFactory.cc b/src/SensorFactory.cc index 99e2de41..2012466c 100644 --- a/src/SensorFactory.cc +++ b/src/SensorFactory.cc @@ -62,7 +62,7 @@ SensorFactory::~SensorFactory() ////////////////////////////////////////////////// std::shared_ptr SensorFactory::LoadSensorPlugin( - const std::string &_filename) + const std::string &_filename, const std::string &_type) { std::string fullPath = this->dataPtr->systemPaths.FindSharedLibrary(_filename); @@ -77,24 +77,90 @@ std::shared_ptr SensorFactory::LoadSensorPlugin( if (pluginNames.empty()) { - ignerr << "Unable to load sensor plugin file for [" << fullPath << "]\n"; + ignerr << "Failed to load plugin [" << _filename << + "] : couldn't load library on path [" << fullPath << + "]." << std::endl; return std::shared_ptr(); } - // Go over all plugin names and get the first one that implements the - // ignition::sensors::SensorPlugin interface - plugin::PluginPtr plugin; - std::shared_ptr sensorPlugin{nullptr}; - for (auto pluginName : pluginNames) + auto sensorNames = pluginLoader.PluginsImplementing< + ignition::sensors::SensorPlugin>(); + + if (sensorNames.empty()) + { + std::stringstream error; + error << "Found no sensor plugins in [" + << _filename << "], available interfaces are:" + << std::endl; + for (auto pluginName : pluginNames) + { + error << "- " << pluginName << std::endl; + } + ignerr << error.str(); + return nullptr; + } + + std::string sensorName; + if (sensorNames.size() == 1) + { + sensorName = *sensorNames.begin(); + } + else + { + // If multiple sensors are found, try to match the type + // This is super hacky, ideally we'd know the exact interface to look for + for (auto pluginName : sensorNames) + { + std::string massagedPlugin; + massagedPlugin.resize(pluginName.size()); + std::transform(pluginName.begin(), + pluginName.end(), + massagedPlugin.begin(), + ::tolower); + + std::string massagedType = _type; + massagedType.erase(std::remove(massagedType.begin(), massagedType.end(), '_'), + massagedType.end()); + + if (massagedPlugin.find(massagedType) != std::string::npos) + { + sensorName = pluginName; + break; + } + } + + // As a last resort, load a random one + if (sensorName.empty()) + { + sensorName = *sensorNames.begin(); + std::stringstream warn; + warn << "Found multiple sensor plugins in [" << _filename << "]:" + << std::endl; + for (auto pluginName : sensorNames) + { + warn << "- " << pluginName << std::endl; + } + warn << "Loading [" << sensorName << "]." << std::endl; + ignwarn << warn.str(); + } + } + + auto plugin = pluginLoader.Instantiate(sensorName); + if (!plugin) { - plugin = pluginLoader.Instantiate(pluginName); - if (!plugin) - continue; - - sensorPlugin = - plugin->QueryInterfaceSharedPtr(); - if (sensorPlugin) - break; + ignerr << "Failed to instantiate plugin [" << sensorName << "]" + << std::endl; + return nullptr; + } + + auto sensorPlugin = + plugin->QueryInterfaceSharedPtr(); + + if (!sensorPlugin) + { + ignerr << "Failed to query interface from [" << sensorName << "]" + << std::endl; + return nullptr; } return sensorPlugin; @@ -113,7 +179,7 @@ std::unique_ptr SensorFactory::CreateSensor(const sdf::Sensor &_sdf) sensorPlugin = it->second; else { - sensorPlugin = this->LoadSensorPlugin(fullPath); + sensorPlugin = this->LoadSensorPlugin(fullPath, type); if (!sensorPlugin) { ignerr << "Unable to instantiate sensor plugin for [" << fullPath @@ -163,7 +229,7 @@ std::unique_ptr SensorFactory::CreateSensor(sdf::ElementPtr _sdf) sensorPlugin = it->second; else { - sensorPlugin = this->LoadSensorPlugin(fullPath); + sensorPlugin = this->LoadSensorPlugin(fullPath, type); if (!sensorPlugin) { ignerr << "Unable to instantiate sensor plugin for [" << fullPath diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index 1773b0a1..6318fbfb 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -36,7 +36,8 @@ #include #include -#include +// This plugin loads another plugin, so it shouldn't include Register.hh +#include #include diff --git a/test/integration/camera_plugin.cc b/test/integration/camera_plugin.cc index ad86ee99..ba701f1f 100644 --- a/test/integration/camera_plugin.cc +++ b/test/integration/camera_plugin.cc @@ -57,6 +57,8 @@ class CameraSensorTest: public testing::Test, void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) { + ignition::common::Console::SetVerbosity(4); + // get the darn test data std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", "integration", "camera_sensor_builtin.sdf"); @@ -120,7 +122,6 @@ INSTANTIATE_TEST_CASE_P(CameraSensor, CameraSensorTest, ////////////////////////////////////////////////// int main(int argc, char **argv) { - ignition::common::Console::SetVerbosity(4); ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } From 1c16a1201e6d535f0ff62d7bea4da52c1240ba19 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 8 Feb 2021 22:01:00 -0800 Subject: [PATCH 03/18] Set custom flags, get exact plugin class Signed-off-by: Louise Poubel --- Migration.md | 1 + include/ignition/sensors/SensorFactory.hh | 3 +- src/SensorFactory.cc | 121 +++++++++------------- 3 files changed, 52 insertions(+), 73 deletions(-) diff --git a/Migration.md b/Migration.md index e9230a53..db94df77 100644 --- a/Migration.md +++ b/Migration.md @@ -13,6 +13,7 @@ release will remove the deprecated code. + ***Replacement*** IGNITION_ADD_PLUGIN(ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) * Use `#include ` * Privately link against `ignition-plugin${IGN_PLUGIN_VER}::register` + * Sensor class name should be `CamelCaseSensor` ## Ignition Sensors 3.X to 4.X diff --git a/include/ignition/sensors/SensorFactory.hh b/include/ignition/sensors/SensorFactory.hh index 0b24c7e2..71eff487 100644 --- a/include/ignition/sensors/SensorFactory.hh +++ b/include/ignition/sensors/SensorFactory.hh @@ -168,11 +168,10 @@ namespace ignition public: void AddPluginPaths(const std::string &_path); /// \brief load a plugin and return a pointer - /// \param[in] _filename Sensor plugin file to load. /// \param[in] _type Sensor type from SDF /// \return Pointer to the new sensor, nullptr on error. private: std::shared_ptr LoadSensorPlugin( - const std::string &_filename, const std::string &_type); + const std::string &_type); IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief private data pointer diff --git a/src/SensorFactory.cc b/src/SensorFactory.cc index 2012466c..e4b49fcb 100644 --- a/src/SensorFactory.cc +++ b/src/SensorFactory.cc @@ -15,6 +15,8 @@ * */ +#include + #include #include #include @@ -62,94 +64,73 @@ SensorFactory::~SensorFactory() ////////////////////////////////////////////////// std::shared_ptr SensorFactory::LoadSensorPlugin( - const std::string &_filename, const std::string &_type) + const std::string &_type) { + auto filename = IGN_SENSORS_PLUGIN_NAME(_type); + std::string fullPath = - this->dataPtr->systemPaths.FindSharedLibrary(_filename); + this->dataPtr->systemPaths.FindSharedLibrary(filename); if (fullPath.empty()) { - ignerr << "Unable to find sensor plugin path for [" << _filename << "]\n"; + ignerr << "Unable to find sensor plugin path for [" << filename << "]\n"; return std::shared_ptr(); } plugin::Loader pluginLoader; + pluginLoader.SetFlags(RTLD_LAZY | RTLD_GLOBAL); auto pluginNames = pluginLoader.LoadLib(fullPath); if (pluginNames.empty()) { - ignerr << "Failed to load plugin [" << _filename << + ignerr << "Failed to load plugin [" << filename << "] : couldn't load library on path [" << fullPath << "]." << std::endl; return std::shared_ptr(); } - auto sensorNames = pluginLoader.PluginsImplementing< - ignition::sensors::SensorPlugin>(); + std::string pluginName; + bool capitalize{true}; - if (sensorNames.empty()) + // snake_case to CamelCase + for (const auto &c : _type) { - std::stringstream error; - error << "Found no sensor plugins in [" - << _filename << "], available interfaces are:" - << std::endl; - for (auto pluginName : pluginNames) + if (c == '_') { - error << "- " << pluginName << std::endl; + capitalize = true; + continue; } - ignerr << error.str(); - return nullptr; - } - std::string sensorName; - if (sensorNames.size() == 1) - { - sensorName = *sensorNames.begin(); - } - else - { - // If multiple sensors are found, try to match the type - // This is super hacky, ideally we'd know the exact interface to look for - for (auto pluginName : sensorNames) + if (!std::isalpha(c)) + continue; + + if (!capitalize) { - std::string massagedPlugin; - massagedPlugin.resize(pluginName.size()); - std::transform(pluginName.begin(), - pluginName.end(), - massagedPlugin.begin(), - ::tolower); - - std::string massagedType = _type; - massagedType.erase(std::remove(massagedType.begin(), massagedType.end(), '_'), - massagedType.end()); - - if (massagedPlugin.find(massagedType) != std::string::npos) - { - sensorName = pluginName; - break; - } + pluginName += c; } - - // As a last resort, load a random one - if (sensorName.empty()) + else { - sensorName = *sensorNames.begin(); - std::stringstream warn; - warn << "Found multiple sensor plugins in [" << _filename << "]:" - << std::endl; - for (auto pluginName : sensorNames) - { - warn << "- " << pluginName << std::endl; - } - warn << "Loading [" << sensorName << "]." << std::endl; - ignwarn << warn.str(); + pluginName += std::toupper(c); + capitalize = false; } } - auto plugin = pluginLoader.Instantiate(sensorName); + auto ns = "ignition::sensors::v" + + std::to_string(IGNITION_SENSORS_MAJOR_VERSION) + "::"; + + pluginName = ns + "SensorTypePlugin<" + ns + pluginName + "Sensor>"; + + auto plugin = pluginLoader.Instantiate(pluginName); if (!plugin) { - ignerr << "Failed to instantiate plugin [" << sensorName << "]" - << std::endl; + std::stringstream error; + error << "Failed to instantiate plugin [" << pluginName << "] from [" + << fullPath << "]. Available plugins:" << std::endl; + for (auto name : pluginNames) + { + error << "- " << name << std::endl; + } + ignerr << error.str(); + return nullptr; } @@ -158,7 +139,7 @@ std::shared_ptr SensorFactory::LoadSensorPlugin( if (!sensorPlugin) { - ignerr << "Failed to query interface from [" << sensorName << "]" + ignerr << "Failed to query interface from [" << pluginName << "]" << std::endl; return nullptr; } @@ -172,17 +153,16 @@ std::unique_ptr SensorFactory::CreateSensor(const sdf::Sensor &_sdf) std::unique_ptr result; std::shared_ptr sensorPlugin; std::string type = _sdf.TypeStr(); - std::string fullPath = IGN_SENSORS_PLUGIN_NAME(type); auto it = this->dataPtr->sensorPlugins.find(type); if (it != this->dataPtr->sensorPlugins.end()) sensorPlugin = it->second; else { - sensorPlugin = this->LoadSensorPlugin(fullPath, type); + sensorPlugin = this->LoadSensorPlugin(type); if (!sensorPlugin) { - ignerr << "Unable to instantiate sensor plugin for [" << fullPath + ignerr << "Unable to instantiate sensor plugin for [" << type << "]\n"; return nullptr; } @@ -193,19 +173,19 @@ std::unique_ptr SensorFactory::CreateSensor(const sdf::Sensor &_sdf) auto sensor = sensorPlugin->New(); if (!sensor) { - ignerr << "Unable to instantiate sensor for [" << fullPath << "]\n"; + ignerr << "Unable to instantiate sensor for [" << type << "]\n"; return nullptr; } if (!sensor->Load(_sdf)) { - ignerr << "Sensor::Load failed for plugin [" << fullPath << "]\n"; + ignerr << "Sensor::Load failed for plugin [" << type << "]\n"; return nullptr; } if (!sensor->Init()) { - ignerr << "Sensor::Init failed for plugin [" << fullPath << "]\n"; + ignerr << "Sensor::Init failed for plugin [" << type << "]\n"; return nullptr; } @@ -223,16 +203,15 @@ std::unique_ptr SensorFactory::CreateSensor(sdf::ElementPtr _sdf) { std::shared_ptr sensorPlugin; std::string type = _sdf->Get("type"); - std::string fullPath = IGN_SENSORS_PLUGIN_NAME(type); auto it = this->dataPtr->sensorPlugins.find(type); if (it != this->dataPtr->sensorPlugins.end()) sensorPlugin = it->second; else { - sensorPlugin = this->LoadSensorPlugin(fullPath, type); + sensorPlugin = this->LoadSensorPlugin(type); if (!sensorPlugin) { - ignerr << "Unable to instantiate sensor plugin for [" << fullPath + ignerr << "Unable to instantiate sensor plugin for [" << type << "]\n"; return nullptr; } @@ -243,19 +222,19 @@ std::unique_ptr SensorFactory::CreateSensor(sdf::ElementPtr _sdf) auto sensor = sensorPlugin->New(); if (!sensor) { - ignerr << "Unable to instantiate sensor for [" << fullPath << "]\n"; + ignerr << "Unable to instantiate sensor for [" << type << "]\n"; return nullptr; } if (!sensor->Load(_sdf)) { - ignerr << "Sensor::Load failed for plugin [" << fullPath << "]\n"; + ignerr << "Sensor::Load failed for plugin [" << type << "]\n"; return nullptr; } if (!sensor->Init()) { - ignerr << "Sensor::Init failed for plugin [" << fullPath << "]\n"; + ignerr << "Sensor::Init failed for plugin [" << type << "]\n"; return nullptr; } result.reset(sensor); From c5d331a54f95dd2d7780816f57c10fb7ec255dfd Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 16 Feb 2021 16:25:47 -0800 Subject: [PATCH 04/18] simpler alias Signed-off-by: Louise Poubel --- Migration.md | 8 +++++--- src/AirPressureSensor.cc | 3 +++ src/AltimeterSensor.cc | 3 +++ src/CameraSensor.cc | 3 +++ src/DepthCameraSensor.cc | 3 +++ src/GpuLidarSensor.cc | 3 +++ src/ImuSensor.cc | 3 +++ src/Lidar.cc | 10 ++++++++++ src/LogicalCameraSensor.cc | 3 +++ src/MagnetometerSensor.cc | 3 +++ src/RgbdCameraSensor.cc | 3 +++ src/SensorFactory.cc | 37 +++---------------------------------- src/ThermalCameraSensor.cc | 3 +++ 13 files changed, 48 insertions(+), 37 deletions(-) diff --git a/Migration.md b/Migration.md index db94df77..4cb36172 100644 --- a/Migration.md +++ b/Migration.md @@ -8,12 +8,14 @@ release will remove the deprecated code. ## Ignition Sensors 4.X to 5.X * Plugins now use Ignition Plugin instead of Ignition Common's plugin framework. - * Macro `IGN_SENSORS_REGISTER_SENSOR` has been removed. Use `IGNITION_ADD_PLUGIN` instead. + * Macro `IGN_SENSORS_REGISTER_SENSOR` has been removed. Use + `IGNITION_ADD_PLUGIN` and `IGNITION_ADD_PLUGIN_ALIAS` instead. + ***Deprecation*** IGN_SENSORS_REGISTER_SENSOR(SensorName) - + ***Replacement*** IGNITION_ADD_PLUGIN(ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) + + ***Replacement*** + IGNITION_ADD_PLUGIN(ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) + IGNITION_ADD_PLUGIN_ALIAS(ignition::sensors::SensorTypePlugin, "sensor_type") * Use `#include ` * Privately link against `ignition-plugin${IGN_PLUGIN_VER}::register` - * Sensor class name should be `CamelCaseSensor` ## Ignition Sensors 3.X to 4.X diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc index f0f56523..7447d8f3 100644 --- a/src/AirPressureSensor.cc +++ b/src/AirPressureSensor.cc @@ -228,3 +228,6 @@ double AirPressureSensor::ReferenceAltitude() const IGNITION_ADD_PLUGIN( ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) +IGNITION_ADD_PLUGIN_ALIAS( + ignition::sensors::SensorTypePlugin, + "air_pressure") diff --git a/src/AltimeterSensor.cc b/src/AltimeterSensor.cc index 18040102..66afc71c 100644 --- a/src/AltimeterSensor.cc +++ b/src/AltimeterSensor.cc @@ -223,3 +223,6 @@ double AltimeterSensor::VerticalVelocity() const IGNITION_ADD_PLUGIN( ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) +IGNITION_ADD_PLUGIN_ALIAS( + ignition::sensors::SensorTypePlugin, + "altimeter") diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 3ecb9c3a..03431d5b 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -651,3 +651,6 @@ double CameraSensor::Baseline() const IGNITION_ADD_PLUGIN( ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) +IGNITION_ADD_PLUGIN_ALIAS( + ignition::sensors::SensorTypePlugin, + "camera") diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 921d9240..88fba9f8 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -637,3 +637,6 @@ double DepthCameraSensor::NearClip() const IGNITION_ADD_PLUGIN( ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) +IGNITION_ADD_PLUGIN_ALIAS( + ignition::sensors::SensorTypePlugin, + "depth_camera") diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc index 4ac7c69f..615b7a8c 100644 --- a/src/GpuLidarSensor.cc +++ b/src/GpuLidarSensor.cc @@ -376,3 +376,6 @@ void GpuLidarSensorPrivate::FillPointCloudMsg() IGNITION_ADD_PLUGIN( ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) +IGNITION_ADD_PLUGIN_ALIAS( + ignition::sensors::SensorTypePlugin, + "gpu_lidar") diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index c08bfde4..a726558f 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -306,3 +306,6 @@ math::Quaterniond ImuSensor::Orientation() const IGNITION_ADD_PLUGIN( ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) +IGNITION_ADD_PLUGIN_ALIAS( + ignition::sensors::SensorTypePlugin, + "imu") diff --git a/src/Lidar.cc b/src/Lidar.cc index 89996ae6..205d28ef 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -449,6 +449,16 @@ bool Lidar::IsActive() const return true; } +// Register as LidarSensor to conform to naming requirements IGNITION_ADD_PLUGIN( ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) +IGNITION_ADD_PLUGIN_ALIAS( + ignition::sensors::SensorTypePlugin, + "ignition::sensors::v" + std::to_string(IGNITION_SENSORS_MAJOR_VERSION) + + "::SensorTypePlugin<" + + "ignition::sensors::v" + std::to_string(IGNITION_SENSORS_MAJOR_VERSION) + + "::LidarSensor>") +IGNITION_ADD_PLUGIN_ALIAS( + ignition::sensors::SensorTypePlugin, + "lidar") diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc index c9b79791..84c158d0 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -211,3 +211,6 @@ msgs::LogicalCameraImage LogicalCameraSensor::Image() const IGNITION_ADD_PLUGIN( ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) +IGNITION_ADD_PLUGIN_ALIAS( + ignition::sensors::SensorTypePlugin, + "logical_camera") diff --git a/src/MagnetometerSensor.cc b/src/MagnetometerSensor.cc index f82089d5..e3413843 100644 --- a/src/MagnetometerSensor.cc +++ b/src/MagnetometerSensor.cc @@ -242,3 +242,6 @@ math::Vector3d MagnetometerSensor::MagneticField() const IGNITION_ADD_PLUGIN( ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) +IGNITION_ADD_PLUGIN_ALIAS( + ignition::sensors::SensorTypePlugin, + "magnetometer") diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 94cde9f5..656464d0 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -614,3 +614,6 @@ unsigned int RgbdCameraSensor::ImageHeight() const IGNITION_ADD_PLUGIN( ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) +IGNITION_ADD_PLUGIN_ALIAS( + ignition::sensors::SensorTypePlugin, + "rgbd_camera") diff --git a/src/SensorFactory.cc b/src/SensorFactory.cc index e4b49fcb..029d9205 100644 --- a/src/SensorFactory.cc +++ b/src/SensorFactory.cc @@ -88,42 +88,11 @@ std::shared_ptr SensorFactory::LoadSensorPlugin( return std::shared_ptr(); } - std::string pluginName; - bool capitalize{true}; - - // snake_case to CamelCase - for (const auto &c : _type) - { - if (c == '_') - { - capitalize = true; - continue; - } - - if (!std::isalpha(c)) - continue; - - if (!capitalize) - { - pluginName += c; - } - else - { - pluginName += std::toupper(c); - capitalize = false; - } - } - - auto ns = "ignition::sensors::v" - + std::to_string(IGNITION_SENSORS_MAJOR_VERSION) + "::"; - - pluginName = ns + "SensorTypePlugin<" + ns + pluginName + "Sensor>"; - - auto plugin = pluginLoader.Instantiate(pluginName); + auto plugin = pluginLoader.Instantiate(_type); if (!plugin) { std::stringstream error; - error << "Failed to instantiate plugin [" << pluginName << "] from [" + error << "Failed to instantiate plugin [" << _type << "] from [" << fullPath << "]. Available plugins:" << std::endl; for (auto name : pluginNames) { @@ -139,7 +108,7 @@ std::shared_ptr SensorFactory::LoadSensorPlugin( if (!sensorPlugin) { - ignerr << "Failed to query interface from [" << pluginName << "]" + ignerr << "Failed to query interface from [" << fullPath << "]" << std::endl; return nullptr; } diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index 6318fbfb..8f230e81 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -599,3 +599,6 @@ bool ThermalCameraSensorPrivate::SaveImage(const uint16_t *_data, IGNITION_ADD_PLUGIN( ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) +IGNITION_ADD_PLUGIN_ALIAS( + ignition::sensors::SensorTypePlugin, + "thermal_camera") From 5ea038086472842fa0e5b40f9756409a475e6b6a Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 16 Feb 2021 18:32:32 -0800 Subject: [PATCH 05/18] Support custom sensors Signed-off-by: Louise Poubel --- examples/custom_sensor/CMakeLists.txt | 15 ++ examples/custom_sensor/CustomSensor.cc | 93 ++++++++++++ examples/custom_sensor/CustomSensor.hh | 54 +++++++ examples/custom_sensor/README.md | 23 +++ examples/custom_sensor/custom_sensor.sdf | 16 ++ examples/loop_sensor/CMakeLists.txt | 8 + examples/loop_sensor/README.md | 163 +++++++++++++++++++++ examples/loop_sensor/altimeter_example.sdf | 27 ++++ examples/loop_sensor/main.cc | 96 ++++++++++++ include/ignition/sensors/Sensor.hh | 6 +- src/SensorFactory.cc | 4 + 11 files changed, 504 insertions(+), 1 deletion(-) create mode 100644 examples/custom_sensor/CMakeLists.txt create mode 100644 examples/custom_sensor/CustomSensor.cc create mode 100644 examples/custom_sensor/CustomSensor.hh create mode 100644 examples/custom_sensor/README.md create mode 100644 examples/custom_sensor/custom_sensor.sdf create mode 100644 examples/loop_sensor/CMakeLists.txt create mode 100644 examples/loop_sensor/README.md create mode 100644 examples/loop_sensor/altimeter_example.sdf create mode 100644 examples/loop_sensor/main.cc diff --git a/examples/custom_sensor/CMakeLists.txt b/examples/custom_sensor/CMakeLists.txt new file mode 100644 index 00000000..41e548e5 --- /dev/null +++ b/examples/custom_sensor/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +find_package(ignition-cmake2 REQUIRED) + +# Must follow the pattern "ignition-sensors-" +project(ignition-sensors5-custom_sensor) + +find_package(ignition-plugin1 REQUIRED COMPONENTS register) +find_package(ignition-sensors5 REQUIRED) + +add_library(${PROJECT_NAME} SHARED CustomSensor.cc) +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17) +target_link_libraries(${PROJECT_NAME} + PRIVATE ignition-plugin1::ignition-plugin1 + PRIVATE ignition-sensors5::ignition-sensors5) diff --git a/examples/custom_sensor/CustomSensor.cc b/examples/custom_sensor/CustomSensor.cc new file mode 100644 index 00000000..ed6e137a --- /dev/null +++ b/examples/custom_sensor/CustomSensor.cc @@ -0,0 +1,93 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include +#include +#include + +#include "CustomSensor.hh" + +using namespace custom; + +////////////////////////////////////////////////// +bool CustomSensor::Load(sdf::ElementPtr _sdf) +{ + if (!_sdf) + { + ignerr << "Null SDF pointer." << std::endl; + return false; + } + + if (_sdf->GetName() != "sensor") + { + ignerr << "SDF element is not a sensor." << std::endl; + return false; + } + + std::string type = _sdf->Get("type"); + if (type != "custom_sensor") + { + ignerr << "Trying to load [custom_sensor], got [" << type << "] instead." + << std::endl; + return false; + } + + // Load common sensor params + ignition::sensors::Sensor::Load(_sdf); + + // Load noise + if (_sdf->HasElement("noise")) + { + sdf::Noise noiseSdf; + noiseSdf.Load(_sdf->GetElement("noise")); + this->noise = ignition::sensors::NoiseFactory::NewNoiseModel(noiseSdf); + } + + // Advertise topic where data will be published + this->pub = this->node.Advertise(this->Topic()); + + return true; +} + +////////////////////////////////////////////////// +bool CustomSensor::Update(const std::chrono::steady_clock::duration &_now) +{ + ignition::msgs::Double msg; + *msg.mutable_header()->mutable_stamp() = ignition::msgs::Convert(_now); + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->Name()); + + this->data = this->noise->Apply(this->data); + + msg.set_data(this->data); + + this->AddSequence(msg.mutable_header()); + this->pub.Publish(msg); + + return true; +} + +IGNITION_ADD_PLUGIN( + ignition::sensors::SensorTypePlugin, + ignition::sensors::SensorPlugin) +IGNITION_ADD_PLUGIN_ALIAS( + ignition::sensors::SensorTypePlugin, + "custom_sensor") diff --git a/examples/custom_sensor/CustomSensor.hh b/examples/custom_sensor/CustomSensor.hh new file mode 100644 index 00000000..10b5d502 --- /dev/null +++ b/examples/custom_sensor/CustomSensor.hh @@ -0,0 +1,54 @@ +/* + * 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 CUSTOMSENSORSENSOR_HH_ +#define CUSTOMSENSORSENSOR_HH_ + +#include +#include +#include + +namespace custom +{ + /// \brief CustomSensor Sensor Class + class CustomSensor : public ignition::sensors::Sensor + { + /// \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 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 Noise that will be applied to the sensor data + private: ignition::sensors::NoisePtr noise; + + /// \brief Node for communication + private: ignition::transport::Node node; + + /// \brief Publishes sensor data + private: ignition::transport::Node::Publisher pub; + + /// \brief Latest data + private: double data{0.0}; + }; +} + +#endif diff --git a/examples/custom_sensor/README.md b/examples/custom_sensor/README.md new file mode 100644 index 00000000..0c8068d0 --- /dev/null +++ b/examples/custom_sensor/README.md @@ -0,0 +1,23 @@ +# Custom sensor + +This example creates a simple custom sensor that produces a single number as +data and publishes it to a topic. + +## Build + +Compile the sensor as follows: + +``` +cd examples/custom_sensor +mkdir build +cd build +cmake .. +make +``` + +This will generate a shared library with the sensor called `ignition-sensors5-custom_sensor`. + +## Test + +The "loop_sensor" example can be used to load an SDF file with configuration for +this sensor and run it in a loop. See that example's instructions. diff --git a/examples/custom_sensor/custom_sensor.sdf b/examples/custom_sensor/custom_sensor.sdf new file mode 100644 index 00000000..243e58d8 --- /dev/null +++ b/examples/custom_sensor/custom_sensor.sdf @@ -0,0 +1,16 @@ + + + + + + 10 + /custom_data + + gaussian + 1.0 + 0.05 + + + + + diff --git a/examples/loop_sensor/CMakeLists.txt b/examples/loop_sensor/CMakeLists.txt new file mode 100644 index 00000000..0595bd34 --- /dev/null +++ b/examples/loop_sensor/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 3.5 FATAL_ERROR) +project(loop_sensor) + +find_package(ignition-sensors5 REQUIRED) + +add_executable(${PROJECT_NAME} main.cc) +target_link_libraries(${PROJECT_NAME} PUBLIC + ignition-sensors5::ignition-sensors5) diff --git a/examples/loop_sensor/README.md b/examples/loop_sensor/README.md new file mode 100644 index 00000000..e7675c53 --- /dev/null +++ b/examples/loop_sensor/README.md @@ -0,0 +1,163 @@ +# Loop sensor + +Example executable that loads a sensor from an SDF file and updates it in a +loop until the user closes the program. + +This example can't load rendering sensors. + +## Build + +Compile as follows: + +``` +cd examples/loop_sensor +mkdir build +cd build +cmake .. +make +``` + +This will generate an executable called `loop_sensor`. + +## Load an official sensor + +Try for example loading the accompanying `altimeter_sensor.sdf` file to run +an altimeter sensor, which is installed with Ignition Sensors: + +``` +cd examples/loop_sensor/build +./loop_sensor ../altimeter_example.sdf +``` + +On another terminal, check that the altimeter is generating data on a topic: + +``` +ign topic -l +``` + +You should see: + +`` +/altimeter +``` + +Then listen to the data: + +``` +ign topic -e -t /altimeter +``` + +You'll see data like: + +``` +... +header { + stamp { + sec: 12 + } + data { + key: "frame_id" + value: "altimeter" + } + data { + key: "seq" + value: "12" + } +} +vertical_position: 0.9903850972191578 +vertical_velocity: 2.9159486154028573 + +header { + stamp { + sec: 13 + } + data { + key: "frame_id" + value: "altimeter" + } + data { + key: "seq" + value: "13" + } +} +vertical_position: 1.139457534900868 +vertical_velocity: 3.1484160275030266 +... +``` + +## Load a custom sensor + +Let's try loading the sensor from the `custom_sensor` example. + +First we need to tell Ignition Sensors where to find the custom sensor +by setting the `IGN_SENSORS_PATH` environment variable to the path where +`ignition-sensors5-custom_sensor` is located: + +``` +cd examples/loop_sensor/build +export IGN_SENSORS_PATH=../..//custom_sensor/build +``` + +Then we can run the `custom_sensor.sdf` file provided with that sensor: + +``` +cd examples/loop_sensor/build +./loop_sensor ../../custom_sensor/custom_sensor.sdf +``` + +On another terminal, check that the sensor is generating data on a topic: + +``` +ign topic -l +``` + +You should see: + +`` +/custom_data +``` + +Then listen to the data: + +``` +ign topic -e -t /custom_data +``` + +You'll see data like: + +``` +... +header { + stamp { + sec: 12 + } + data { + key: "frame_id" + value: "my_sensor" + } + data { + key: "seq" + value: "12" + } +} +data: 12.799322041404857 + +header { + stamp { + sec: 13 + } + data { + key: "frame_id" + value: "my_sensor" + } + data { + key: "seq" + value: "13" + } +} +data: 13.783310442250663 +... +``` + + + diff --git a/examples/loop_sensor/altimeter_example.sdf b/examples/loop_sensor/altimeter_example.sdf new file mode 100644 index 00000000..61d6f935 --- /dev/null +++ b/examples/loop_sensor/altimeter_example.sdf @@ -0,0 +1,27 @@ + + + + + + 1 + 30 + true + altimeter + + + + 0.1 + 0.2 + + + + + 0.2 + 0.1 + + + + + + + diff --git a/examples/loop_sensor/main.cc b/examples/loop_sensor/main.cc new file mode 100644 index 00000000..520950ac --- /dev/null +++ b/examples/loop_sensor/main.cc @@ -0,0 +1,96 @@ +/* + * 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. + * +*/ + +#include +#include + +#include + +#include +#include + +#include + +using namespace std::literals::chrono_literals; + +int main(int argc, char **argv) +{ + ignition::common::Console::SetVerbosity(4); + + if (argc < 2) + { + ignerr << "Missing path to SDF file" << std::endl; + return 1; + } + + // Load file + std::string sdfFile(argv[1]); + std::ifstream fileStream(sdfFile, std::ifstream::in); + + std::string sdfString((std::istreambuf_iterator(fileStream)), + std::istreambuf_iterator()); + fileStream.close(); + + // Read SDF + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + if (!sdf::readString(sdfString, sdfParsed)) + { + ignerr << "Failed to parse SDF: " << std::endl << sdfString << std::endl; + return 1; + } + if (!sdfParsed || + !sdfParsed->Root() || + !sdfParsed->Root()->GetElement("model") || + !sdfParsed->Root()->GetElement("model")->GetElement("link") || + !sdfParsed->Root()->GetElement("model")->GetElement("link")->GetElement("sensor")) + { + ignerr << "Failed to find sensor in SDF" << std::endl; + return 1; + } + + auto sdfElem = sdfParsed->Root()->GetElement("model")->GetElement("link") + ->GetElement("sensor"); + + // Create sensor + ignition::sensors::Manager mgr; + auto sensor = mgr.CreateSensor(sdfElem); + + if (!sensor) + { + ignerr << "Unable to load sensor" << std::endl;; + return 1; + } + + // Stop when user presses Ctrl+C + bool signaled{false}; + ignition::common::SignalHandler sigHandler; + sigHandler.AddCallback([&] (int) + { + signaled = true; + }); + + auto time = 0s; + while (!signaled) + { + mgr.RunOnce(time, true); + time += 1s; + std::this_thread::sleep_for(1s); + } + + return 0; +} diff --git a/include/ignition/sensors/Sensor.hh b/include/ignition/sensors/Sensor.hh index 450e7c53..f38a06d5 100644 --- a/include/ignition/sensors/Sensor.hh +++ b/include/ignition/sensors/Sensor.hh @@ -90,8 +90,12 @@ namespace ignition /// \param[in] _now The current time /// \return true if the update was successfull /// \sa SetUpdateRate() + /// \deprecated Use `Update` function that accepts chrono. public: - virtual bool IGN_DEPRECATED(4) Update(const common::Time &_now) = 0; + virtual bool IGN_DEPRECATED(4) Update(const common::Time &/*_now*/) + { + return false; + }; /// \brief Force the sensor to generate data /// diff --git a/src/SensorFactory.cc b/src/SensorFactory.cc index 029d9205..0bc5bb5f 100644 --- a/src/SensorFactory.cc +++ b/src/SensorFactory.cc @@ -35,6 +35,9 @@ class ignition::sensors::SensorFactoryPrivate /// \brief Stores paths to search for on file system public: ignition::common::SystemPaths systemPaths; + + /// \brief Environment variable containing lookup paths for sensors. + public: const std::string pathEnv{"IGN_SENSORS_PATH"}; }; using namespace ignition; @@ -55,6 +58,7 @@ void SensorFactory::AddPluginPaths(const std::string &_path) ////////////////////////////////////////////////// SensorFactory::SensorFactory() : dataPtr(new SensorFactoryPrivate) { + this->dataPtr->systemPaths.SetPluginPathEnv(this->dataPtr->pathEnv); } ////////////////////////////////////////////////// From ecf87cffa2fbe4eef2744aa1f1a4320219658ba2 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 17 Feb 2021 12:19:31 -0800 Subject: [PATCH 06/18] PR feedback Signed-off-by: Louise Poubel --- src/Lidar.cc | 6 ------ test/integration/camera_plugin.cc | 3 +-- 2 files changed, 1 insertion(+), 8 deletions(-) diff --git a/src/Lidar.cc b/src/Lidar.cc index 25e63a1d..9907f03f 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -465,12 +465,6 @@ bool Lidar::IsActive() const IGNITION_ADD_PLUGIN( ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) -IGNITION_ADD_PLUGIN_ALIAS( - ignition::sensors::SensorTypePlugin, - "ignition::sensors::v" + std::to_string(IGNITION_SENSORS_MAJOR_VERSION) + - "::SensorTypePlugin<" + - "ignition::sensors::v" + std::to_string(IGNITION_SENSORS_MAJOR_VERSION) + - "::LidarSensor>") IGNITION_ADD_PLUGIN_ALIAS( ignition::sensors::SensorTypePlugin, "lidar") diff --git a/test/integration/camera_plugin.cc b/test/integration/camera_plugin.cc index ba701f1f..ad86ee99 100644 --- a/test/integration/camera_plugin.cc +++ b/test/integration/camera_plugin.cc @@ -57,8 +57,6 @@ class CameraSensorTest: public testing::Test, void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) { - ignition::common::Console::SetVerbosity(4); - // get the darn test data std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", "integration", "camera_sensor_builtin.sdf"); @@ -122,6 +120,7 @@ INSTANTIATE_TEST_CASE_P(CameraSensor, CameraSensorTest, ////////////////////////////////////////////////// int main(int argc, char **argv) { + ignition::common::Console::SetVerbosity(4); ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } From aef8a6ad96de41469b5034a5e3db34acec0a04ef Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 2 Aug 2021 14:41:32 -0700 Subject: [PATCH 07/18] remove custom flag Signed-off-by: Louise Poubel --- src/SensorFactory.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/SensorFactory.cc b/src/SensorFactory.cc index 029d9205..8185d4cc 100644 --- a/src/SensorFactory.cc +++ b/src/SensorFactory.cc @@ -77,7 +77,6 @@ std::shared_ptr SensorFactory::LoadSensorPlugin( } plugin::Loader pluginLoader; - pluginLoader.SetFlags(RTLD_LAZY | RTLD_GLOBAL); auto pluginNames = pluginLoader.LoadLib(fullPath); if (pluginNames.empty()) From ae566dc93acb4e9a6ab5766817d871fa180a020a Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 2 Aug 2021 15:19:28 -0700 Subject: [PATCH 08/18] AddSensor Signed-off-by: Louise Poubel --- include/ignition/sensors/Manager.hh | 8 ++++++++ src/Manager.cc | 13 ++++++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/include/ignition/sensors/Manager.hh b/include/ignition/sensors/Manager.hh index 9c989b1f..6d70b9fb 100644 --- a/include/ignition/sensors/Manager.hh +++ b/include/ignition/sensors/Manager.hh @@ -158,6 +158,14 @@ namespace ignition /// is returned on erro. public: ignition::sensors::SensorId CreateSensor(sdf::ElementPtr _sdf); + /// \brief Add a sensor from a sensor instance. + /// \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: ignition::sensors::SensorId AddSensor( + const std::shared_ptr &_sensor); + /// \brief Create a sensor from SDF without a known sensor type. /// /// This creates sensors by looking at the given sdf element. diff --git a/src/Manager.cc b/src/Manager.cc index 61b397c2..dc3ec761 100644 --- a/src/Manager.cc +++ b/src/Manager.cc @@ -36,7 +36,7 @@ class ignition::sensors::ManagerPrivate public: ~ManagerPrivate(); /// \brief Loaded sensors. - public: std::map> sensors; + public: std::map> sensors; /// \brief Sensor factory for creating sensors from plugins; public: SensorFactory sensorFactory; @@ -100,6 +100,17 @@ void Manager::RunOnce( } } +///////////////////////////////////////////////// +ignition::sensors::SensorId Manager::AddSensor( + const std::shared_ptr &_sensor) +{ + if (!_sensor) + return NO_SENSOR; + SensorId id = _sensor->Id(); + this->dataPtr->sensors[id] = std::move(_sensor); + return id; +} + ///////////////////////////////////////////////// ignition::sensors::SensorId Manager::CreateSensor(const sdf::Sensor &_sdf) { From 6f5718edc1453bbd78a1beed39a4a6938ebaf3e4 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 2 Aug 2021 15:26:30 -0700 Subject: [PATCH 09/18] tweaks Signed-off-by: Louise Poubel --- Migration.md | 4 ++-- include/ignition/sensors/Manager.hh | 4 +--- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/Migration.md b/Migration.md index 4cb36172..9d8d1386 100644 --- a/Migration.md +++ b/Migration.md @@ -5,12 +5,12 @@ Deprecated code produces compile-time warnings. These warning serve as notification to users that their code should be upgraded. The next major release will remove the deprecated code. -## Ignition Sensors 4.X to 5.X +## Ignition Sensors 5.X to 6.X * Plugins now use Ignition Plugin instead of Ignition Common's plugin framework. * Macro `IGN_SENSORS_REGISTER_SENSOR` has been removed. Use `IGNITION_ADD_PLUGIN` and `IGNITION_ADD_PLUGIN_ALIAS` instead. - + ***Deprecation*** IGN_SENSORS_REGISTER_SENSOR(SensorName) + + ***Removed*** IGN_SENSORS_REGISTER_SENSOR(SensorName) + ***Replacement*** IGNITION_ADD_PLUGIN(ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) IGNITION_ADD_PLUGIN_ALIAS(ignition::sensors::SensorTypePlugin, "sensor_type") diff --git a/include/ignition/sensors/Manager.hh b/include/ignition/sensors/Manager.hh index 6d70b9fb..36510168 100644 --- a/include/ignition/sensors/Manager.hh +++ b/include/ignition/sensors/Manager.hh @@ -163,8 +163,7 @@ namespace ignition /// \param[in] _sensor Pointer to the sensor /// \return A sensor id that refers to the created sensor. NO_SENSOR /// is returned on error. - public: ignition::sensors::SensorId AddSensor( - const std::shared_ptr &_sensor); + public: SensorId AddSensor(const std::shared_ptr &_sensor); /// \brief Create a sensor from SDF without a known sensor type. /// @@ -185,7 +184,6 @@ namespace ignition /// 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. From 9e7d0690675c805ce4a932f2dc49c91ef3e4a2c4 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 2 Aug 2021 18:44:00 -0700 Subject: [PATCH 10/18] tweaks Signed-off-by: Louise Poubel --- examples/custom_sensor/CustomSensor.hh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/custom_sensor/CustomSensor.hh b/examples/custom_sensor/CustomSensor.hh index 10b5d502..d96f86c8 100644 --- a/examples/custom_sensor/CustomSensor.hh +++ b/examples/custom_sensor/CustomSensor.hh @@ -23,12 +23,12 @@ namespace custom { - /// \brief CustomSensor Sensor Class + /// \brief Example sensor that publishes a noisy double value. class CustomSensor : public ignition::sensors::Sensor { /// \brief Load the sensor with SDF parameters. /// \param[in] _sdf SDF Sensor parameters. - /// \return true if loading was successful + /// \return True if loading was successful public: virtual bool Load(sdf::ElementPtr _sdf) override; /// \brief Update the sensor and generate data @@ -38,7 +38,7 @@ namespace custom const std::chrono::steady_clock::duration &_now) override; /// \brief Noise that will be applied to the sensor data - private: ignition::sensors::NoisePtr noise; + private: ignition::sensors::NoisePtr noise{nullptr}; /// \brief Node for communication private: ignition::transport::Node node; From 10396fb60985cf1ac6cf836efe83b6ff5e8db0ec Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 4 Aug 2021 19:24:13 -0700 Subject: [PATCH 11/18] Turn 180 degrees: just don't use plugins at all Signed-off-by: Louise Poubel --- .github/ci/packages.apt | 1 - CMakeLists.txt | 5 - Migration.md | 36 +++- bitbucket-pipelines.yml | 1 - examples/custom_sensor/CMakeLists.txt | 5 +- include/ignition/sensors/Manager.hh | 149 ++++----------- include/ignition/sensors/RgbdCameraSensor.hh | 5 + include/ignition/sensors/SensorFactory.hh | 138 +++++++------- src/AirPressureSensor.cc | 7 - src/AltimeterSensor.cc | 7 - src/CMakeLists.txt | 54 ++---- src/CameraSensor.cc | 7 - src/Camera_TEST.cc | 23 +-- src/DepthCameraSensor.cc | 8 - src/GpuLidarSensor.cc | 9 +- src/ImuSensor.cc | 7 - src/Lidar.cc | 8 - src/LogicalCameraSensor.cc | 7 - src/MagnetometerSensor.cc | 7 - src/Manager.cc | 54 ++---- src/Manager_TEST.cc | 20 +- src/RgbdCameraSensor.cc | 16 +- src/SensorFactory.cc | 183 ++----------------- src/ThermalCameraSensor.cc | 9 - test/integration/CMakeLists.txt | 26 ++- test/integration/air_pressure_plugin.cc | 33 +--- test/integration/altimeter_plugin.cc | 33 +--- test/integration/camera_plugin.cc | 8 +- test/integration/gpu_lidar_sensor_plugin.cc | 23 +-- test/integration/imu_plugin.cc | 20 +- test/integration/logical_camera_plugin.cc | 28 +-- test/integration/magnetometer_plugin.cc | 33 ++-- tutorials/02_install.md | 2 +- 33 files changed, 288 insertions(+), 684 deletions(-) diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index e91044eb..759282e8 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -2,7 +2,6 @@ libignition-cmake2-dev libignition-common4-dev libignition-math6-dev libignition-msgs8-dev -libignition-plugin-dev libignition-rendering6-dev libignition-tools-dev libignition-transport11-dev diff --git a/CMakeLists.txt b/CMakeLists.txt index 946c5236..d842db55 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,11 +78,6 @@ endif() ign_find_package(ignition-msgs8 REQUIRED) set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) -#-------------------------------------- -# Find ignition-plugin -ign_find_package(ignition-plugin1 REQUIRED COMPONENTS all) -set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) - #-------------------------------------- # Find SDFormat ign_find_package(sdformat12 REQUIRED) diff --git a/Migration.md b/Migration.md index 9d8d1386..ab85ebdf 100644 --- a/Migration.md +++ b/Migration.md @@ -7,15 +7,33 @@ release will remove the deprecated code. ## Ignition Sensors 5.X to 6.X -* Plugins now use Ignition Plugin instead of Ignition Common's plugin framework. - * Macro `IGN_SENSORS_REGISTER_SENSOR` has been removed. Use - `IGNITION_ADD_PLUGIN` and `IGNITION_ADD_PLUGIN_ALIAS` instead. - + ***Removed*** IGN_SENSORS_REGISTER_SENSOR(SensorName) - + ***Replacement*** - IGNITION_ADD_PLUGIN(ignition::sensors::SensorTypePlugin, ignition::sensors::SensorPlugin) - IGNITION_ADD_PLUGIN_ALIAS(ignition::sensors::SensorTypePlugin, "sensor_type") - * Use `#include ` - * Privately link against `ignition-plugin${IGN_PLUGIN_VER}::register` +1. Sensors aren't loaded as plugins anymore. Instead, downstream libraries must + link to the library of the sensor they're interested in, and instantiate + new sensors knowing their type. For example: + + * `auto camera = std::make_unique();` + * `auto camera = sensorFactory.CreateSensor(_sdf);` + * `auto camera = manager.CreateSensor(_sdf);` + +1. **include/sensors/SensorFactory.hh** + + ***Deprecation*** SensorPlugin + + ***Replacement*** None; see above. + + ***Deprecation*** SensorTypePlugin + + ***Replacement*** None; see above. + + ***Deprecation*** std::unique_ptr CreateSensor(sdf::ElementPtr); + + ***Replacement*** template std::unique_ptr CreateSensor(sdf::ElementPtr); + + ***Deprecation*** std::unique_ptr CreateSensor(const sdf::Sensor &); + + ***Replacement*** template std::unique_ptr CreateSensor(const sdf::Sensor &); + + ***Deprecation*** void AddPluginPaths(const std::string &) + + ***Replacement*** None; see above. + +1. **include/sensors/Manager.hh** + + ***Deprecation*** SensorId CreateSensor(sdf::ElementPtr); + + ***Replacement*** template SensorType> \*CreateSensor(SdfType); + + ***Deprecation*** SensorId CreateSensor(const sdf::Sensor &); + + ***Replacement*** template SensorType \*CreateSensor(SdfType); + + ***Deprecation*** void AddPluginPaths(const std::string &) + + ***Replacement*** None; see above. ## Ignition Sensors 3.X to 4.X diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml index 10731124..02c81840 100644 --- a/bitbucket-pipelines.yml +++ b/bitbucket-pipelines.yml @@ -24,7 +24,6 @@ pipelines: libignition-math6-dev libignition-msgs5-dev libignition-tools-dev - libignition-plugin-dev libignition-transport8-dev libsdformat9-dev # libignition-rendering3-dev diff --git a/examples/custom_sensor/CMakeLists.txt b/examples/custom_sensor/CMakeLists.txt index 08a1b233..6b0d08ed 100644 --- a/examples/custom_sensor/CMakeLists.txt +++ b/examples/custom_sensor/CMakeLists.txt @@ -2,14 +2,11 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) find_package(ignition-cmake2 REQUIRED) -# Must follow the pattern "ignition-sensors-" -project(ignition-sensors6-custom_sensor) +project(custom_sensor) -find_package(ignition-plugin1 REQUIRED COMPONENTS register) find_package(ignition-sensors6 REQUIRED) add_library(${PROJECT_NAME} SHARED CustomSensor.cc) set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17) target_link_libraries(${PROJECT_NAME} - PRIVATE ignition-plugin1::ignition-plugin1 PRIVATE ignition-sensors6::ignition-sensors6) diff --git a/include/ignition/sensors/Manager.hh b/include/ignition/sensors/Manager.hh index 36510168..195f56cb 100644 --- a/include/ignition/sensors/Manager.hh +++ b/include/ignition/sensors/Manager.hh @@ -27,6 +27,7 @@ #include #include #include +#include namespace ignition { @@ -60,129 +61,61 @@ 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) { - ignition::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) - { - ignition::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. - /// 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 Add a sensor from a sensor instance. - /// \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(const std::shared_ptr &_sensor); + /// \deprecated Sensor registration is deprecated, so it's necessary to + /// provide the specific sensor type to create it. Use the templated + /// `CreateSensor` function. + public: ignition::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. - /// 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); + /// \deprecated Sensor registration is deprecated, so it's necessary to + /// provide the specific sensor type to create it. Use the templated + /// `CreateSensor` function. + public: ignition::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. @@ -203,13 +136,7 @@ namespace ignition 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); + 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/ignition/sensors/RgbdCameraSensor.hh b/include/ignition/sensors/RgbdCameraSensor.hh index 91069043..7fbf0985 100644 --- a/include/ignition/sensors/RgbdCameraSensor.hh +++ b/include/ignition/sensors/RgbdCameraSensor.hh @@ -62,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; diff --git a/include/ignition/sensors/SensorFactory.hh b/include/ignition/sensors/SensorFactory.hh index 71eff487..225f9394 100644 --- a/include/ignition/sensors/SensorFactory.hh +++ b/include/ignition/sensors/SensorFactory.hh @@ -40,29 +40,32 @@ namespace ignition class SensorFactoryPrivate; /// \brief Base sensor plugin interface + /// \deprecated Sensor plugins are deprecated. Instantiate sensor objects + /// instead. class IGNITION_SENSORS_VISIBLE 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. class IGNITION_SENSORS_VISIBLE SensorFactory { /// \brief Constructor @@ -72,81 +75,93 @@ namespace ignition 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) + /// \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(); - if (sensor) + auto type = _sdf->Get("type"); + auto name = _sdf->Get("name"); + + 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. - /// 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. @@ -161,17 +176,16 @@ 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] _type Sensor type from SDF - /// \return Pointer to the new sensor, nullptr on error. - private: std::shared_ptr LoadSensorPlugin( - const std::string &_type); + /// \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 diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc index 3ea510c2..b10ed384 100644 --- a/src/AirPressureSensor.cc +++ b/src/AirPressureSensor.cc @@ -26,7 +26,6 @@ #endif #include -#include #include #include "ignition/sensors/GaussianNoiseModel.hh" @@ -218,9 +217,3 @@ double AirPressureSensor::ReferenceAltitude() const return this->dataPtr->referenceAltitude; } -IGNITION_ADD_PLUGIN( - ignition::sensors::SensorTypePlugin, - ignition::sensors::SensorPlugin) -IGNITION_ADD_PLUGIN_ALIAS( - ignition::sensors::SensorTypePlugin, - "air_pressure") diff --git a/src/AltimeterSensor.cc b/src/AltimeterSensor.cc index 797d8ea1..f77ab47a 100644 --- a/src/AltimeterSensor.cc +++ b/src/AltimeterSensor.cc @@ -16,7 +16,6 @@ */ #include -#include #include #include "ignition/sensors/AltimeterSensor.hh" @@ -213,9 +212,3 @@ double AltimeterSensor::VerticalVelocity() const return this->dataPtr->verticalVelocity; } -IGNITION_ADD_PLUGIN( - ignition::sensors::SensorTypePlugin, - ignition::sensors::SensorPlugin) -IGNITION_ADD_PLUGIN_ALIAS( - ignition::sensors::SensorTypePlugin, - "altimeter") diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3d60405a..d076fdd7 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -41,7 +41,6 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE ignition-common${IGN_COMMON_VER}::profiler ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} - ignition-plugin${IGN_PLUGIN_VER}::loader ) target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PUBLIC DepthPoints_EXPORTS) @@ -61,7 +60,6 @@ target_link_libraries(${camera_target} ${rendering_target} PRIVATE ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} - ignition-plugin${IGN_PLUGIN_VER}::register ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ) @@ -72,7 +70,6 @@ target_link_libraries(${depth_camera_target} PRIVATE ${camera_target} ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} - ignition-plugin${IGN_PLUGIN_VER}::register ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ) @@ -84,7 +81,6 @@ target_link_libraries(${lidar_target} ${rendering_target} PRIVATE ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} - ignition-plugin${IGN_PLUGIN_VER}::register ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ) @@ -94,7 +90,6 @@ target_compile_definitions(${gpu_lidar_target} PUBLIC GpuLidarSensor_EXPORTS) target_link_libraries(${gpu_lidar_target} PRIVATE ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} - ignition-plugin${IGN_PLUGIN_VER}::register ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ${lidar_target} ) @@ -105,37 +100,20 @@ target_compile_definitions(${logical_camera_target} PUBLIC LogicalCameraSensor_E target_link_libraries(${logical_camera_target} PRIVATE ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} - ignition-plugin${IGN_PLUGIN_VER}::register ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ) set(magnetometer_sources MagnetometerSensor.cc) ign_add_component(magnetometer SOURCES ${magnetometer_sources} GET_TARGET_NAME magnetometer_target) -target_link_libraries(${magnetometer_target} - PRIVATE - ignition-plugin${IGN_PLUGIN_VER}::register -) set(imu_sources ImuSensor.cc) ign_add_component(imu SOURCES ${imu_sources} GET_TARGET_NAME imu_target) -target_link_libraries(${imu_target} - PRIVATE - ignition-plugin${IGN_PLUGIN_VER}::register -) set(altimeter_sources AltimeterSensor.cc) ign_add_component(altimeter SOURCES ${altimeter_sources} GET_TARGET_NAME altimeter_target) -target_link_libraries(${altimeter_target} - PRIVATE - ignition-plugin${IGN_PLUGIN_VER}::register -) set(air_pressure_sources AirPressureSensor.cc) ign_add_component(air_pressure SOURCES ${air_pressure_sources} GET_TARGET_NAME air_pressure_target) -target_link_libraries(${air_pressure_target} - PRIVATE - ignition-plugin${IGN_PLUGIN_VER}::register -) set(rgbd_camera_sources RgbdCameraSensor.cc) ign_add_component(rgbd_camera SOURCES ${rgbd_camera_sources} GET_TARGET_NAME rgbd_camera_target) @@ -144,7 +122,6 @@ target_link_libraries(${rgbd_camera_target} PRIVATE ${camera_target} ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} - ignition-plugin${IGN_PLUGIN_VER}::register ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ) @@ -154,7 +131,6 @@ target_link_libraries(${thermal_camera_target} PRIVATE ${camera_target} ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} - ignition-plugin${IGN_PLUGIN_VER}::register ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ) @@ -166,18 +142,18 @@ ign_build_tests(TYPE UNIT SOURCES Lidar_TEST.cc LIB_DEPS ${lidar_target}) ign_build_tests(TYPE UNIT SOURCES Camera_TEST.cc LIB_DEPS ${camera_target}) ign_build_tests(TYPE UNIT SOURCES ImuSensor_TEST.cc LIB_DEPS ${imu_target}) -set(plugin_test_targets) -list(APPEND plugin_test_targets "UNIT_Camera_TEST") -list(APPEND plugin_test_targets "UNIT_Lidar_TEST") -list(APPEND plugin_test_targets "UNIT_ImuSensor_TEST") - -foreach(plugin_test ${plugin_test_targets}) - if(TARGET ${plugin_test}) - set(_env_vars) - list(APPEND _env_vars "IGN_PLUGIN_PATH=$") - list(APPEND _env_vars "DYLD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") - - set_tests_properties(${plugin_test} PROPERTIES - ENVIRONMENT "${_env_vars}") - endif() -endforeach() +#set(plugin_test_targets) +#list(APPEND plugin_test_targets "UNIT_Camera_TEST") +#list(APPEND plugin_test_targets "UNIT_Lidar_TEST") +#list(APPEND plugin_test_targets "UNIT_ImuSensor_TEST") +# +#foreach(plugin_test ${plugin_test_targets}) +# if(TARGET ${plugin_test}) +# set(_env_vars) +# list(APPEND _env_vars "IGN_PLUGIN_PATH=$") +# list(APPEND _env_vars "DYLD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") +# +# set_tests_properties(${plugin_test} PROPERTIES +# ENVIRONMENT "${_env_vars}") +# endif() +#endforeach() diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index f1c19abf..75648486 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -33,7 +33,6 @@ #include #include #include -#include #include #include "ignition/sensors/CameraSensor.hh" @@ -634,9 +633,3 @@ double CameraSensor::Baseline() const return this->dataPtr->baseline; } -IGNITION_ADD_PLUGIN( - ignition::sensors::SensorTypePlugin, - ignition::sensors::SensorPlugin) -IGNITION_ADD_PLUGIN_ALIAS( - ignition::sensors::SensorTypePlugin, - "camera") diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 0f71d6ba..f5cd312c 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -185,15 +185,9 @@ TEST(Camera_TEST, Topic) auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, visualize); - auto sensorId = mgr.CreateSensor(cameraSdf); - EXPECT_NE(ignition::sensors::NO_SENSOR, sensorId); - - auto sensor = mgr.Sensor(sensorId); - EXPECT_NE(nullptr, sensor); - - auto camera = dynamic_cast(sensor); + auto camera = mgr.CreateSensor(cameraSdf); ASSERT_NE(nullptr, camera); - + EXPECT_NE(ignition::sensors::NO_SENSOR, camera->Id()); EXPECT_EQ("/camera", camera->Topic()); } @@ -203,14 +197,9 @@ TEST(Camera_TEST, Topic) auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, visualize); - auto sensorId = mgr.CreateSensor(cameraSdf); - EXPECT_NE(ignition::sensors::NO_SENSOR, sensorId); - - auto sensor = mgr.Sensor(sensorId); - EXPECT_NE(nullptr, sensor); - - auto camera = dynamic_cast(sensor); + auto camera = mgr.CreateSensor(cameraSdf); ASSERT_NE(nullptr, camera); + EXPECT_NE(ignition::sensors::NO_SENSOR, camera->Id()); EXPECT_EQ("/topic_with_spaces/characters", camera->Topic()); } @@ -221,8 +210,8 @@ TEST(Camera_TEST, Topic) auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, visualize); - auto sensorId = mgr.CreateSensor(cameraSdf); - EXPECT_EQ(ignition::sensors::NO_SENSOR, sensorId); + auto sensor = mgr.CreateSensor(cameraSdf); + EXPECT_EQ(nullptr, sensor); } } diff --git a/src/DepthCameraSensor.cc b/src/DepthCameraSensor.cc index 7759eb51..8e231e1f 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -34,8 +34,6 @@ #include #include -#include - #include #include "ignition/sensors/DepthCameraSensor.hh" @@ -628,9 +626,3 @@ double DepthCameraSensor::NearClip() const return this->dataPtr->near; } -IGNITION_ADD_PLUGIN( - ignition::sensors::SensorTypePlugin, - ignition::sensors::SensorPlugin) -IGNITION_ADD_PLUGIN_ALIAS( - ignition::sensors::SensorTypePlugin, - "depth_camera") diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc index f5739eb7..8db59f21 100644 --- a/src/GpuLidarSensor.cc +++ b/src/GpuLidarSensor.cc @@ -28,7 +28,6 @@ #include #include #include -#include #include #include "ignition/sensors/GpuLidarSensor.hh" @@ -109,7 +108,7 @@ void GpuLidarSensor::RemoveGpuRays( ////////////////////////////////////////////////// bool GpuLidarSensor::Load(const sdf::Sensor &_sdf) { - // Check if this is being loaded via "builtin" or via a plugin + // Check if this is being loaded via "builtin" or via another sensor if (!Lidar::Load(_sdf)) { return false; @@ -371,9 +370,3 @@ void GpuLidarSensorPrivate::FillPointCloudMsg(const float *_laserBuffer) } } -IGNITION_ADD_PLUGIN( - ignition::sensors::SensorTypePlugin, - ignition::sensors::SensorPlugin) -IGNITION_ADD_PLUGIN_ALIAS( - ignition::sensors::SensorTypePlugin, - "gpu_lidar") diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 38c7d648..f8e2db5e 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -25,7 +25,6 @@ #endif #include -#include #include #include "ignition/sensors/ImuSensor.hh" @@ -296,9 +295,3 @@ math::Quaterniond ImuSensor::Orientation() const return this->dataPtr->orientation; } -IGNITION_ADD_PLUGIN( - ignition::sensors::SensorTypePlugin, - ignition::sensors::SensorPlugin) -IGNITION_ADD_PLUGIN_ALIAS( - ignition::sensors::SensorTypePlugin, - "imu") diff --git a/src/Lidar.cc b/src/Lidar.cc index 6521b4d7..a73b10ee 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -17,7 +17,6 @@ #include #include #include -#include #include #include @@ -451,10 +450,3 @@ bool Lidar::IsActive() const return true; } -// Register as LidarSensor to conform to naming requirements -IGNITION_ADD_PLUGIN( - ignition::sensors::SensorTypePlugin, - ignition::sensors::SensorPlugin) -IGNITION_ADD_PLUGIN_ALIAS( - ignition::sensors::SensorTypePlugin, - "lidar") diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc index 800fa9f1..b190c099 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -21,7 +21,6 @@ #include #include #include -#include #include #include "ignition/sensors/SensorFactory.hh" @@ -201,9 +200,3 @@ msgs::LogicalCameraImage LogicalCameraSensor::Image() const return this->dataPtr->msg; } -IGNITION_ADD_PLUGIN( - ignition::sensors::SensorTypePlugin, - ignition::sensors::SensorPlugin) -IGNITION_ADD_PLUGIN_ALIAS( - ignition::sensors::SensorTypePlugin, - "logical_camera") diff --git a/src/MagnetometerSensor.cc b/src/MagnetometerSensor.cc index cc55699d..81a0da5f 100644 --- a/src/MagnetometerSensor.cc +++ b/src/MagnetometerSensor.cc @@ -26,7 +26,6 @@ #endif #include -#include #include #include @@ -232,9 +231,3 @@ math::Vector3d MagnetometerSensor::MagneticField() const return this->dataPtr->localField; } -IGNITION_ADD_PLUGIN( - ignition::sensors::SensorTypePlugin, - ignition::sensors::SensorPlugin) -IGNITION_ADD_PLUGIN_ALIAS( - ignition::sensors::SensorTypePlugin, - "magnetometer") diff --git a/src/Manager.cc b/src/Manager.cc index dc3ec761..355c8217 100644 --- a/src/Manager.cc +++ b/src/Manager.cc @@ -29,29 +29,10 @@ using namespace ignition::sensors; class ignition::sensors::ManagerPrivate { - /// \brief constructor - public: ManagerPrivate(); - - /// \brief destructor - public: ~ManagerPrivate(); - /// \brief Loaded sensors. - public: std::map> sensors; - - /// \brief Sensor factory for creating sensors from plugins; - public: SensorFactory sensorFactory; + public: std::map> sensors; }; -////////////////////////////////////////////////// -ManagerPrivate::ManagerPrivate() -{ -} - -////////////////////////////////////////////////// -ManagerPrivate::~ManagerPrivate() -{ -} - ////////////////////////////////////////////////// Manager::Manager() : dataPtr(new ManagerPrivate) @@ -78,9 +59,10 @@ ignition::sensors::Sensor *Manager::Sensor( } ////////////////////////////////////////////////// -void Manager::AddPluginPaths(const std::string &_paths) +void Manager::AddPluginPaths(const std::string &) { - this->dataPtr->sensorFactory.AddPluginPaths(_paths); + ignwarn << "Trying to add plugin paths, but Ignition Sensors doesn't support" + << " plugins anymore." << std::endl; } ////////////////////////////////////////////////// @@ -102,7 +84,7 @@ void Manager::RunOnce( ///////////////////////////////////////////////// ignition::sensors::SensorId Manager::AddSensor( - const std::shared_ptr &_sensor) + std::unique_ptr _sensor) { if (!_sensor) return NO_SENSOR; @@ -112,25 +94,19 @@ ignition::sensors::SensorId Manager::AddSensor( } ///////////////////////////////////////////////// -ignition::sensors::SensorId Manager::CreateSensor(const sdf::Sensor &_sdf) +ignition::sensors::SensorId Manager::CreateSensor(const sdf::Sensor &) { - auto sensor = this->dataPtr->sensorFactory.CreateSensor(_sdf); - if (!sensor) - return NO_SENSOR; - - SensorId id = sensor->Id(); - this->dataPtr->sensors[id] = std::move(sensor); - return id; + ignwarn << "Trying to create sensor without providing sensor type. Ignition" + << " Sensor doesn't support sensor registration anymore. Use the" + << " templated `CreateSensor` function instead." << std::endl; + return NO_SENSOR; } ///////////////////////////////////////////////// -ignition::sensors::SensorId Manager::CreateSensor(sdf::ElementPtr _sdf) +ignition::sensors::SensorId Manager::CreateSensor(sdf::ElementPtr) { - auto sensor = this->dataPtr->sensorFactory.CreateSensor(_sdf); - if (!sensor) - return NO_SENSOR; - - SensorId id = sensor->Id(); - this->dataPtr->sensors[id] = std::move(sensor); - return id; + ignwarn << "Trying to create sensor without providing sensor type. Ignition" + << " Sensor doesn't support sensor registration anymore. Use the" + << " templated `CreateSensor` function instead." << std::endl; + return NO_SENSOR; } diff --git a/src/Manager_TEST.cc b/src/Manager_TEST.cc index a57b2f46..3f67843d 100644 --- a/src/Manager_TEST.cc +++ b/src/Manager_TEST.cc @@ -29,14 +29,24 @@ class Manager_TEST : public ::testing::Test }; ////////////////////////////////////////////////// -TEST(Manager, construct) +class DummySensor : public ignition::sensors::Sensor +{ + public: virtual bool Update( + const std::chrono::steady_clock::duration &) override + { + return true; + } +}; + +////////////////////////////////////////////////// +TEST_F(Manager_TEST, construct) { ignition::sensors::Manager mgr; EXPECT_TRUE(mgr.Init()); - sdf::ElementPtr ptr; - ignition::sensors::SensorId id = mgr.CreateSensor(ptr); - EXPECT_EQ(id, ignition::sensors::NO_SENSOR); + sdf::ElementPtr ptr{nullptr}; + auto createdSensor = mgr.CreateSensor(ptr); + EXPECT_EQ(nullptr, createdSensor); ignition::sensors::Sensor *sensor = mgr.Sensor(0); EXPECT_EQ(sensor, nullptr); @@ -45,7 +55,7 @@ TEST(Manager, construct) } ////////////////////////////////////////////////// -TEST(Manager, removeSensor) +TEST_F(Manager_TEST, removeSensor) { ignition::sensors::Manager mgr; EXPECT_TRUE(mgr.Init()); diff --git a/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 0029eea2..d368d930 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -42,8 +42,6 @@ #pragma warning(pop) #endif -// This plugin loads another plugin, so it shouldn't include Register.hh -#include #include #include @@ -176,6 +174,14 @@ bool RgbdCameraSensor::Init() return this->Sensor::Init(); } +////////////////////////////////////////////////// +bool RgbdCameraSensor::Load(sdf::ElementPtr _sdf) +{ + sdf::Sensor sdfSensor; + sdfSensor.Load(_sdf); + return this->Load(sdfSensor); +} + ////////////////////////////////////////////////// bool RgbdCameraSensor::Load(const sdf::Sensor &_sdf) { @@ -605,9 +611,3 @@ unsigned int RgbdCameraSensor::ImageHeight() const return this->dataPtr->depthCamera->ImageHeight(); } -IGNITION_ADD_PLUGIN( - ignition::sensors::SensorTypePlugin, - ignition::sensors::SensorPlugin) -IGNITION_ADD_PLUGIN_ALIAS( - ignition::sensors::SensorTypePlugin, - "rgbd_camera") diff --git a/src/SensorFactory.cc b/src/SensorFactory.cc index 3c6a40ae..0a8dae55 100644 --- a/src/SensorFactory.cc +++ b/src/SensorFactory.cc @@ -15,50 +15,28 @@ * */ -#include - -#include #include -#include -#include "ignition/sensors/config.hh" #include "ignition/sensors/SensorFactory.hh" /// \brief Private data class for SensorFactory class ignition::sensors::SensorFactoryPrivate { - /// \brief Constructor - public: SensorFactoryPrivate(); - - /// \brief A map of loaded sensor plugins and their type. - public: std::map> sensorPlugins; - - /// \brief Stores paths to search for on file system - public: ignition::common::SystemPaths systemPaths; - - /// \brief Environment variable containing lookup paths for sensors. - public: const std::string pathEnv{"IGN_SENSORS_PATH"}; }; using namespace ignition; using namespace sensors; ////////////////////////////////////////////////// -SensorFactoryPrivate::SensorFactoryPrivate() -{ - this->systemPaths.AddPluginPaths(IGN_SENSORS_PLUGIN_PATH); -} - -////////////////////////////////////////////////// -void SensorFactory::AddPluginPaths(const std::string &_path) +void SensorFactory::AddPluginPaths(const std::string &) { - this->dataPtr->systemPaths.AddPluginPaths(_path); + ignwarn << "Trying to add plugin paths, but Ignition Sensors doesn't support" + << " plugins anymore." << std::endl; } ////////////////////////////////////////////////// SensorFactory::SensorFactory() : dataPtr(new SensorFactoryPrivate) { - this->dataPtr->systemPaths.SetPluginPathEnv(this->dataPtr->pathEnv); } ////////////////////////////////////////////////// @@ -66,157 +44,20 @@ SensorFactory::~SensorFactory() { } -////////////////////////////////////////////////// -std::shared_ptr SensorFactory::LoadSensorPlugin( - const std::string &_type) -{ - auto filename = IGN_SENSORS_PLUGIN_NAME(_type); - - std::string fullPath = - this->dataPtr->systemPaths.FindSharedLibrary(filename); - if (fullPath.empty()) - { - ignerr << "Unable to find sensor plugin path for [" << filename << "]\n"; - return std::shared_ptr(); - } - - plugin::Loader pluginLoader; - auto pluginNames = pluginLoader.LoadLib(fullPath); - - if (pluginNames.empty()) - { - ignerr << "Failed to load plugin [" << filename << - "] : couldn't load library on path [" << fullPath << - "]." << std::endl; - return std::shared_ptr(); - } - - auto plugin = pluginLoader.Instantiate(_type); - if (!plugin) - { - std::stringstream error; - error << "Failed to instantiate plugin [" << _type << "] from [" - << fullPath << "]. Available plugins:" << std::endl; - for (auto name : pluginNames) - { - error << "- " << name << std::endl; - } - ignerr << error.str(); - - return nullptr; - } - - auto sensorPlugin = - plugin->QueryInterfaceSharedPtr(); - - if (!sensorPlugin) - { - ignerr << "Failed to query interface from [" << fullPath << "]" - << std::endl; - return nullptr; - } - - return sensorPlugin; -} - ///////////////////////////////////////////////// -std::unique_ptr SensorFactory::CreateSensor(const sdf::Sensor &_sdf) +std::unique_ptr SensorFactory::CreateSensor(const sdf::Sensor &) { - std::unique_ptr result; - std::shared_ptr sensorPlugin; - std::string type = _sdf.TypeStr(); - - auto it = this->dataPtr->sensorPlugins.find(type); - if (it != this->dataPtr->sensorPlugins.end()) - sensorPlugin = it->second; - else - { - sensorPlugin = this->LoadSensorPlugin(type); - if (!sensorPlugin) - { - ignerr << "Unable to instantiate sensor plugin for [" << type - << "]\n"; - return nullptr; - } - - this->dataPtr->sensorPlugins[type] = sensorPlugin; - } - - auto sensor = sensorPlugin->New(); - if (!sensor) - { - ignerr << "Unable to instantiate sensor for [" << type << "]\n"; - return nullptr; - } - - if (!sensor->Load(_sdf)) - { - ignerr << "Sensor::Load failed for plugin [" << type << "]\n"; - return nullptr; - } - - if (!sensor->Init()) - { - ignerr << "Sensor::Init failed for plugin [" << type << "]\n"; - return nullptr; - } - - result.reset(sensor); - return result; + ignwarn << "Trying to create sensor without providing sensor type. Ignition" + << " Sensor doesn't support sensor registration anymore. Use the" + << " templated `CreateSensor` function instead." << std::endl; + return nullptr; } ///////////////////////////////////////////////// -std::unique_ptr SensorFactory::CreateSensor(sdf::ElementPtr _sdf) +std::unique_ptr SensorFactory::CreateSensor(sdf::ElementPtr) { - std::unique_ptr result; - if (_sdf) - { - if (_sdf->GetName() == "sensor") - { - std::shared_ptr sensorPlugin; - std::string type = _sdf->Get("type"); - auto it = this->dataPtr->sensorPlugins.find(type); - if (it != this->dataPtr->sensorPlugins.end()) - sensorPlugin = it->second; - else - { - sensorPlugin = this->LoadSensorPlugin(type); - if (!sensorPlugin) - { - ignerr << "Unable to instantiate sensor plugin for [" << type - << "]\n"; - return nullptr; - } - - this->dataPtr->sensorPlugins[type] = sensorPlugin; - } - - auto sensor = sensorPlugin->New(); - if (!sensor) - { - ignerr << "Unable to instantiate sensor for [" << type << "]\n"; - return nullptr; - } - - if (!sensor->Load(_sdf)) - { - ignerr << "Sensor::Load failed for plugin [" << type << "]\n"; - return nullptr; - } - - if (!sensor->Init()) - { - ignerr << "Sensor::Init failed for plugin [" << type << "]\n"; - return nullptr; - } - result.reset(sensor); - return result; - } - else - { - ignerr << "Provided SDF is not a element.\n"; - } - } - + ignwarn << "Trying to create sensor without providing sensor type. Ignition" + << " Sensor doesn't support sensor registration anymore. Use the" + << " templated `CreateSensor` function instead." << std::endl; return nullptr; } diff --git a/src/ThermalCameraSensor.cc b/src/ThermalCameraSensor.cc index 41addd9c..d47a64b4 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -36,9 +36,6 @@ #include #include -// This plugin loads another plugin, so it shouldn't include Register.hh -#include - #include #include "ignition/sensors/ThermalCameraSensor.hh" @@ -651,9 +648,3 @@ bool ThermalCameraSensorPrivate::SaveImage(const uint16_t *_data, return true; } -IGNITION_ADD_PLUGIN( - ignition::sensors::SensorTypePlugin, - ignition::sensors::SensorPlugin) -IGNITION_ADD_PLUGIN_ALIAS( - ignition::sensors::SensorTypePlugin, - "thermal_camera") diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index fe376c93..e4a9d7c4 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -39,7 +39,6 @@ if (DRI_TESTS) ${PROJECT_LIBRARY_TARGET_NAME}-gpu_lidar ${PROJECT_LIBRARY_TARGET_NAME}-rgbd_camera ${PROJECT_LIBRARY_TARGET_NAME}-thermal_camera - ignition-plugin${IGN_PLUGIN_VER}::core ) endif() @@ -52,18 +51,17 @@ ign_build_tests(TYPE INTEGRATION ${PROJECT_LIBRARY_TARGET_NAME}-logical_camera ${PROJECT_LIBRARY_TARGET_NAME}-magnetometer ${PROJECT_LIBRARY_TARGET_NAME}-imu - ignition-plugin${IGN_PLUGIN_VER}::core ) -foreach(plugin_test ${dri_tests} ${tests}) - get_filename_component(BINARY_NAME ${plugin_test} NAME_WE) - set(BINARY_NAME "${TEST_TYPE}_${BINARY_NAME}") - if(TARGET ${BINARY_NAME}) - set(_env_vars) - list(APPEND _env_vars "IGN_PLUGIN_PATH=$") - list(APPEND _env_vars "DYLD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") - - set_tests_properties(${BINARY_NAME} PROPERTIES - ENVIRONMENT "${_env_vars}") - endif() -endforeach() +#foreach(plugin_test ${dri_tests} ${tests}) +# get_filename_component(BINARY_NAME ${plugin_test} NAME_WE) +# set(BINARY_NAME "${TEST_TYPE}_${BINARY_NAME}") +# if(TARGET ${BINARY_NAME}) +# set(_env_vars) +# list(APPEND _env_vars "IGN_PLUGIN_PATH=$") +# list(APPEND _env_vars "DYLD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") +# +# set_tests_properties(${BINARY_NAME} PROPERTIES +# ENVIRONMENT "${_env_vars}") +# endif() +#endforeach() diff --git a/test/integration/air_pressure_plugin.cc b/test/integration/air_pressure_plugin.cc index a7b74fb4..347b2649 100644 --- a/test/integration/air_pressure_plugin.cc +++ b/test/integration/air_pressure_plugin.cc @@ -172,20 +172,12 @@ 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 = - sf.CreateSensor(airPressureSdf); - std::unique_ptr sensor( - dynamic_cast(s.release())); - - // Make sure the above dynamic cast worked. + auto sensor = sf.CreateSensor( + airPressureSdf); ASSERT_NE(nullptr, sensor); - std::unique_ptr sNoise = - sf.CreateSensor(airPressureSdfNoise); - std::unique_ptr sensorNoise( - dynamic_cast(sNoise.release())); - - // Make sure the above dynamic cast worked. + auto sensorNoise = sf.CreateSensor( + airPressureSdfNoise); ASSERT_NE(nullptr, sensorNoise); // verify initial readings @@ -245,11 +237,8 @@ TEST_F(AirPressureSensorTest, Topic) auto airPressureSdf = AirPressureToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); - auto sensor = factory.CreateSensor(airPressureSdf); - EXPECT_NE(nullptr, sensor); - - auto airPressure = - dynamic_cast(sensor.release()); + auto airPressure = factory.CreateSensor< + ignition::sensors::AirPressureSensor>(airPressureSdf); ASSERT_NE(nullptr, airPressure); EXPECT_EQ("/air_pressure", airPressure->Topic()); @@ -261,11 +250,8 @@ TEST_F(AirPressureSensorTest, Topic) auto airPressureSdf = AirPressureToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); - auto sensor = factory.CreateSensor(airPressureSdf); - EXPECT_NE(nullptr, sensor); - - auto airPressure = - dynamic_cast(sensor.release()); + auto airPressure = factory.CreateSensor< + ignition::sensors::AirPressureSensor>(airPressureSdf); ASSERT_NE(nullptr, airPressure); EXPECT_EQ("/topic_with_spaces/characters", airPressure->Topic()); @@ -277,7 +263,8 @@ TEST_F(AirPressureSensorTest, Topic) auto airPressureSdf = AirPressureToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); - auto sensor = factory.CreateSensor(airPressureSdf); + auto sensor = factory.CreateSensor< + ignition::sensors::AirPressureSensor>(airPressureSdf); ASSERT_EQ(nullptr, sensor); } } diff --git a/test/integration/altimeter_plugin.cc b/test/integration/altimeter_plugin.cc index 4139ad0e..653f07df 100644 --- a/test/integration/altimeter_plugin.cc +++ b/test/integration/altimeter_plugin.cc @@ -178,20 +178,12 @@ 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 = - sf.CreateSensor(altimeterSdf); - std::unique_ptr sensor( - dynamic_cast(s.release())); - - // Make sure the above dynamic cast worked. + auto sensor = + sf.CreateSensor(altimeterSdf); ASSERT_NE(nullptr, sensor); - std::unique_ptr sNoise = - sf.CreateSensor(altimeterSdfNoise); - std::unique_ptr sensorNoise( - dynamic_cast(sNoise.release())); - - // Make sure the above dynamic cast worked. + auto sensorNoise = + sf.CreateSensor(altimeterSdfNoise); ASSERT_NE(nullptr, sensorNoise); // verify initial readings @@ -273,11 +265,8 @@ TEST_F(AltimeterSensorTest, Topic) auto altimeterSdf = AltimeterToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); - auto sensor = factory.CreateSensor(altimeterSdf); - EXPECT_NE(nullptr, sensor); - - auto altimeter = - dynamic_cast(sensor.release()); + auto altimeter = factory.CreateSensor< + ignition::sensors::AltimeterSensor>(altimeterSdf); ASSERT_NE(nullptr, altimeter); EXPECT_EQ("/altimeter", altimeter->Topic()); @@ -289,11 +278,8 @@ TEST_F(AltimeterSensorTest, Topic) auto altimeterSdf = AltimeterToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); - auto sensor = factory.CreateSensor(altimeterSdf); - EXPECT_NE(nullptr, sensor); - - auto altimeter = - dynamic_cast(sensor.release()); + auto altimeter = factory.CreateSensor< + ignition::sensors::AltimeterSensor>(altimeterSdf); ASSERT_NE(nullptr, altimeter); EXPECT_EQ("/topic_with_spaces/characters", altimeter->Topic()); @@ -305,7 +291,8 @@ TEST_F(AltimeterSensorTest, Topic) auto altimeterSdf = AltimeterToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); - auto sensor = factory.CreateSensor(altimeterSdf); + auto sensor = factory.CreateSensor< + ignition::sensors::AltimeterSensor>(altimeterSdf); ASSERT_EQ(nullptr, sensor); } } diff --git a/test/integration/camera_plugin.cc b/test/integration/camera_plugin.cc index ad86ee99..e2983c0e 100644 --- a/test/integration/camera_plugin.cc +++ b/test/integration/camera_plugin.cc @@ -17,6 +17,7 @@ #include +#include #include #include #include @@ -47,10 +48,15 @@ #include "test_config.h" // NOLINT(build/include) #include "TransportTestTools.hh" - class CameraSensorTest: public testing::Test, public testing::WithParamInterface { + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + } + // Create a Camera sensor from a SDF and gets a image message public: void ImagesWithBuiltinSDF(const std::string &_renderEngine); }; diff --git a/test/integration/gpu_lidar_sensor_plugin.cc b/test/integration/gpu_lidar_sensor_plugin.cc index d906e62e..f76ec4e2 100644 --- a/test/integration/gpu_lidar_sensor_plugin.cc +++ b/test/integration/gpu_lidar_sensor_plugin.cc @@ -866,15 +866,9 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); - auto sensorId = mgr.CreateSensor(lidarSdf); - EXPECT_NE(ignition::sensors::NO_SENSOR, sensorId); - - auto sensor = mgr.Sensor(sensorId); - EXPECT_NE(nullptr, sensor); - - auto lidar = dynamic_cast(sensor); + auto lidar = mgr.CreateSensor(lidarSdf); ASSERT_NE(nullptr, lidar); - + EXPECT_NE(ignition::sensors::NO_SENSOR, lidar->Id()); EXPECT_EQ("/lidar/points", lidar->Topic()); } @@ -886,14 +880,9 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); - auto sensorId = mgr.CreateSensor(lidarSdf); - EXPECT_NE(ignition::sensors::NO_SENSOR, sensorId); - - auto sensor = mgr.Sensor(sensorId); - EXPECT_NE(nullptr, sensor); - - auto lidar = dynamic_cast(sensor); + auto lidar = mgr.CreateSensor(lidarSdf); ASSERT_NE(nullptr, lidar); + EXPECT_NE(ignition::sensors::NO_SENSOR, lidar->Id()); EXPECT_EQ("/topic_with_spaces/characters/points", lidar->Topic()); } @@ -906,8 +895,8 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) vertSamples, vertResolution, vertMinAngle, vertMaxAngle, rangeResolution, rangeMin, rangeMax, alwaysOn, visualize); - auto sensorId = mgr.CreateSensor(lidarSdf); - EXPECT_EQ(ignition::sensors::NO_SENSOR, sensorId); + auto sensor = mgr.CreateSensor(lidarSdf); + EXPECT_EQ(nullptr, sensor); } } diff --git a/test/integration/imu_plugin.cc b/test/integration/imu_plugin.cc index f24fcff6..43c6a991 100644 --- a/test/integration/imu_plugin.cc +++ b/test/integration/imu_plugin.cc @@ -113,12 +113,7 @@ TEST_F(ImuSensorTest, 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 = - sf.CreateSensor(imuSdf); - std::unique_ptr sensor( - dynamic_cast(s.release())); - - // Make sure the above dynamic cast worked. + auto sensor = sf.CreateSensor(imuSdf); ASSERT_NE(nullptr, sensor); // subscribe to the topic @@ -250,10 +245,7 @@ TEST_F(ImuSensorTest, Topic) auto imuSdf = ImuToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); - auto sensor = factory.CreateSensor(imuSdf); - EXPECT_NE(nullptr, sensor); - - auto imu = dynamic_cast(sensor.release()); + auto imu = factory.CreateSensor(imuSdf); ASSERT_NE(nullptr, imu); EXPECT_EQ("/imu", imu->Topic()); @@ -265,11 +257,7 @@ TEST_F(ImuSensorTest, Topic) auto imuSdf = ImuToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); - auto sensor = factory.CreateSensor(imuSdf); - EXPECT_NE(nullptr, sensor); - - auto imu = - dynamic_cast(sensor.release()); + auto imu = factory.CreateSensor(imuSdf); ASSERT_NE(nullptr, imu); EXPECT_EQ("/topic_with_spaces/characters", imu->Topic()); @@ -281,7 +269,7 @@ TEST_F(ImuSensorTest, Topic) auto imuSdf = ImuToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); - auto sensor = factory.CreateSensor(imuSdf); + auto sensor = factory.CreateSensor(imuSdf); ASSERT_EQ(nullptr, sensor); } } diff --git a/test/integration/logical_camera_plugin.cc b/test/integration/logical_camera_plugin.cc index 61191220..d4ff2d3e 100644 --- a/test/integration/logical_camera_plugin.cc +++ b/test/integration/logical_camera_plugin.cc @@ -154,14 +154,9 @@ TEST_F(LogicalCameraSensorTest, DetectBox) 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 = - sf.CreateSensor(logicalCameraSdf); - std::unique_ptr sensor( - dynamic_cast(s.release())); - - // Make sure the above dynamic cast worked. + auto sensor = sf.CreateSensor( + logicalCameraSdf); ASSERT_NE(nullptr, sensor); // verify initial image @@ -264,12 +259,8 @@ TEST_F(LogicalCameraSensorTest, Topic) updateRate, topic, near, far, horzFov, aspectRatio, alwaysOn, visualize); - auto sensor = factory.CreateSensor(logicalCameraSdf); - EXPECT_NE(nullptr, sensor); - - auto logicalCamera = - dynamic_cast( - sensor.release()); + auto logicalCamera = factory.CreateSensor< + ignition::sensors::LogicalCameraSensor>(logicalCameraSdf); ASSERT_NE(nullptr, logicalCamera); EXPECT_EQ("/logical_camera", logicalCamera->Topic()); @@ -282,12 +273,8 @@ TEST_F(LogicalCameraSensorTest, Topic) updateRate, topic, near, far, horzFov, aspectRatio, alwaysOn, visualize); - auto sensor = factory.CreateSensor(logicalCameraSdf); - EXPECT_NE(nullptr, sensor); - - auto logicalCamera = - dynamic_cast( - sensor.release()); + auto logicalCamera = factory.CreateSensor< + ignition::sensors::LogicalCameraSensor>(logicalCameraSdf); ASSERT_NE(nullptr, logicalCamera); EXPECT_EQ("/topic_with_spaces/characters", logicalCamera->Topic()); @@ -300,7 +287,8 @@ TEST_F(LogicalCameraSensorTest, Topic) updateRate, topic, near, far, horzFov, aspectRatio, alwaysOn, visualize); - auto sensor = factory.CreateSensor(logicalCameraSdf); + auto sensor = factory.CreateSensor< + ignition::sensors::LogicalCameraSensor>(logicalCameraSdf); ASSERT_EQ(nullptr, sensor); } } diff --git a/test/integration/magnetometer_plugin.cc b/test/integration/magnetometer_plugin.cc index 2767515b..a96fe766 100644 --- a/test/integration/magnetometer_plugin.cc +++ b/test/integration/magnetometer_plugin.cc @@ -183,18 +183,12 @@ 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 = - sf.CreateSensor(magnetometerSdf); - std::unique_ptr sensor( - dynamic_cast(s.release())); - - std::unique_ptr sNoise = - sf.CreateSensor(magnetometerSdfNoise); - std::unique_ptr sensorNoise( - dynamic_cast(sNoise.release())); - - // Make sure the above dynamic cast worked. + auto sensor = sf.CreateSensor( + magnetometerSdf); ASSERT_NE(nullptr, sensor); + + auto sensorNoise = sf.CreateSensor( + magnetometerSdfNoise); ASSERT_NE(nullptr, sensorNoise); // subscribe to the topic @@ -309,11 +303,8 @@ TEST_F(MagnetometerSensorTest, Topic) auto magnetometerSdf = MagnetometerToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); - auto sensor = factory.CreateSensor(magnetometerSdf); - EXPECT_NE(nullptr, sensor); - - auto magnetometer = - dynamic_cast(sensor.release()); + auto magnetometer = factory.CreateSensor< + ignition::sensors::MagnetometerSensor>(magnetometerSdf); ASSERT_NE(nullptr, magnetometer); EXPECT_EQ("/magnetometer", magnetometer->Topic()); @@ -325,11 +316,8 @@ TEST_F(MagnetometerSensorTest, Topic) auto magnetometerSdf = MagnetometerToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); - auto sensor = factory.CreateSensor(magnetometerSdf); - EXPECT_NE(nullptr, sensor); - - auto magnetometer = - dynamic_cast(sensor.release()); + auto magnetometer = factory.CreateSensor< + ignition::sensors::MagnetometerSensor>(magnetometerSdf); ASSERT_NE(nullptr, magnetometer); EXPECT_EQ("/topic_with_spaces/characters", magnetometer->Topic()); @@ -341,7 +329,8 @@ TEST_F(MagnetometerSensorTest, Topic) auto magnetometerSdf = MagnetometerToSdf(name, sensorPose, updateRate, topic, alwaysOn, visualize); - auto sensor = factory.CreateSensor(magnetometerSdf); + auto sensor = factory.CreateSensor< + ignition::sensors::MagnetometerSensor>(magnetometerSdf); ASSERT_EQ(nullptr, sensor); } } diff --git a/tutorials/02_install.md b/tutorials/02_install.md index 73a3f8d2..eb2f23d1 100644 --- a/tutorials/02_install.md +++ b/tutorials/02_install.md @@ -90,7 +90,7 @@ conda activate ign-ws Install Ignition dependencies, replacing `<#>` with the desired versions: ``` -conda install libignition-cmake<#> libignition-common<#> libignition-math<#> libignition-transport<#> libignition-msgs<#> libignition-plugin<#> --channel conda-forge +conda install libignition-cmake<#> libignition-common<#> libignition-math<#> libignition-transport<#> libignition-msgs<#> --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. From a42953b9cb5171c6b194e2c269a27d2cda3d5efd Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 5 Aug 2021 14:25:06 -0700 Subject: [PATCH 12/18] Update examples and some other cleanup Signed-off-by: Louise Poubel --- Migration.md | 2 + examples/custom_sensor/CMakeLists.txt | 8 +- .../{CustomSensor.cc => DoubleSensor.cc} | 68 +++++++---- .../{CustomSensor.hh => DoubleSensor.hh} | 14 +-- examples/custom_sensor/README.md | 27 ++++- examples/custom_sensor/custom_sensor.sdf | 16 --- examples/loop_sensor/CMakeLists.txt | 17 ++- examples/loop_sensor/README.md | 110 +++++------------- examples/loop_sensor/altimeter_example.sdf | 27 ----- examples/loop_sensor/main.cc | 104 ++++++++++++----- examples/loop_sensor/sensors.sdf | 39 +++++++ src/CMakeLists.txt | 15 --- src/Manager_TEST.cc | 36 +++++- test/integration/CMakeLists.txt | 12 -- 14 files changed, 265 insertions(+), 230 deletions(-) rename examples/custom_sensor/{CustomSensor.cc => DoubleSensor.cc} (53%) rename examples/custom_sensor/{CustomSensor.hh => DoubleSensor.hh} (88%) delete mode 100644 examples/custom_sensor/custom_sensor.sdf delete mode 100644 examples/loop_sensor/altimeter_example.sdf create mode 100644 examples/loop_sensor/sensors.sdf diff --git a/Migration.md b/Migration.md index ab85ebdf..341e4c13 100644 --- a/Migration.md +++ b/Migration.md @@ -26,6 +26,8 @@ release will remove the deprecated code. + ***Replacement*** template std::unique_ptr CreateSensor(const sdf::Sensor &); + ***Deprecation*** void AddPluginPaths(const std::string &) + ***Replacement*** None; see above. + + ***Deprecation*** IGN_SENSORS_REGISTER_SENSOR + + ***Replacement*** None; see above. 1. **include/sensors/Manager.hh** + ***Deprecation*** SensorId CreateSensor(sdf::ElementPtr); diff --git a/examples/custom_sensor/CMakeLists.txt b/examples/custom_sensor/CMakeLists.txt index 6b0d08ed..8df1bcaa 100644 --- a/examples/custom_sensor/CMakeLists.txt +++ b/examples/custom_sensor/CMakeLists.txt @@ -1,12 +1,10 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) -find_package(ignition-cmake2 REQUIRED) - -project(custom_sensor) +project(double_sensor) +find_package(ignition-cmake2 REQUIRED) find_package(ignition-sensors6 REQUIRED) -add_library(${PROJECT_NAME} SHARED CustomSensor.cc) -set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17) +add_library(${PROJECT_NAME} SHARED DoubleSensor.cc) target_link_libraries(${PROJECT_NAME} PRIVATE ignition-sensors6::ignition-sensors6) diff --git a/examples/custom_sensor/CustomSensor.cc b/examples/custom_sensor/DoubleSensor.cc similarity index 53% rename from examples/custom_sensor/CustomSensor.cc rename to examples/custom_sensor/DoubleSensor.cc index ed6e137a..f1dba418 100644 --- a/examples/custom_sensor/CustomSensor.cc +++ b/examples/custom_sensor/DoubleSensor.cc @@ -18,56 +18,80 @@ #include #include -#include #include #include -#include "CustomSensor.hh" +#include "DoubleSensor.hh" using namespace custom; ////////////////////////////////////////////////// -bool CustomSensor::Load(sdf::ElementPtr _sdf) +bool DoubleSensor::Load(const sdf::Sensor &_sdf) { - if (!_sdf) + if (_sdf.Type() != sdf::SensorType::CUSTOM) { - ignerr << "Null SDF pointer." << std::endl; + ignerr << "Trying to load custom sensor, but got type [" + << static_cast(_sdf.Type()) << "] instead." << std::endl; return false; } - if (_sdf->GetName() != "sensor") + auto elem = _sdf.Element(); + if (nullptr == elem) { - ignerr << "SDF element is not a sensor." << std::endl; + ignerr << "SDF missing element pointer." << std::endl; return false; } - std::string type = _sdf->Get("type"); - if (type != "custom_sensor") + if (!elem->HasAttribute("ignition:type")) { - ignerr << "Trying to load [custom_sensor], got [" << type << "] instead." - << std::endl; + ignerr << "Custom sensor missing `ignition::type` attribute. " + << "Not loading." << std::endl; + return false; + } + + auto customType = elem->Get("ignition:type"); + if (customType != "double") + { + ignerr << "Trying to load sensor of type [double], but got sensor of type [" + << customType << "]." << std::endl; return false; } // Load common sensor params ignition::sensors::Sensor::Load(_sdf); - // Load noise - if (_sdf->HasElement("noise")) + // Advertise topic where data will be published + this->pub = this->node.Advertise(this->Topic()); + + if (!elem->HasElement("ignition:double")) + { + igndbg << "No custom configuration for [" << this->Topic() << "]" + << std::endl; + return true; + } + + auto customElem = elem->GetElement("ignition:double"); + + if (!customElem->HasElement("noise")) { - sdf::Noise noiseSdf; - noiseSdf.Load(_sdf->GetElement("noise")); - this->noise = ignition::sensors::NoiseFactory::NewNoiseModel(noiseSdf); + igndbg << "No noise for [" << this->Topic() << "]" << std::endl; + return true; } - // Advertise topic where data will be published - this->pub = this->node.Advertise(this->Topic()); + sdf::Noise noiseSdf; + noiseSdf.Load(customElem->GetElement("noise")); + this->noise = ignition::sensors::NoiseFactory::NewNoiseModel(noiseSdf); + if (nullptr == this->noise) + { + ignerr << "Failed to load noise." << std::endl; + return false; + } return true; } ////////////////////////////////////////////////// -bool CustomSensor::Update(const std::chrono::steady_clock::duration &_now) +bool DoubleSensor::Update(const std::chrono::steady_clock::duration &_now) { ignition::msgs::Double msg; *msg.mutable_header()->mutable_stamp() = ignition::msgs::Convert(_now); @@ -85,9 +109,3 @@ bool CustomSensor::Update(const std::chrono::steady_clock::duration &_now) return true; } -IGNITION_ADD_PLUGIN( - ignition::sensors::SensorTypePlugin, - ignition::sensors::SensorPlugin) -IGNITION_ADD_PLUGIN_ALIAS( - ignition::sensors::SensorTypePlugin, - "custom_sensor") diff --git a/examples/custom_sensor/CustomSensor.hh b/examples/custom_sensor/DoubleSensor.hh similarity index 88% rename from examples/custom_sensor/CustomSensor.hh rename to examples/custom_sensor/DoubleSensor.hh index d96f86c8..3406afd7 100644 --- a/examples/custom_sensor/CustomSensor.hh +++ b/examples/custom_sensor/DoubleSensor.hh @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef CUSTOMSENSORSENSOR_HH_ -#define CUSTOMSENSORSENSOR_HH_ +#ifndef DOUBLESENSOR_HH_ +#define DOUBLESENSOR_HH_ #include #include @@ -24,12 +24,12 @@ namespace custom { /// \brief Example sensor that publishes a noisy double value. - class CustomSensor : public ignition::sensors::Sensor + class DoubleSensor : public ignition::sensors::Sensor { /// \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; + public: virtual bool Load(const sdf::Sensor &_sdf) override; /// \brief Update the sensor and generate data /// \param[in] _now The current time @@ -37,6 +37,9 @@ namespace custom public: virtual bool Update( const std::chrono::steady_clock::duration &_now) override; + /// \brief Latest data + public: double data{0.0}; + /// \brief Noise that will be applied to the sensor data private: ignition::sensors::NoisePtr noise{nullptr}; @@ -45,9 +48,6 @@ namespace custom /// \brief Publishes sensor data private: ignition::transport::Node::Publisher pub; - - /// \brief Latest data - private: double data{0.0}; }; } diff --git a/examples/custom_sensor/README.md b/examples/custom_sensor/README.md index 0c8068d0..8d410464 100644 --- a/examples/custom_sensor/README.md +++ b/examples/custom_sensor/README.md @@ -1,7 +1,7 @@ # Custom sensor -This example creates a simple custom sensor that produces a single number as -data and publishes it to a topic. +This example creates a simple custom sensor called `DoubleSensor`, which +produces a single number as data and publishes it to a topic. ## Build @@ -15,9 +15,24 @@ cmake .. make ``` -This will generate a shared library with the sensor called `ignition-sensors5-custom_sensor`. +This will generate a shared library with the sensor called `double_sensor`. -## Test +## Use + +This sensor with Ignition Gazebo, or with any downstream application that uses +the Ignition Sensors API. Listed here are two ways of testing this sensor, one +with Gazebo and one with a custom program. + +### With a custom program + +The [loop_sensor](../loop_sensor) example can be used to load an SDF file with +configuration for this sensor and run it in a loop. See that example's +instructions. + +### With Ignition Gazebo + +The +[custom_sensor_system](https://github.com/ignitionrobotics/ign-gazebo/tree/main/examples/plugin/custom_sensor_system) +example can be used to load this sensor into Gazebo and update it during the +simulation. -The "loop_sensor" example can be used to load an SDF file with configuration for -this sensor and run it in a loop. See that example's instructions. diff --git a/examples/custom_sensor/custom_sensor.sdf b/examples/custom_sensor/custom_sensor.sdf deleted file mode 100644 index 243e58d8..00000000 --- a/examples/custom_sensor/custom_sensor.sdf +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - 10 - /custom_data - - gaussian - 1.0 - 0.05 - - - - - diff --git a/examples/loop_sensor/CMakeLists.txt b/examples/loop_sensor/CMakeLists.txt index dddc347c..d3f898f3 100644 --- a/examples/loop_sensor/CMakeLists.txt +++ b/examples/loop_sensor/CMakeLists.txt @@ -1,8 +1,21 @@ cmake_minimum_required(VERSION 3.5 FATAL_ERROR) project(loop_sensor) -find_package(ignition-sensors6 REQUIRED) +find_package(ignition-sensors6 REQUIRED + COMPONENTS + # Find built-in sensors + altimeter +) + +# A simple example of how to find custom sensors +add_subdirectory(../custom_sensor double_sensor) add_executable(${PROJECT_NAME} main.cc) target_link_libraries(${PROJECT_NAME} PUBLIC - ignition-sensors6::ignition-sensors6) + ignition-sensors6::ignition-sensors6 + + # Link to custom sensors + double_sensor + + # Link to built-in sensors + ignition-sensors6::altimeter) diff --git a/examples/loop_sensor/README.md b/examples/loop_sensor/README.md index 765d2e92..c8fa38f7 100644 --- a/examples/loop_sensor/README.md +++ b/examples/loop_sensor/README.md @@ -1,9 +1,12 @@ # Loop sensor -Example executable that loads a sensor from an SDF file and updates it in a -loop until the user closes the program. +Example executable that loads sensors from an SDF world file and updates them in +a loop until the user closes the program. -This example can't load rendering sensors. +The example works with 2 types of sensors: + +* A built-in type: altimeter +* A custom type: "double sensor" ## Build @@ -19,17 +22,20 @@ make This will generate an executable called `loop_sensor`. -## Load an official sensor +It will also compile the `custom_sensor` example, creating a library called +`libdouble_sensor`. + +## Run -Try for example loading the accompanying `altimeter_sensor.sdf` file to run -an altimeter sensor, which is installed with Ignition Sensors: +Try loading the accompanying `sensors.sdf`: ``` cd examples/loop_sensor/build -./loop_sensor ../altimeter_example.sdf +./loop_sensor ../sensors.sdf ``` -On another terminal, check that the altimeter is generating data on a topic: +On another terminal, check that the altimeter and double sensor are generating +data on a topic: ``` ign topic -l @@ -39,9 +45,10 @@ You should see: ``` /altimeter +/double ``` -Then listen to the data: +Then listen to the altimeter data: ``` ign topic -e -t /altimeter @@ -53,23 +60,7 @@ You'll see data like: ... header { stamp { - sec: 12 - } - data { - key: "frame_id" - value: "altimeter" - } - data { - key: "seq" - value: "12" - } -} -vertical_position: 0.9903850972191578 -vertical_velocity: 2.9159486154028573 - -header { - stamp { - sec: 13 + sec: 35 } data { key: "frame_id" @@ -77,52 +68,18 @@ header { } data { key: "seq" - value: "13" + value: "35" } } -vertical_position: 1.139457534900868 -vertical_velocity: 3.1484160275030266 +vertical_position: 0.91297697595395233 +vertical_velocity: 10.596832320931542 ... ``` -## Load a custom sensor - -Let's try loading the sensor from the `custom_sensor` example. -Make sure you build that example first, following the instructions on -its README. - -Then we need to tell Ignition Sensors where to find the custom sensor -by setting the `IGN_SENSORS_PATH` environment variable to the path where -`ignition-sensors5-custom_sensor` is located: - -``` -cd examples/loop_sensor/build -export IGN_SENSORS_PATH=../..//custom_sensor/build -``` - -Then we can run the `custom_sensor.sdf` file provided with that sensor: +Then listen to the custom sensor: ``` -cd examples/loop_sensor/build -./loop_sensor ../../custom_sensor/custom_sensor.sdf -``` - -On another terminal, check that the sensor is generating data on a topic: - -``` -ign topic -l -``` - -You should see: - -``` -/custom_data -``` - -Then listen to the data: - -``` -ign topic -e -t /custom_data +ign topic -e -t /double ``` You'll see data like: @@ -131,33 +88,18 @@ You'll see data like: ... header { stamp { - sec: 12 - } - data { - key: "frame_id" - value: "my_sensor" - } - data { - key: "seq" - value: "12" - } -} -data: 12.799322041404857 - -header { - stamp { - sec: 13 + sec: 14 } data { key: "frame_id" - value: "my_sensor" + value: "custom_double" } data { key: "seq" - value: "13" + value: "14" } } -data: 13.783310442250663 +data: 3.1325939574820185 ... ``` diff --git a/examples/loop_sensor/altimeter_example.sdf b/examples/loop_sensor/altimeter_example.sdf deleted file mode 100644 index 61d6f935..00000000 --- a/examples/loop_sensor/altimeter_example.sdf +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - 1 - 30 - true - altimeter - - - - 0.1 - 0.2 - - - - - 0.2 - 0.1 - - - - - - - diff --git a/examples/loop_sensor/main.cc b/examples/loop_sensor/main.cc index 0354e0ab..2129c9e3 100644 --- a/examples/loop_sensor/main.cc +++ b/examples/loop_sensor/main.cc @@ -15,16 +15,18 @@ * */ -#include -#include +#include #include #include #include - #include +// Include all supported sensors +#include +#include "../custom_sensor/DoubleSensor.hh" + using namespace std::literals::chrono_literals; int main(int argc, char **argv) @@ -39,43 +41,71 @@ int main(int argc, char **argv) // Load file std::string sdfFile(argv[1]); - std::ifstream fileStream(sdfFile, std::ifstream::in); - std::string sdfString((std::istreambuf_iterator(fileStream)), - std::istreambuf_iterator()); - fileStream.close(); + sdf::Root root; + auto errors = root.Load(sdfFile); - // Read SDF - sdf::SDFPtr sdfParsed(new sdf::SDF()); - sdf::init(sdfParsed); - if (!sdf::readString(sdfString, sdfParsed)) + for (const auto &error : errors) { - ignerr << "Failed to parse SDF: " << std::endl << sdfString << std::endl; - return 1; + ignerr << error << std::endl; } - if (!sdfParsed || - !sdfParsed->Root() || - !sdfParsed->Root()->GetElement("model") || - !sdfParsed->Root()->GetElement("model")->GetElement("link") || - !sdfParsed->Root()->GetElement("model")->GetElement("link")-> - GetElement("sensor")) + auto world = root.WorldByIndex(0); + if (nullptr == world) { - ignerr << "Failed to find sensor in SDF" << std::endl; + ignerr << "Failed to load world from [" << sdfFile << "]" << std::endl; return 1; } - auto sdfElem = sdfParsed->Root()->GetElement("model")->GetElement("link") - ->GetElement("sensor"); - - // Create sensor - // TODO(louise) Create from sdf::Sensor + // Initiate sensor manager ignition::sensors::Manager mgr; - auto sensor = mgr.CreateSensor(sdfElem); - if (!sensor) + // Loop thorough SDF file and add all supported sensors to manager + std::vector sensors; + for (auto m = 0; m < world->ModelCount(); ++m) { - ignerr << "Unable to load sensor" << std::endl;; + auto model = world->ModelByIndex(m); + for (auto l = 0; l < model->LinkCount(); ++l) + { + auto link = model->LinkByIndex(l); + for (auto s = 0; s < link->SensorCount(); ++s) + { + auto sensor = link->SensorByIndex(s); + + ignition::sensors::Sensor *sensorPtr; + if (sensor->Type() == sdf::SensorType::ALTIMETER) + { + sensorPtr = mgr.CreateSensor( + *sensor); + } + else if (sensor->Type() == sdf::SensorType::CUSTOM) + { + sensorPtr = mgr.CreateSensor(*sensor); + } + else + { + ignerr << "Sensor type [" << static_cast(sensor->Type()) + << "] not supported." << std::endl; + } + + if (nullptr == sensorPtr) + { + ignerr << "Failed to create sensor [" << sensor->Name() << "]" + << std::endl; + continue; + } + + sensors.push_back(sensorPtr); + + ignmsg << "Added sensor [" << sensor->Name() << "] to manager." + << std::endl; + } + } + } + + if (sensors.empty()) + { + ignerr << "No sensors have been added to the manager." << std::endl; return 1; } @@ -87,9 +117,27 @@ int main(int argc, char **argv) signaled = true; }); + ignmsg << "Looping sensor manager. Press Ctrl + C to stop." << std::endl; + auto time = 0s; while (!signaled) { + // Update each sensor using their specific APIs + for (const auto &sensorPtr : sensors) + { + if (auto altimeter = dynamic_cast( + sensorPtr)) + { + altimeter->SetVerticalVelocity(altimeter->VerticalVelocity() + 0.1); + } + else if (auto custom = dynamic_cast(sensorPtr)) + { + custom->data += 0.1; + } + } + + // Call all sensor's own update functions, which will apply noise, publish + // data, etc. mgr.RunOnce(time, true); time += 1s; std::this_thread::sleep_for(1s); diff --git a/examples/loop_sensor/sensors.sdf b/examples/loop_sensor/sensors.sdf new file mode 100644 index 00000000..bb916b82 --- /dev/null +++ b/examples/loop_sensor/sensors.sdf @@ -0,0 +1,39 @@ + + + + + + + 1 + 30 + altimeter + + + + 0.1 + 0.2 + + + + + 0.2 + 0.1 + + + + + + 1 + 30 + double + + + 0.1 + 0.2 + + + + + + + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index d076fdd7..be233ff9 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -142,18 +142,3 @@ ign_build_tests(TYPE UNIT SOURCES Lidar_TEST.cc LIB_DEPS ${lidar_target}) ign_build_tests(TYPE UNIT SOURCES Camera_TEST.cc LIB_DEPS ${camera_target}) ign_build_tests(TYPE UNIT SOURCES ImuSensor_TEST.cc LIB_DEPS ${imu_target}) -#set(plugin_test_targets) -#list(APPEND plugin_test_targets "UNIT_Camera_TEST") -#list(APPEND plugin_test_targets "UNIT_Lidar_TEST") -#list(APPEND plugin_test_targets "UNIT_ImuSensor_TEST") -# -#foreach(plugin_test ${plugin_test_targets}) -# if(TARGET ${plugin_test}) -# set(_env_vars) -# list(APPEND _env_vars "IGN_PLUGIN_PATH=$") -# list(APPEND _env_vars "DYLD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") -# -# set_tests_properties(${plugin_test} PROPERTIES -# ENVIRONMENT "${_env_vars}") -# endif() -#endforeach() diff --git a/src/Manager_TEST.cc b/src/Manager_TEST.cc index 3f67843d..d0b3d294 100644 --- a/src/Manager_TEST.cc +++ b/src/Manager_TEST.cc @@ -39,7 +39,7 @@ class DummySensor : public ignition::sensors::Sensor }; ////////////////////////////////////////////////// -TEST_F(Manager_TEST, construct) +TEST_F(Manager_TEST, Construct) { ignition::sensors::Manager mgr; EXPECT_TRUE(mgr.Init()); @@ -55,14 +55,44 @@ TEST_F(Manager_TEST, construct) } ////////////////////////////////////////////////// -TEST_F(Manager_TEST, removeSensor) +TEST_F(Manager_TEST, AddSensor) +{ + ignition::sensors::Manager mgr; + + // Fail + std::unique_ptr dummyNull{nullptr}; + EXPECT_EQ(ignition::sensors::NO_SENSOR, mgr.AddSensor(std::move(dummyNull))); + + // Succeed + sdf::Sensor sdfSensor; + sdfSensor.SetTopic("/some/topic"); + sdfSensor.SetType(sdf::SensorType::CUSTOM); + + auto dummyGood = std::make_unique(); + EXPECT_TRUE(dummyGood->Load(sdfSensor)); + EXPECT_TRUE(dummyGood->Init()); + EXPECT_NE(ignition::sensors::NO_SENSOR, mgr.AddSensor(std::move(dummyGood))); +} + +////////////////////////////////////////////////// +TEST_F(Manager_TEST, RemoveSensor) { ignition::sensors::Manager mgr; EXPECT_TRUE(mgr.Init()); + // Fail EXPECT_FALSE(mgr.Remove(ignition::sensors::NO_SENSOR)); - // \todo(nkoenig) Add a sensor, then remove it + // Succeed + sdf::Sensor sdfSensor; + sdfSensor.SetTopic("/some/topic"); + sdfSensor.SetType(sdf::SensorType::CUSTOM); + + auto createdSensor = mgr.CreateSensor(sdfSensor); + ASSERT_NE(nullptr, createdSensor); + EXPECT_NE(nullptr, mgr.Sensor(createdSensor->Id())); + + EXPECT_TRUE(mgr.Remove(createdSensor->Id())); } ////////////////////////////////////////////////// diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index e4a9d7c4..51802cd5 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -53,15 +53,3 @@ ign_build_tests(TYPE INTEGRATION ${PROJECT_LIBRARY_TARGET_NAME}-imu ) -#foreach(plugin_test ${dri_tests} ${tests}) -# get_filename_component(BINARY_NAME ${plugin_test} NAME_WE) -# set(BINARY_NAME "${TEST_TYPE}_${BINARY_NAME}") -# if(TARGET ${BINARY_NAME}) -# set(_env_vars) -# list(APPEND _env_vars "IGN_PLUGIN_PATH=$") -# list(APPEND _env_vars "DYLD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") -# -# set_tests_properties(${BINARY_NAME} PROPERTIES -# ENVIRONMENT "${_env_vars}") -# endif() -#endforeach() From 01855a2bf564771e81712d0cfa7045984dc2217e Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 5 Aug 2021 15:37:48 -0700 Subject: [PATCH 13/18] Helper function to get custom sensor type Signed-off-by: Louise Poubel --- examples/custom_sensor/CMakeLists.txt | 2 +- examples/custom_sensor/DoubleSensor.cc | 32 ++---- examples/loop_sensor/CMakeLists.txt | 2 +- include/ignition/sensors/Util.hh | 66 +++++++++++++ src/CMakeLists.txt | 2 + src/Util.cc | 57 +++++++++++ src/Util_TEST.cc | 99 +++++++++++++++++++ test/integration/camera_plugin.cc | 2 +- test/integration/depth_camera_plugin.cc | 2 +- test/integration/rgbd_camera_plugin.cc | 2 +- test/integration/thermal_camera_plugin.cc | 4 +- .../camera_sensor_builtin.sdf | 0 test/sdf/custom_sensors.sdf | 18 ++++ .../depth_camera_sensor_builtin.sdf | 0 .../rgbd_camera_sensor_builtin.sdf | 0 .../thermal_camera_sensor_8bit_builtin.sdf | 0 .../thermal_camera_sensor_builtin.sdf | 0 17 files changed, 255 insertions(+), 33 deletions(-) create mode 100644 include/ignition/sensors/Util.hh create mode 100644 src/Util.cc create mode 100644 src/Util_TEST.cc rename test/{integration => sdf}/camera_sensor_builtin.sdf (100%) create mode 100644 test/sdf/custom_sensors.sdf rename test/{integration => sdf}/depth_camera_sensor_builtin.sdf (100%) rename test/{integration => sdf}/rgbd_camera_sensor_builtin.sdf (100%) rename test/{integration => sdf}/thermal_camera_sensor_8bit_builtin.sdf (100%) rename test/{integration => sdf}/thermal_camera_sensor_builtin.sdf (100%) diff --git a/examples/custom_sensor/CMakeLists.txt b/examples/custom_sensor/CMakeLists.txt index 8df1bcaa..b2a80a69 100644 --- a/examples/custom_sensor/CMakeLists.txt +++ b/examples/custom_sensor/CMakeLists.txt @@ -7,4 +7,4 @@ find_package(ignition-sensors6 REQUIRED) add_library(${PROJECT_NAME} SHARED DoubleSensor.cc) target_link_libraries(${PROJECT_NAME} - PRIVATE ignition-sensors6::ignition-sensors6) + PUBLIC ignition-sensors6::ignition-sensors6) diff --git a/examples/custom_sensor/DoubleSensor.cc b/examples/custom_sensor/DoubleSensor.cc index f1dba418..2f725a74 100644 --- a/examples/custom_sensor/DoubleSensor.cc +++ b/examples/custom_sensor/DoubleSensor.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include "DoubleSensor.hh" @@ -28,32 +29,11 @@ using namespace custom; ////////////////////////////////////////////////// bool DoubleSensor::Load(const sdf::Sensor &_sdf) { - if (_sdf.Type() != sdf::SensorType::CUSTOM) + auto type = ignition::sensors::customType(_sdf); + if ("double" != type) { ignerr << "Trying to load custom sensor, but got type [" - << static_cast(_sdf.Type()) << "] instead." << std::endl; - return false; - } - - auto elem = _sdf.Element(); - if (nullptr == elem) - { - ignerr << "SDF missing element pointer." << std::endl; - return false; - } - - if (!elem->HasAttribute("ignition:type")) - { - ignerr << "Custom sensor missing `ignition::type` attribute. " - << "Not loading." << std::endl; - return false; - } - - auto customType = elem->Get("ignition:type"); - if (customType != "double") - { - ignerr << "Trying to load sensor of type [double], but got sensor of type [" - << customType << "]." << std::endl; + << type << "] instead." << std::endl; return false; } @@ -63,14 +43,14 @@ bool DoubleSensor::Load(const sdf::Sensor &_sdf) // Advertise topic where data will be published this->pub = this->node.Advertise(this->Topic()); - if (!elem->HasElement("ignition:double")) + if (!_sdf.Element()->HasElement("ignition:double")) { igndbg << "No custom configuration for [" << this->Topic() << "]" << std::endl; return true; } - auto customElem = elem->GetElement("ignition:double"); + auto customElem = _sdf.Element()->GetElement("ignition:double"); if (!customElem->HasElement("noise")) { diff --git a/examples/loop_sensor/CMakeLists.txt b/examples/loop_sensor/CMakeLists.txt index d3f898f3..3b560096 100644 --- a/examples/loop_sensor/CMakeLists.txt +++ b/examples/loop_sensor/CMakeLists.txt @@ -2,8 +2,8 @@ cmake_minimum_required(VERSION 3.5 FATAL_ERROR) project(loop_sensor) find_package(ignition-sensors6 REQUIRED + # Find built-in sensors COMPONENTS - # Find built-in sensors altimeter ) diff --git a/include/ignition/sensors/Util.hh b/include/ignition/sensors/Util.hh new file mode 100644 index 00000000..7189cb2a --- /dev/null +++ b/include/ignition/sensors/Util.hh @@ -0,0 +1,66 @@ +/* + * 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 "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); + +/// \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); +} +} +} + +#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index be233ff9..1eb3bb4a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -6,6 +6,7 @@ set (sources PointCloudUtil.cc SensorFactory.cc SensorTypes.cc + Util.cc ) set(rendering_sources @@ -19,6 +20,7 @@ set (gtest_sources Manager_TEST.cc Noise_TEST.cc Sensor_TEST.cc + Util_TEST.cc ) if (MSVC) diff --git a/src/Util.cc b/src/Util.cc new file mode 100644 index 00000000..39b0e0b5 --- /dev/null +++ b/src/Util.cc @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include "ignition/sensors/Util.hh" + +////////////////////////////////////////////////// +std::string ignition::sensors::customType(const sdf::Sensor &_sdf) +{ + if (nullptr == _sdf.Element()) + return std::string(); + + return customType(_sdf.Element()); +} + +////////////////////////////////////////////////// +std::string ignition::sensors::customType(sdf::ElementPtr _sdf) +{ + if (_sdf == nullptr) + return std::string(); + + if (!_sdf->HasAttribute("type")) + { + ignerr << "Sensor missing `type` attribute." << std::endl; + return std::string(); + } + auto type = _sdf->Get("type"); + if ("custom" != type) + { + ignerr << "Sensor `type` is not [custom]; it's [" << type << "]." + << std::endl; + return std::string(); + } + + if (!_sdf->HasAttribute("ignition:type")) + { + ignerr << "Custom sensor missing `ignition:type` attribute." << std::endl; + return std::string(); + } + + return _sdf->Get("ignition:type"); +} diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc new file mode 100644 index 00000000..3381ded4 --- /dev/null +++ b/src/Util_TEST.cc @@ -0,0 +1,99 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include +#include +#include +#include +#include + +#include "test_config.h" // NOLINT(build/include) +#include "ignition/sensors/Util.hh" + +/// \brief Test Util functions +class Util_TEST : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + } +}; + +using namespace ignition; +using namespace sensors; + +////////////////////////////////////////////////// +TEST_F(Util_TEST, customType) +{ + auto sdfFile = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "custom_sensors.sdf"); + + sdf::Root root; + auto errors = root.Load(sdfFile); + EXPECT_TRUE(errors.empty()); + + EXPECT_EQ(1u, root.WorldCount()); + auto world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + EXPECT_EQ(1u, world->ModelCount()); + auto model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + EXPECT_EQ(1u, model->LinkCount()); + auto link = model->LinkByIndex(0); + ASSERT_NE(nullptr, link); + + EXPECT_EQ(3u, link->SensorCount()); + + { + auto sensor = link->SensorByIndex(0); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ("complete", sensor->Name()); + EXPECT_EQ("sensor_type", customType(*sensor)); + EXPECT_EQ("sensor_type", customType(sensor->Element())); + } + + { + auto sensor = link->SensorByIndex(1); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ("wrong_type", sensor->Name()); + EXPECT_TRUE(customType(*sensor).empty()); + EXPECT_TRUE(customType(sensor->Element()).empty()); + } + + { + auto sensor = link->SensorByIndex(2); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ("missing_ignition_type", sensor->Name()); + EXPECT_TRUE(customType(*sensor).empty()); + EXPECT_TRUE(customType(sensor->Element()).empty()); + } + + sdf::ElementPtr ptr{nullptr}; + EXPECT_TRUE(customType(ptr).empty()); +} + +////////////////////////////////////////////////// +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/integration/camera_plugin.cc b/test/integration/camera_plugin.cc index e2983c0e..a8bc14bc 100644 --- a/test/integration/camera_plugin.cc +++ b/test/integration/camera_plugin.cc @@ -65,7 +65,7 @@ void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) { // get the darn test data std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", - "integration", "camera_sensor_builtin.sdf"); + "sdf", "camera_sensor_builtin.sdf"); sdf::SDFPtr doc(new sdf::SDF()); sdf::init(doc); ASSERT_TRUE(sdf::readFile(path, doc)); diff --git a/test/integration/depth_camera_plugin.cc b/test/integration/depth_camera_plugin.cc index 991418fa..19e0377d 100644 --- a/test/integration/depth_camera_plugin.cc +++ b/test/integration/depth_camera_plugin.cc @@ -156,7 +156,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( { // get the darn test data std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", - "integration", "depth_camera_sensor_builtin.sdf"); + "sdf", "depth_camera_sensor_builtin.sdf"); sdf::SDFPtr doc(new sdf::SDF()); sdf::init(doc); ASSERT_TRUE(sdf::readFile(path, doc)); diff --git a/test/integration/rgbd_camera_plugin.cc b/test/integration/rgbd_camera_plugin.cc index 2094e449..c7432ec8 100644 --- a/test/integration/rgbd_camera_plugin.cc +++ b/test/integration/rgbd_camera_plugin.cc @@ -175,7 +175,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( { // get the darn test data std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", - "integration", "rgbd_camera_sensor_builtin.sdf"); + "sdf", "rgbd_camera_sensor_builtin.sdf"); sdf::SDFPtr doc(new sdf::SDF()); sdf::init(doc); ASSERT_TRUE(sdf::readFile(path, doc)); diff --git a/test/integration/thermal_camera_plugin.cc b/test/integration/thermal_camera_plugin.cc index 59c0843b..f7bf9298 100644 --- a/test/integration/thermal_camera_plugin.cc +++ b/test/integration/thermal_camera_plugin.cc @@ -108,7 +108,7 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( { // get the darn test data std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", - "integration", "thermal_camera_sensor_builtin.sdf"); + "sdf", "thermal_camera_sensor_builtin.sdf"); sdf::SDFPtr doc(new sdf::SDF()); sdf::init(doc); ASSERT_TRUE(sdf::readFile(path, doc)); @@ -366,7 +366,7 @@ void ThermalCameraSensorTest::Images8BitWithBuiltinSDF( { // get the darn test data std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", - "integration", "thermal_camera_sensor_8bit_builtin.sdf"); + "sdf", "thermal_camera_sensor_8bit_builtin.sdf"); sdf::SDFPtr doc(new sdf::SDF()); sdf::init(doc); ASSERT_TRUE(sdf::readFile(path, doc)); diff --git a/test/integration/camera_sensor_builtin.sdf b/test/sdf/camera_sensor_builtin.sdf similarity index 100% rename from test/integration/camera_sensor_builtin.sdf rename to test/sdf/camera_sensor_builtin.sdf diff --git a/test/sdf/custom_sensors.sdf b/test/sdf/custom_sensors.sdf new file mode 100644 index 00000000..0ed0c731 --- /dev/null +++ b/test/sdf/custom_sensors.sdf @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/test/integration/depth_camera_sensor_builtin.sdf b/test/sdf/depth_camera_sensor_builtin.sdf similarity index 100% rename from test/integration/depth_camera_sensor_builtin.sdf rename to test/sdf/depth_camera_sensor_builtin.sdf diff --git a/test/integration/rgbd_camera_sensor_builtin.sdf b/test/sdf/rgbd_camera_sensor_builtin.sdf similarity index 100% rename from test/integration/rgbd_camera_sensor_builtin.sdf rename to test/sdf/rgbd_camera_sensor_builtin.sdf diff --git a/test/integration/thermal_camera_sensor_8bit_builtin.sdf b/test/sdf/thermal_camera_sensor_8bit_builtin.sdf similarity index 100% rename from test/integration/thermal_camera_sensor_8bit_builtin.sdf rename to test/sdf/thermal_camera_sensor_8bit_builtin.sdf diff --git a/test/integration/thermal_camera_sensor_builtin.sdf b/test/sdf/thermal_camera_sensor_builtin.sdf similarity index 100% rename from test/integration/thermal_camera_sensor_builtin.sdf rename to test/sdf/thermal_camera_sensor_builtin.sdf From c3c560eb2d5a333ce673c81dd735bf65ab913038 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 5 Aug 2021 19:27:21 -0700 Subject: [PATCH 14/18] Rename tests, add tutorial Signed-off-by: Louise Poubel --- examples/custom_sensor/DoubleSensor.cc | 2 +- examples/custom_sensor/DoubleSensor.hh | 1 - test/integration/CMakeLists.txt | 20 ++-- ...air_pressure_plugin.cc => air_pressure.cc} | 0 .../{altimeter_plugin.cc => altimeter.cc} | 0 .../{camera_plugin.cc => camera.cc} | 0 ...depth_camera_plugin.cc => depth_camera.cc} | 0 ...r_sensor_plugin.cc => gpu_lidar_sensor.cc} | 0 test/integration/{imu_plugin.cc => imu.cc} | 0 ...cal_camera_plugin.cc => logical_camera.cc} | 0 ...magnetometer_plugin.cc => magnetometer.cc} | 0 .../{rgbd_camera_plugin.cc => rgbd_camera.cc} | 0 ...mal_camera_plugin.cc => thermal_camera.cc} | 0 tutorials.md.in | 1 + tutorials/custom_sensors.md | 109 ++++++++++++++++++ 15 files changed, 121 insertions(+), 12 deletions(-) rename test/integration/{air_pressure_plugin.cc => air_pressure.cc} (100%) rename test/integration/{altimeter_plugin.cc => altimeter.cc} (100%) rename test/integration/{camera_plugin.cc => camera.cc} (100%) rename test/integration/{depth_camera_plugin.cc => depth_camera.cc} (100%) rename test/integration/{gpu_lidar_sensor_plugin.cc => gpu_lidar_sensor.cc} (100%) rename test/integration/{imu_plugin.cc => imu.cc} (100%) rename test/integration/{logical_camera_plugin.cc => logical_camera.cc} (100%) rename test/integration/{magnetometer_plugin.cc => magnetometer.cc} (100%) rename test/integration/{rgbd_camera_plugin.cc => rgbd_camera.cc} (100%) rename test/integration/{thermal_camera_plugin.cc => thermal_camera.cc} (100%) create mode 100644 tutorials/custom_sensors.md diff --git a/examples/custom_sensor/DoubleSensor.cc b/examples/custom_sensor/DoubleSensor.cc index 2f725a74..46d9fcef 100644 --- a/examples/custom_sensor/DoubleSensor.cc +++ b/examples/custom_sensor/DoubleSensor.cc @@ -19,7 +19,6 @@ #include #include -#include #include #include "DoubleSensor.hh" @@ -50,6 +49,7 @@ bool DoubleSensor::Load(const sdf::Sensor &_sdf) return true; } + // Load custom sensor params auto customElem = _sdf.Element()->GetElement("ignition:double"); if (!customElem->HasElement("noise")) diff --git a/examples/custom_sensor/DoubleSensor.hh b/examples/custom_sensor/DoubleSensor.hh index 3406afd7..e8d08d83 100644 --- a/examples/custom_sensor/DoubleSensor.hh +++ b/examples/custom_sensor/DoubleSensor.hh @@ -18,7 +18,6 @@ #define DOUBLESENSOR_HH_ #include -#include #include namespace custom diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 51802cd5..39ee1ef6 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -1,19 +1,19 @@ set(TEST_TYPE "INTEGRATION") set(dri_tests - camera_plugin.cc - depth_camera_plugin.cc - gpu_lidar_sensor_plugin.cc - rgbd_camera_plugin.cc - thermal_camera_plugin.cc + camera.cc + depth_camera.cc + gpu_lidar_sensor.cc + rgbd_camera.cc + thermal_camera.cc ) set(tests - air_pressure_plugin.cc - altimeter_plugin.cc - logical_camera_plugin.cc - magnetometer_plugin.cc - imu_plugin.cc + air_pressure.cc + altimeter.cc + logical_camera.cc + magnetometer.cc + imu.cc ) link_directories(${PROJECT_BINARY_DIR}/test) diff --git a/test/integration/air_pressure_plugin.cc b/test/integration/air_pressure.cc similarity index 100% rename from test/integration/air_pressure_plugin.cc rename to test/integration/air_pressure.cc diff --git a/test/integration/altimeter_plugin.cc b/test/integration/altimeter.cc similarity index 100% rename from test/integration/altimeter_plugin.cc rename to test/integration/altimeter.cc diff --git a/test/integration/camera_plugin.cc b/test/integration/camera.cc similarity index 100% rename from test/integration/camera_plugin.cc rename to test/integration/camera.cc diff --git a/test/integration/depth_camera_plugin.cc b/test/integration/depth_camera.cc similarity index 100% rename from test/integration/depth_camera_plugin.cc rename to test/integration/depth_camera.cc diff --git a/test/integration/gpu_lidar_sensor_plugin.cc b/test/integration/gpu_lidar_sensor.cc similarity index 100% rename from test/integration/gpu_lidar_sensor_plugin.cc rename to test/integration/gpu_lidar_sensor.cc diff --git a/test/integration/imu_plugin.cc b/test/integration/imu.cc similarity index 100% rename from test/integration/imu_plugin.cc rename to test/integration/imu.cc diff --git a/test/integration/logical_camera_plugin.cc b/test/integration/logical_camera.cc similarity index 100% rename from test/integration/logical_camera_plugin.cc rename to test/integration/logical_camera.cc diff --git a/test/integration/magnetometer_plugin.cc b/test/integration/magnetometer.cc similarity index 100% rename from test/integration/magnetometer_plugin.cc rename to test/integration/magnetometer.cc diff --git a/test/integration/rgbd_camera_plugin.cc b/test/integration/rgbd_camera.cc similarity index 100% rename from test/integration/rgbd_camera_plugin.cc rename to test/integration/rgbd_camera.cc diff --git a/test/integration/thermal_camera_plugin.cc b/test/integration/thermal_camera.cc similarity index 100% rename from test/integration/thermal_camera_plugin.cc rename to test/integration/thermal_camera.cc diff --git a/tutorials.md.in b/tutorials.md.in index 7b961b9f..a9e4e9be 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -8,6 +8,7 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. 1. \subpage introduction "Introduction" 2. \subpage installation "Installation" +2. \subpage custom_sensors "Custom sensors": Creating your own sensors 3. \subpage thermalcameraigngazebo "Thermal Camera in Ignition Gazebo": Using a thermal camera in Ignition Gazebo to detect objects of specific temperatures in camera images. ## License diff --git a/tutorials/custom_sensors.md b/tutorials/custom_sensors.md new file mode 100644 index 00000000..1b9eade9 --- /dev/null +++ b/tutorials/custom_sensors.md @@ -0,0 +1,109 @@ +\page custom_sensors Custom sensors + +Ignition Sensors comes with various built-in sensor types ready to be used. +Users aren't limited to those sensor types though. This tutorial will go over +the process of implementing a custom sensor that leverages Ignition Sensors +and can be used in downstream applications such as Ignition Gazebo. + +## Why custom sensors + +The set of built-in sensors provided by Ignition Sensors is tied to the +SDFormat specification. SDFormat is meant to be as general-purpose as possible, +so adding lots of exotic sensors to the specification may become a maintenance +burden and counter-productive. Therefore, sensors that are very specific to +certain applications are better implemented closer to the applications that +need them, than added to the general-purpose specification. + +It may also be interesting for downstream projects to implement very specific +versions of sensors which are not generally useful for other projects. + +Finally, another reason to develop custom sensors is for projects that need to +implement proprietary sensors that can't be shared publicly. + +## Do I need to use Ignition Sensors? + +A simulated sensor is some code that can be used to extract specific data from +a simulation. A camera extracts pixels, an IMU extracts accelerations, etc. On +Ignition Gazebo, for example, it's possible to get information using a +system plugin without any connection to Ignition Sensors, for example. + +With that in mind, it's worth going over some of the features that Ignition +Sensors provides that make it easier to implement sensors, instead of writing +everything from scratch: + +* Standard handling of common parameters, like topic and update rate; +* Common APIs for updating the sensors in a loop; +* A standard way for loading custom sensors from an SDF `` tag; +* Efficient updates for various rendering sensors using the same scene. + +## Let's get started! + +Let's go over the creation of a custom +[odometer](https://en.wikipedia.org/wiki/Odometer) sensor which will measure +the total distance travelled by a robot. + +This example is located at +[ign-sensors/examples/custom_sensor](https://github.com/ignitionrobotics/ign-sensors/tree/main/examples/custom_sensor), +and the integration with Ignition Gazebo is at +[ign-gazebo/examples/plugins/custom_sensor_system](https://github.com/ignitionrobotics/ign-gazebo/tree/main/examples/plugin/custom_sensor_system). + +### SDF + +Custom sensors follow these rules to be loaded from SDFormat: + +* Use `type="custom"` inside the `` tag +* Add an extra `ignition:type="odometer"` attribute, where `odometer` + is a string that uniquely identifies the sensor type. +* Optionally, add an `` element with configuration + specific to that sensor. + +With that in mind, here's what the sensor tag would look like: + +``` + + 1 + 30 + odom + + + 0.2 + 0.1 + + + +``` + +### Sensor implementation + +The sensor consists of a class, `Odometer`, which inherits from +`ignition::sensors::Sensor`. + +Take a look at +[ign-sensors/examples/custom_sensor](https://github.com/ignitionrobotics/ign-sensors/tree/main/examples/custom_sensor) +for the full code. Here are some important pointers: + +* Be sure to link your sensor to Ignition Sensors +* Implement all pure abstraction functions, like `Update` +* Be sure to implement at least one of the `Load` functions so the sensor can be initialized +* Don't forget to call `Sensor::Load` to load common sensor properties +* It's recommended that the sensor publishes its data using Ignition Transport + +You may choose to compile your sensor as a shared library, or to embed it +directly into other application code. + +### Loading into Ignition Gazebo + +In order to use the sensor on Ignition Gazebo, one needs to write a system +plugin that instantiates the sensor and updates it periodically with data from +simulation. + +Take a look at +[ign-gazebo/examples/plugins/custom_sensor_system](https://github.com/ignitionrobotics/ign-gazebo/tree/main/examples/plugin/custom_sensor_system). +for the full code. Here are some important pointers: + +* Check for new entities that have `ignition::gazebo::components::CustomSensor` + during the `PreUpdate` callback and instantiate new sensors as they appear + in simulation. +* During `PostUpdate`, update all sensors with new data coming from simulation. +* Also during `PostUpdate`, delete any sensors that have been removed from + simulation. From 05ef96045ac2ea2a53bdd90d232f5e5663dce87e Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 6 Aug 2021 11:08:57 -0700 Subject: [PATCH 15/18] odometer Signed-off-by: Louise Poubel --- examples/custom_sensor/CMakeLists.txt | 4 +-- .../{DoubleSensor.cc => Odometer.cc} | 31 +++++++++++++------ .../{DoubleSensor.hh => Odometer.hh} | 26 ++++++++++++---- examples/custom_sensor/README.md | 12 +++---- examples/loop_sensor/CMakeLists.txt | 4 +-- examples/loop_sensor/README.md | 18 +++++------ examples/loop_sensor/main.cc | 8 ++--- examples/loop_sensor/sensors.sdf | 8 ++--- tutorials/custom_sensors.md | 18 ++++++++--- 9 files changed, 82 insertions(+), 47 deletions(-) rename examples/custom_sensor/{DoubleSensor.cc => Odometer.cc} (72%) rename examples/custom_sensor/{DoubleSensor.hh => Odometer.hh} (63%) diff --git a/examples/custom_sensor/CMakeLists.txt b/examples/custom_sensor/CMakeLists.txt index b2a80a69..aff5959d 100644 --- a/examples/custom_sensor/CMakeLists.txt +++ b/examples/custom_sensor/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) -project(double_sensor) +project(odometer) find_package(ignition-cmake2 REQUIRED) find_package(ignition-sensors6 REQUIRED) -add_library(${PROJECT_NAME} SHARED DoubleSensor.cc) +add_library(${PROJECT_NAME} SHARED Odometer.cc) target_link_libraries(${PROJECT_NAME} PUBLIC ignition-sensors6::ignition-sensors6) diff --git a/examples/custom_sensor/DoubleSensor.cc b/examples/custom_sensor/Odometer.cc similarity index 72% rename from examples/custom_sensor/DoubleSensor.cc rename to examples/custom_sensor/Odometer.cc index 46d9fcef..53db243e 100644 --- a/examples/custom_sensor/DoubleSensor.cc +++ b/examples/custom_sensor/Odometer.cc @@ -21,17 +21,17 @@ #include #include -#include "DoubleSensor.hh" +#include "Odometer.hh" using namespace custom; ////////////////////////////////////////////////// -bool DoubleSensor::Load(const sdf::Sensor &_sdf) +bool Odometer::Load(const sdf::Sensor &_sdf) { auto type = ignition::sensors::customType(_sdf); - if ("double" != type) + if ("odometer" != type) { - ignerr << "Trying to load custom sensor, but got type [" + ignerr << "Trying to load [odometer] sensor, but got type [" << type << "] instead." << std::endl; return false; } @@ -42,7 +42,7 @@ bool DoubleSensor::Load(const sdf::Sensor &_sdf) // Advertise topic where data will be published this->pub = this->node.Advertise(this->Topic()); - if (!_sdf.Element()->HasElement("ignition:double")) + if (!_sdf.Element()->HasElement("ignition:odometer")) { igndbg << "No custom configuration for [" << this->Topic() << "]" << std::endl; @@ -50,7 +50,7 @@ bool DoubleSensor::Load(const sdf::Sensor &_sdf) } // Load custom sensor params - auto customElem = _sdf.Element()->GetElement("ignition:double"); + auto customElem = _sdf.Element()->GetElement("ignition:odometer"); if (!customElem->HasElement("noise")) { @@ -71,7 +71,7 @@ bool DoubleSensor::Load(const sdf::Sensor &_sdf) } ////////////////////////////////////////////////// -bool DoubleSensor::Update(const std::chrono::steady_clock::duration &_now) +bool Odometer::Update(const std::chrono::steady_clock::duration &_now) { ignition::msgs::Double msg; *msg.mutable_header()->mutable_stamp() = ignition::msgs::Convert(_now); @@ -79,9 +79,9 @@ bool DoubleSensor::Update(const std::chrono::steady_clock::duration &_now) frame->set_key("frame_id"); frame->add_value(this->Name()); - this->data = this->noise->Apply(this->data); + this->totalDistance = this->noise->Apply(this->totalDistance); - msg.set_data(this->data); + msg.set_data(this->totalDistance); this->AddSequence(msg.mutable_header()); this->pub.Publish(msg); @@ -89,3 +89,16 @@ bool DoubleSensor::Update(const std::chrono::steady_clock::duration &_now) return true; } +////////////////////////////////////////////////// +void Odometer::NewPosition(const ignition::math::Vector3d &_pos) +{ + this->totalDistance += this->prevPos.Distance(_pos); + this->prevPos = _pos; +} + +////////////////////////////////////////////////// +const ignition::math::Vector3d &Odometer::Position() const +{ + return this->prevPos; +} + diff --git a/examples/custom_sensor/DoubleSensor.hh b/examples/custom_sensor/Odometer.hh similarity index 63% rename from examples/custom_sensor/DoubleSensor.hh rename to examples/custom_sensor/Odometer.hh index e8d08d83..05f0c809 100644 --- a/examples/custom_sensor/DoubleSensor.hh +++ b/examples/custom_sensor/Odometer.hh @@ -14,16 +14,18 @@ * limitations under the License. * */ -#ifndef DOUBLESENSOR_HH_ -#define DOUBLESENSOR_HH_ +#ifndef ODOMETER_HH_ +#define ODOMETER_HH_ #include +#include #include namespace custom { - /// \brief Example sensor that publishes a noisy double value. - class DoubleSensor : public ignition::sensors::Sensor + /// \brief Example sensor that publishes the total distance travelled by a + /// robot, with noise. + class Odometer : public ignition::sensors::Sensor { /// \brief Load the sensor with SDF parameters. /// \param[in] _sdf SDF Sensor parameters. @@ -36,8 +38,20 @@ namespace custom public: virtual bool Update( const std::chrono::steady_clock::duration &_now) override; - /// \brief Latest data - public: double data{0.0}; + /// \brief Set the current postiion of the robot, so the odometer can + /// calculate the distance travelled. + /// \param[in] _pos Current position in world coordinates. + public: void NewPosition(const ignition::math::Vector3d &_pos); + + /// \brief Get the latest world postiion of the robot. + /// \return The latest position given to the odometer. + public: const ignition::math::Vector3d &Position() const; + + /// \brief Previous position of the robot. + private: ignition::math::Vector3d prevPos; + + /// \brief Latest total distance. + private: double totalDistance{0.0}; /// \brief Noise that will be applied to the sensor data private: ignition::sensors::NoisePtr noise{nullptr}; diff --git a/examples/custom_sensor/README.md b/examples/custom_sensor/README.md index 8d410464..7540b6eb 100644 --- a/examples/custom_sensor/README.md +++ b/examples/custom_sensor/README.md @@ -1,7 +1,7 @@ # Custom sensor -This example creates a simple custom sensor called `DoubleSensor`, which -produces a single number as data and publishes it to a topic. +This example creates a simple custom sensor called `Odometer`, which +publishes the total distance travelled by a robot. ## Build @@ -15,13 +15,13 @@ cmake .. make ``` -This will generate a shared library with the sensor called `double_sensor`. +This will generate a shared library with the sensor called `libodometer`. ## Use -This sensor with Ignition Gazebo, or with any downstream application that uses -the Ignition Sensors API. Listed here are two ways of testing this sensor, one -with Gazebo and one with a custom program. +This sensor can be used with Ignition Gazebo, or with any downstream +application that uses the Ignition Sensors API. Listed here are two ways of +testing this sensor, one with Gazebo and one with a custom program. ### With a custom program diff --git a/examples/loop_sensor/CMakeLists.txt b/examples/loop_sensor/CMakeLists.txt index 3b560096..45d9fa77 100644 --- a/examples/loop_sensor/CMakeLists.txt +++ b/examples/loop_sensor/CMakeLists.txt @@ -8,14 +8,14 @@ find_package(ignition-sensors6 REQUIRED ) # A simple example of how to find custom sensors -add_subdirectory(../custom_sensor double_sensor) +add_subdirectory(../custom_sensor odometer) add_executable(${PROJECT_NAME} main.cc) target_link_libraries(${PROJECT_NAME} PUBLIC ignition-sensors6::ignition-sensors6 # Link to custom sensors - double_sensor + odometer # Link to built-in sensors ignition-sensors6::altimeter) diff --git a/examples/loop_sensor/README.md b/examples/loop_sensor/README.md index c8fa38f7..8ea50a7a 100644 --- a/examples/loop_sensor/README.md +++ b/examples/loop_sensor/README.md @@ -6,7 +6,7 @@ a loop until the user closes the program. The example works with 2 types of sensors: * A built-in type: altimeter -* A custom type: "double sensor" +* A custom type: odometer ## Build @@ -23,7 +23,7 @@ make This will generate an executable called `loop_sensor`. It will also compile the `custom_sensor` example, creating a library called -`libdouble_sensor`. +`libodometer`. ## Run @@ -34,7 +34,7 @@ cd examples/loop_sensor/build ./loop_sensor ../sensors.sdf ``` -On another terminal, check that the altimeter and double sensor are generating +On another terminal, check that the altimeter and odometer are generating data on a topic: ``` @@ -45,7 +45,7 @@ You should see: ``` /altimeter -/double +/odometer ``` Then listen to the altimeter data: @@ -79,7 +79,7 @@ vertical_velocity: 10.596832320931542 Then listen to the custom sensor: ``` -ign topic -e -t /double +ign topic -e -t /odometer ``` You'll see data like: @@ -88,18 +88,18 @@ You'll see data like: ... header { stamp { - sec: 14 + sec: 83 } data { key: "frame_id" - value: "custom_double" + value: "custom_odometer" } data { key: "seq" - value: "14" + value: "83" } } -data: 3.1325939574820185 +data: 21.632033902371308 ... ``` diff --git a/examples/loop_sensor/main.cc b/examples/loop_sensor/main.cc index 2129c9e3..7a366a3f 100644 --- a/examples/loop_sensor/main.cc +++ b/examples/loop_sensor/main.cc @@ -25,7 +25,7 @@ // Include all supported sensors #include -#include "../custom_sensor/DoubleSensor.hh" +#include "../custom_sensor/Odometer.hh" using namespace std::literals::chrono_literals; @@ -80,7 +80,7 @@ int main(int argc, char **argv) } else if (sensor->Type() == sdf::SensorType::CUSTOM) { - sensorPtr = mgr.CreateSensor(*sensor); + sensorPtr = mgr.CreateSensor(*sensor); } else { @@ -130,9 +130,9 @@ int main(int argc, char **argv) { altimeter->SetVerticalVelocity(altimeter->VerticalVelocity() + 0.1); } - else if (auto custom = dynamic_cast(sensorPtr)) + else if (auto odometer = dynamic_cast(sensorPtr)) { - custom->data += 0.1; + odometer->NewPosition(odometer->Position() + 0.1); } } diff --git a/examples/loop_sensor/sensors.sdf b/examples/loop_sensor/sensors.sdf index bb916b82..a52827f4 100644 --- a/examples/loop_sensor/sensors.sdf +++ b/examples/loop_sensor/sensors.sdf @@ -22,16 +22,16 @@ - + 1 30 - double - + odometer + 0.1 0.2 - + diff --git a/tutorials/custom_sensors.md b/tutorials/custom_sensors.md index 1b9eade9..675bcd88 100644 --- a/tutorials/custom_sensors.md +++ b/tutorials/custom_sensors.md @@ -3,7 +3,7 @@ Ignition Sensors comes with various built-in sensor types ready to be used. Users aren't limited to those sensor types though. This tutorial will go over the process of implementing a custom sensor that leverages Ignition Sensors -and can be used in downstream applications such as Ignition Gazebo. +and can be used with downstream applications such as Ignition Gazebo. ## Why custom sensors @@ -22,10 +22,12 @@ implement proprietary sensors that can't be shared publicly. ## Do I need to use Ignition Sensors? -A simulated sensor is some code that can be used to extract specific data from -a simulation. A camera extracts pixels, an IMU extracts accelerations, etc. On -Ignition Gazebo, for example, it's possible to get information using a -system plugin without any connection to Ignition Sensors, for example. +A simulated sensor consists of code that can be used to extract specific data +from a running simulation. A camera extracts pixels, an IMU extracts +accelerations, etc. This doesn't necessarily need to be done though Ignition +Sensors. On Ignition Gazebo, for example, it's implement functionality similar +to a sensor just using a system plugin without any connection to Ignition +Sensors. With that in mind, it's worth going over some of the features that Ignition Sensors provides that make it easier to implement sensors, instead of writing @@ -47,6 +49,9 @@ This example is located at and the integration with Ignition Gazebo is at [ign-gazebo/examples/plugins/custom_sensor_system](https://github.com/ignitionrobotics/ign-gazebo/tree/main/examples/plugin/custom_sensor_system). +Wherever "odometer" is used, that can be substituted for any other custom sensor +type. Seismometer? Breathalyzer? Just adapt the logic to your needs. + ### SDF Custom sensors follow these rules to be loaded from SDFormat: @@ -104,6 +109,9 @@ for the full code. Here are some important pointers: * Check for new entities that have `ignition::gazebo::components::CustomSensor` during the `PreUpdate` callback and instantiate new sensors as they appear in simulation. +* Don't assume all `CustomSensors` are of the type you need, be sure to check + the type using the `ignition::sensors::customType` function. * During `PostUpdate`, update all sensors with new data coming from simulation. * Also during `PostUpdate`, delete any sensors that have been removed from simulation. + From a533f70d0146f887e0cb289384c79839afde88a9 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 6 Aug 2021 13:49:12 -0700 Subject: [PATCH 16/18] cleanup Signed-off-by: Louise Poubel --- examples/custom_sensor/Odometer.cc | 7 ++++++- examples/custom_sensor/Odometer.hh | 3 ++- examples/loop_sensor/main.cc | 3 ++- include/ignition/sensors/SensorFactory.hh | 3 +++ 4 files changed, 13 insertions(+), 3 deletions(-) diff --git a/examples/custom_sensor/Odometer.cc b/examples/custom_sensor/Odometer.cc index 53db243e..326bae4f 100644 --- a/examples/custom_sensor/Odometer.cc +++ b/examples/custom_sensor/Odometer.cc @@ -15,6 +15,8 @@ * */ +#include + #include #include @@ -92,7 +94,10 @@ bool Odometer::Update(const std::chrono::steady_clock::duration &_now) ////////////////////////////////////////////////// void Odometer::NewPosition(const ignition::math::Vector3d &_pos) { - this->totalDistance += this->prevPos.Distance(_pos); + if (!isnan(this->prevPos.X())) + { + this->totalDistance += this->prevPos.Distance(_pos); + } this->prevPos = _pos; } diff --git a/examples/custom_sensor/Odometer.hh b/examples/custom_sensor/Odometer.hh index 05f0c809..17494fbe 100644 --- a/examples/custom_sensor/Odometer.hh +++ b/examples/custom_sensor/Odometer.hh @@ -48,7 +48,8 @@ namespace custom public: const ignition::math::Vector3d &Position() const; /// \brief Previous position of the robot. - private: ignition::math::Vector3d prevPos; + private: ignition::math::Vector3d prevPos{std::nan(""), std::nan(""), + std::nan("")}; /// \brief Latest total distance. private: double totalDistance{0.0}; diff --git a/examples/loop_sensor/main.cc b/examples/loop_sensor/main.cc index 7a366a3f..be80b415 100644 --- a/examples/loop_sensor/main.cc +++ b/examples/loop_sensor/main.cc @@ -132,7 +132,8 @@ int main(int argc, char **argv) } else if (auto odometer = dynamic_cast(sensorPtr)) { - odometer->NewPosition(odometer->Position() + 0.1); + odometer->NewPosition(odometer->Position() + + ignition::math::Vector3d(0.1, 0.1, 0.0)); } } diff --git a/include/ignition/sensors/SensorFactory.hh b/include/ignition/sensors/SensorFactory.hh index 225f9394..92f68cbb 100644 --- a/include/ignition/sensors/SensorFactory.hh +++ b/include/ignition/sensors/SensorFactory.hh @@ -66,6 +66,9 @@ namespace ignition /// \brief A factory class for creating sensors /// 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 From fdde936e1aadd7755117dbbf94bc777340d3a948 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 10 Aug 2021 10:44:49 -0700 Subject: [PATCH 17/18] PR feedback, windows deprecation test Signed-off-by: Louise Poubel --- include/ignition/sensors/SensorFactory.hh | 4 ++-- tutorials/custom_sensors.md | 24 +++++++++++------------ 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/include/ignition/sensors/SensorFactory.hh b/include/ignition/sensors/SensorFactory.hh index 92f68cbb..2bf45209 100644 --- a/include/ignition/sensors/SensorFactory.hh +++ b/include/ignition/sensors/SensorFactory.hh @@ -46,7 +46,7 @@ namespace ignition { /// \brief Instantiate new sensor /// \return New sensor - public: virtual Sensor * IGN_DEPRECATED(6) New() = 0; + public: virtual Sensor IGN_DEPRECATED(6) * New() = 0; }; /// \brief Templated class for instantiating sensors of the specified type @@ -57,7 +57,7 @@ namespace ignition class SensorTypePlugin : public SensorPlugin { // Documentation inherited - public: SensorType * IGN_DEPRECATED(6) New() override + public: SensorType IGN_DEPRECATED(6) * New() override { return new SensorType(); }; diff --git a/tutorials/custom_sensors.md b/tutorials/custom_sensors.md index 675bcd88..c6cd5ac4 100644 --- a/tutorials/custom_sensors.md +++ b/tutorials/custom_sensors.md @@ -64,18 +64,18 @@ Custom sensors follow these rules to be loaded from SDFormat: With that in mind, here's what the sensor tag would look like: -``` - - 1 - 30 - odom - - - 0.2 - 0.1 - - - +```xml + + 1 + 30 + odom + + + 0.2 + 0.1 + + + ``` ### Sensor implementation From 949f51976da5da166855693986c1528b84b6bc7f Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 10 Aug 2021 11:42:00 -0700 Subject: [PATCH 18/18] linters Signed-off-by: Louise Poubel --- include/ignition/sensors/Manager.hh | 1 + include/ignition/sensors/SensorFactory.hh | 4 ++-- include/ignition/sensors/Util.hh | 6 ++++-- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/include/ignition/sensors/Manager.hh b/include/ignition/sensors/Manager.hh index 195f56cb..543f7059 100644 --- a/include/ignition/sensors/Manager.hh +++ b/include/ignition/sensors/Manager.hh @@ -19,6 +19,7 @@ #include #include +#include #include #include #include diff --git a/include/ignition/sensors/SensorFactory.hh b/include/ignition/sensors/SensorFactory.hh index 2bf45209..b82af96c 100644 --- a/include/ignition/sensors/SensorFactory.hh +++ b/include/ignition/sensors/SensorFactory.hh @@ -122,8 +122,8 @@ namespace ignition { if (nullptr == _sdf) { - ignerr << "Failed to create sensor, received null SDF pointer." - << std::endl; + ignerr << "Failed to create sensor, received null SDF " + << "pointer." << std::endl; return nullptr; } diff --git a/include/ignition/sensors/Util.hh b/include/ignition/sensors/Util.hh index 7189cb2a..e9d2db43 100644 --- a/include/ignition/sensors/Util.hh +++ b/include/ignition/sensors/Util.hh @@ -17,6 +17,8 @@ #ifndef IGNITION_SENSORS_UTIL_HH_ #define IGNITION_SENSORS_UTIL_HH_ +#include + #include #include @@ -43,7 +45,7 @@ inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { /// /// \param[in] _sdf Sensor SDF object. /// \return _sensorType Name of sensor type. -std::string IGNITION_SENSORS_VISIBLE customType(const sdf::Sensor &_sdf); +std::string IGNITION_SENSORS_VISIBLE customType(const sdf::Sensor &_sdf); // NOLINT /// \brief Get the name of a sensor's custom type from SDF. /// @@ -58,7 +60,7 @@ std::string IGNITION_SENSORS_VISIBLE customType(const sdf::Sensor &_sdf); /// /// \param[in] _sdf Sensor SDF object. /// \return _sensorType Name of sensor type. -std::string IGNITION_SENSORS_VISIBLE customType(sdf::ElementPtr _sdf); +std::string IGNITION_SENSORS_VISIBLE customType(sdf::ElementPtr _sdf); // NOLINT } } }