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 1ca8c577..341e4c13 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,37 @@ 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 5.X to 6.X + +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. + + ***Deprecation*** IGN_SENSORS_REGISTER_SENSOR + + ***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 new file mode 100644 index 00000000..aff5959d --- /dev/null +++ b/examples/custom_sensor/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +project(odometer) + +find_package(ignition-cmake2 REQUIRED) +find_package(ignition-sensors6 REQUIRED) + +add_library(${PROJECT_NAME} SHARED Odometer.cc) +target_link_libraries(${PROJECT_NAME} + PUBLIC ignition-sensors6::ignition-sensors6) diff --git a/examples/custom_sensor/Odometer.cc b/examples/custom_sensor/Odometer.cc new file mode 100644 index 00000000..326bae4f --- /dev/null +++ b/examples/custom_sensor/Odometer.cc @@ -0,0 +1,109 @@ +/* + * 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 "Odometer.hh" + +using namespace custom; + +////////////////////////////////////////////////// +bool Odometer::Load(const sdf::Sensor &_sdf) +{ + auto type = ignition::sensors::customType(_sdf); + if ("odometer" != type) + { + ignerr << "Trying to load [odometer] sensor, but got type [" + << type << "] instead." << std::endl; + return false; + } + + // Load common sensor params + ignition::sensors::Sensor::Load(_sdf); + + // Advertise topic where data will be published + this->pub = this->node.Advertise(this->Topic()); + + if (!_sdf.Element()->HasElement("ignition:odometer")) + { + igndbg << "No custom configuration for [" << this->Topic() << "]" + << std::endl; + return true; + } + + // Load custom sensor params + auto customElem = _sdf.Element()->GetElement("ignition:odometer"); + + if (!customElem->HasElement("noise")) + { + igndbg << "No noise for [" << this->Topic() << "]" << std::endl; + return true; + } + + 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 Odometer::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->totalDistance = this->noise->Apply(this->totalDistance); + + msg.set_data(this->totalDistance); + + this->AddSequence(msg.mutable_header()); + this->pub.Publish(msg); + + return true; +} + +////////////////////////////////////////////////// +void Odometer::NewPosition(const ignition::math::Vector3d &_pos) +{ + if (!isnan(this->prevPos.X())) + { + 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/Odometer.hh b/examples/custom_sensor/Odometer.hh new file mode 100644 index 00000000..17494fbe --- /dev/null +++ b/examples/custom_sensor/Odometer.hh @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef ODOMETER_HH_ +#define ODOMETER_HH_ + +#include +#include +#include + +namespace custom +{ + /// \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. + /// \return True if loading was successful + public: virtual bool Load(const sdf::Sensor &_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 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{std::nan(""), std::nan(""), + std::nan("")}; + + /// \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}; + + /// \brief Node for communication + private: ignition::transport::Node node; + + /// \brief Publishes sensor data + private: ignition::transport::Node::Publisher pub; + }; +} + +#endif diff --git a/examples/custom_sensor/README.md b/examples/custom_sensor/README.md new file mode 100644 index 00000000..7540b6eb --- /dev/null +++ b/examples/custom_sensor/README.md @@ -0,0 +1,38 @@ +# Custom sensor + +This example creates a simple custom sensor called `Odometer`, which +publishes the total distance travelled by a robot. + +## 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 `libodometer`. + +## Use + +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 + +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. + diff --git a/examples/loop_sensor/CMakeLists.txt b/examples/loop_sensor/CMakeLists.txt new file mode 100644 index 00000000..45d9fa77 --- /dev/null +++ b/examples/loop_sensor/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.5 FATAL_ERROR) +project(loop_sensor) + +find_package(ignition-sensors6 REQUIRED + # Find built-in sensors + COMPONENTS + altimeter +) + +# A simple example of how to find custom sensors +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 + odometer + + # Link to built-in sensors + ignition-sensors6::altimeter) diff --git a/examples/loop_sensor/README.md b/examples/loop_sensor/README.md new file mode 100644 index 00000000..8ea50a7a --- /dev/null +++ b/examples/loop_sensor/README.md @@ -0,0 +1,105 @@ +# Loop sensor + +Example executable that loads sensors from an SDF world file and updates them in +a loop until the user closes the program. + +The example works with 2 types of sensors: + +* A built-in type: altimeter +* A custom type: odometer + +## Build + +Compile as follows: + +``` +cd examples/loop_sensor +mkdir build +cd build +cmake .. +make +``` + +This will generate an executable called `loop_sensor`. + +It will also compile the `custom_sensor` example, creating a library called +`libodometer`. + +## Run + +Try loading the accompanying `sensors.sdf`: + +``` +cd examples/loop_sensor/build +./loop_sensor ../sensors.sdf +``` + +On another terminal, check that the altimeter and odometer are generating +data on a topic: + +``` +ign topic -l +``` + +You should see: + +``` +/altimeter +/odometer +``` + +Then listen to the altimeter data: + +``` +ign topic -e -t /altimeter +``` + +You'll see data like: + +``` +... +header { + stamp { + sec: 35 + } + data { + key: "frame_id" + value: "altimeter" + } + data { + key: "seq" + value: "35" + } +} +vertical_position: 0.91297697595395233 +vertical_velocity: 10.596832320931542 +... +``` + +Then listen to the custom sensor: + +``` +ign topic -e -t /odometer +``` + +You'll see data like: + +``` +... +header { + stamp { + sec: 83 + } + data { + key: "frame_id" + value: "custom_odometer" + } + data { + key: "seq" + value: "83" + } +} +data: 21.632033902371308 +... +``` + diff --git a/examples/loop_sensor/main.cc b/examples/loop_sensor/main.cc new file mode 100644 index 00000000..be80b415 --- /dev/null +++ b/examples/loop_sensor/main.cc @@ -0,0 +1,148 @@ +/* + * 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 all supported sensors +#include +#include "../custom_sensor/Odometer.hh" + +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]); + + sdf::Root root; + auto errors = root.Load(sdfFile); + + for (const auto &error : errors) + { + ignerr << error << std::endl; + } + + auto world = root.WorldByIndex(0); + if (nullptr == world) + { + ignerr << "Failed to load world from [" << sdfFile << "]" << std::endl; + return 1; + } + + // Initiate sensor manager + ignition::sensors::Manager mgr; + + // Loop thorough SDF file and add all supported sensors to manager + std::vector sensors; + for (auto m = 0; m < world->ModelCount(); ++m) + { + 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; + } + + // Stop when user presses Ctrl+C + bool signaled{false}; + ignition::common::SignalHandler sigHandler; + sigHandler.AddCallback([&] (int) + { + 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 odometer = dynamic_cast(sensorPtr)) + { + odometer->NewPosition(odometer->Position() + + ignition::math::Vector3d(0.1, 0.1, 0.0)); + } + } + + // 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); + } + + return 0; +} diff --git a/examples/loop_sensor/sensors.sdf b/examples/loop_sensor/sensors.sdf new file mode 100644 index 00000000..a52827f4 --- /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 + odometer + + + 0.1 + 0.2 + + + + + + + diff --git a/include/ignition/sensors/CameraSensor.hh b/include/ignition/sensors/CameraSensor.hh index 6945b75b..a068a621 100644 --- a/include/ignition/sensors/CameraSensor.hh +++ b/include/ignition/sensors/CameraSensor.hh @@ -23,7 +23,6 @@ #include -#include #include #ifdef _WIN32 diff --git a/include/ignition/sensors/DepthCameraSensor.hh b/include/ignition/sensors/DepthCameraSensor.hh index 34e886da..c54dff4d 100644 --- a/include/ignition/sensors/DepthCameraSensor.hh +++ b/include/ignition/sensors/DepthCameraSensor.hh @@ -24,7 +24,6 @@ #include #include -#include #include #ifdef _WIN32 diff --git a/include/ignition/sensors/LogicalCameraSensor.hh b/include/ignition/sensors/LogicalCameraSensor.hh index d7bfa002..b8eb8bac 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/Manager.hh b/include/ignition/sensors/Manager.hh index 9c989b1f..543f7059 100644 --- a/include/ignition/sensors/Manager.hh +++ b/include/ignition/sensors/Manager.hh @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -27,6 +28,7 @@ #include #include #include +#include namespace ignition { @@ -60,123 +62,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); + /// \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. @@ -197,13 +137,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 32004a21..b82af96c 100644 --- a/include/ignition/sensors/SensorFactory.hh +++ b/include/ignition/sensors/SensorFactory.hh @@ -23,7 +23,6 @@ #include #include -#include #include #include @@ -41,32 +40,35 @@ namespace ignition class SensorFactoryPrivate; /// \brief Base sensor plugin interface + /// \deprecated Sensor plugins are deprecated. Instantiate sensor objects + /// instead. class IGNITION_SENSORS_VISIBLE SensorPlugin { - /// \brief Allows using shorter APIS in common::PluginLoader - public: IGN_COMMON_SPECIALIZE_INTERFACE(ignition::sensors::SensorPlugin) - /// \brief Instantiate new sensor /// \return New sensor - public: virtual Sensor *New() = 0; + public: virtual Sensor IGN_DEPRECATED(6) * New() = 0; }; /// \brief Templated class for instantiating sensors of the specified type /// \tparam Type of sensor being instantiated. + /// \deprecated Sensor plugins are deprecated. Instantiate sensor objects + /// instead. template class SensorTypePlugin : public SensorPlugin { // Documentation inherited - public: SensorType *New() override + public: SensorType IGN_DEPRECATED(6) * New() override { return new SensorType(); }; }; /// \brief A factory class for creating sensors - /// This class wll load a sensor plugin based on the given sensor type and - /// instantiates a sensor object - /// + /// This class instantiates sensor objects based on the sensor type and + /// makes sure they're initialized correctly. + // After removing plugin functionality, the sensor factory class doesn't + // hold any internal state. Consider converting the functionality in this + // class to helper functions. class IGNITION_SENSORS_VISIBLE SensorFactory { /// \brief Constructor @@ -76,81 +78,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. @@ -165,29 +179,22 @@ namespace ignition /// \param[in] _sdf SDF Sensor DOM object. /// \return A sensor id that refers to the created sensor. Null is /// is returned on error. - public: std::unique_ptr CreateSensor(const sdf::Sensor &_sdf); + /// \deprecated Sensor registration is deprecated, so it's necessary to + /// provide the specific sensor type to create it. Use the templated + /// `CreateSensor` function. + public: std::unique_ptr IGN_DEPRECATED(6) CreateSensor( + const sdf::Sensor &_sdf); /// \brief Add additional path to search for sensor plugins /// \param[in] _path Search path - public: void AddPluginPaths(const std::string &_path); - - /// \brief load a plugin and return a pointer - /// \param[in] _filename Sensor plugin file to load. - /// \return Pointer to the new sensor, nullptr on error. - private: std::shared_ptr LoadSensorPlugin( - const std::string &_filename); + /// \deprecated Sensor plugins aren't supported anymore. + public: void IGN_DEPRECATED(6) AddPluginPaths(const std::string &_path); IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief private data pointer private: std::unique_ptr dataPtr; IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING }; - - /// \brief Sensor registration macro - #define IGN_SENSORS_REGISTER_SENSOR(classname) \ - IGN_COMMON_REGISTER_SINGLE_PLUGIN(\ - ignition::sensors::SensorTypePlugin, \ - ignition::sensors::SensorPlugin) } } } diff --git a/include/ignition/sensors/ThermalCameraSensor.hh b/include/ignition/sensors/ThermalCameraSensor.hh index 58f68240..16702c7f 100644 --- a/include/ignition/sensors/ThermalCameraSensor.hh +++ b/include/ignition/sensors/ThermalCameraSensor.hh @@ -24,7 +24,6 @@ #include #include -#include #include #ifdef _WIN32 diff --git a/include/ignition/sensors/Util.hh b/include/ignition/sensors/Util.hh new file mode 100644 index 00000000..e9d2db43 --- /dev/null +++ b/include/ignition/sensors/Util.hh @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_SENSORS_UTIL_HH_ +#define IGNITION_SENSORS_UTIL_HH_ + +#include + +#include +#include + +#include "ignition/sensors/config.hh" +#include "ignition/sensors/Export.hh" + +namespace ignition +{ +namespace sensors +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { +// +/// \brief Get the name of a sensor's custom type from SDF. +/// +/// Given an SDF tag as follows: +/// +/// +/// +/// This function returns `sensor_type`. +/// +/// It will return an empty string if the element is malformed. For example, +/// if it misses the `ignition:type` attribute or is not of `type="custom"`. +/// +/// \param[in] _sdf Sensor SDF object. +/// \return _sensorType Name of sensor type. +std::string IGNITION_SENSORS_VISIBLE customType(const sdf::Sensor &_sdf); // NOLINT + +/// \brief Get the name of a sensor's custom type from SDF. +/// +/// Given an SDF tag as follows: +/// +/// +/// +/// This function returns `sensor_type`. +/// +/// It will return an empty string if the element is malformed. For example, +/// if it misses the `ignition:type` attribute or is not of `type="custom"`. +/// +/// \param[in] _sdf Sensor SDF object. +/// \return _sensorType Name of sensor type. +std::string IGNITION_SENSORS_VISIBLE customType(sdf::ElementPtr _sdf); // NOLINT +} +} +} + +#endif diff --git a/src/AirPressureSensor.cc b/src/AirPressureSensor.cc index f3bed50d..b10ed384 100644 --- a/src/AirPressureSensor.cc +++ b/src/AirPressureSensor.cc @@ -217,4 +217,3 @@ double AirPressureSensor::ReferenceAltitude() const return this->dataPtr->referenceAltitude; } -IGN_SENSORS_REGISTER_SENSOR(AirPressureSensor) diff --git a/src/AltimeterSensor.cc b/src/AltimeterSensor.cc index d051c590..f77ab47a 100644 --- a/src/AltimeterSensor.cc +++ b/src/AltimeterSensor.cc @@ -212,4 +212,3 @@ double AltimeterSensor::VerticalVelocity() const return this->dataPtr->verticalVelocity; } -IGN_SENSORS_REGISTER_SENSOR(AltimeterSensor) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index bf252e95..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) @@ -142,18 +144,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/CameraSensor.cc b/src/CameraSensor.cc index 4c3c3d27..75648486 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -633,4 +633,3 @@ double CameraSensor::Baseline() const return this->dataPtr->baseline; } -IGN_SENSORS_REGISTER_SENSOR(CameraSensor) 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 a726a3f7..8e231e1f 100644 --- a/src/DepthCameraSensor.cc +++ b/src/DepthCameraSensor.cc @@ -626,4 +626,3 @@ double DepthCameraSensor::NearClip() const return this->dataPtr->near; } -IGN_SENSORS_REGISTER_SENSOR(DepthCameraSensor) diff --git a/src/GpuLidarSensor.cc b/src/GpuLidarSensor.cc index 5abc5e62..8db59f21 100644 --- a/src/GpuLidarSensor.cc +++ b/src/GpuLidarSensor.cc @@ -108,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; @@ -370,4 +370,3 @@ void GpuLidarSensorPrivate::FillPointCloudMsg(const float *_laserBuffer) } } -IGN_SENSORS_REGISTER_SENSOR(GpuLidarSensor) diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index b7efff39..f8e2db5e 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -295,4 +295,3 @@ math::Quaterniond ImuSensor::Orientation() const return this->dataPtr->orientation; } -IGN_SENSORS_REGISTER_SENSOR(ImuSensor) diff --git a/src/Lidar.cc b/src/Lidar.cc index c1dd1320..a73b10ee 100644 --- a/src/Lidar.cc +++ b/src/Lidar.cc @@ -450,4 +450,3 @@ bool Lidar::IsActive() const return true; } -IGN_SENSORS_REGISTER_SENSOR(Lidar) diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc index 93c74c3d..b190c099 100644 --- a/src/LogicalCameraSensor.cc +++ b/src/LogicalCameraSensor.cc @@ -19,11 +19,9 @@ #include #include - -#include - #include #include +#include #include "ignition/sensors/SensorFactory.hh" #include "ignition/sensors/LogicalCameraSensor.hh" @@ -202,4 +200,3 @@ msgs::LogicalCameraImage LogicalCameraSensor::Image() const return this->dataPtr->msg; } -IGN_SENSORS_REGISTER_SENSOR(LogicalCameraSensor) diff --git a/src/MagnetometerSensor.cc b/src/MagnetometerSensor.cc index 61d93264..81a0da5f 100644 --- a/src/MagnetometerSensor.cc +++ b/src/MagnetometerSensor.cc @@ -231,4 +231,3 @@ math::Vector3d MagnetometerSensor::MagneticField() const return this->dataPtr->localField; } -IGN_SENSORS_REGISTER_SENSOR(MagnetometerSensor) diff --git a/src/Manager.cc b/src/Manager.cc index 518ce950..355c8217 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 @@ -31,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; }; -////////////////////////////////////////////////// -ManagerPrivate::ManagerPrivate() -{ -} - -////////////////////////////////////////////////// -ManagerPrivate::~ManagerPrivate() -{ -} - ////////////////////////////////////////////////// Manager::Manager() : dataPtr(new ManagerPrivate) @@ -80,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; } ////////////////////////////////////////////////// @@ -103,25 +83,30 @@ void Manager::RunOnce( } ///////////////////////////////////////////////// -ignition::sensors::SensorId Manager::CreateSensor(const sdf::Sensor &_sdf) +ignition::sensors::SensorId Manager::AddSensor( + std::unique_ptr _sensor) { - auto sensor = this->dataPtr->sensorFactory.CreateSensor(_sdf); - if (!sensor) + if (!_sensor) return NO_SENSOR; - - SensorId id = sensor->Id(); - this->dataPtr->sensors[id] = std::move(sensor); + SensorId id = _sensor->Id(); + this->dataPtr->sensors[id] = std::move(_sensor); return id; } ///////////////////////////////////////////////// -ignition::sensors::SensorId Manager::CreateSensor(sdf::ElementPtr _sdf) +ignition::sensors::SensorId Manager::CreateSensor(const sdf::Sensor &) { - auto sensor = this->dataPtr->sensorFactory.CreateSensor(_sdf); - if (!sensor) - return NO_SENSOR; + 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; +} - SensorId id = sensor->Id(); - this->dataPtr->sensors[id] = std::move(sensor); - return id; +///////////////////////////////////////////////// +ignition::sensors::SensorId Manager::CreateSensor(sdf::ElementPtr) +{ + 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..d0b3d294 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,14 +55,44 @@ TEST(Manager, construct) } ////////////////////////////////////////////////// -TEST(Manager, 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/src/RgbdCameraSensor.cc b/src/RgbdCameraSensor.cc index 3c5fe6ef..d368d930 100644 --- a/src/RgbdCameraSensor.cc +++ b/src/RgbdCameraSensor.cc @@ -174,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) { @@ -603,4 +611,3 @@ unsigned int RgbdCameraSensor::ImageHeight() const return this->dataPtr->depthCamera->ImageHeight(); } -IGN_SENSORS_REGISTER_SENSOR(RgbdCameraSensor) diff --git a/src/SensorFactory.cc b/src/SensorFactory.cc index 5e9cfc58..0a8dae55 100644 --- a/src/SensorFactory.cc +++ b/src/SensorFactory.cc @@ -15,42 +15,23 @@ * */ -#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 For loading plugins - public: ignition::common::PluginLoader pluginLoader; }; 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; } ////////////////////////////////////////////////// @@ -63,136 +44,20 @@ SensorFactory::~SensorFactory() { } -////////////////////////////////////////////////// -std::shared_ptr SensorFactory::LoadSensorPlugin( - const std::string &_filename) -{ - 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(); - } - - auto pluginNames = this->dataPtr->pluginLoader.LoadLibrary(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); - - auto sensorPlugin = pluginPtr->QueryInterfaceSharedPtr< - ignition::sensors::SensorPlugin>(); - 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(); - 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); - if (!sensorPlugin) - { - ignerr << "Unable to instantiate sensor plugin for [" << fullPath - << "]\n"; - return nullptr; - } - - this->dataPtr->sensorPlugins[type] = sensorPlugin; - } - - auto sensor = sensorPlugin->New(); - if (!sensor) - { - ignerr << "Unable to instantiate sensor for [" << fullPath << "]\n"; - return nullptr; - } - - if (!sensor->Load(_sdf)) - { - ignerr << "Sensor::Load failed for plugin [" << fullPath << "]\n"; - return nullptr; - } - - if (!sensor->Init()) - { - ignerr << "Sensor::Init failed for plugin [" << fullPath << "]\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"); - 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); - if (!sensorPlugin) - { - ignerr << "Unable to instantiate sensor plugin for [" << fullPath - << "]\n"; - return nullptr; - } - - this->dataPtr->sensorPlugins[type] = sensorPlugin; - } - - auto sensor = sensorPlugin->New(); - if (!sensor) - { - ignerr << "Unable to instantiate sensor for [" << fullPath << "]\n"; - return nullptr; - } - - if (!sensor->Load(_sdf)) - { - ignerr << "Sensor::Load failed for plugin [" << fullPath << "]\n"; - return nullptr; - } - - if (!sensor->Init()) - { - ignerr << "Sensor::Init failed for plugin [" << fullPath << "]\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 bf1870bf..d47a64b4 100644 --- a/src/ThermalCameraSensor.cc +++ b/src/ThermalCameraSensor.cc @@ -648,4 +648,3 @@ bool ThermalCameraSensorPrivate::SaveImage(const uint16_t *_data, return true; } -IGN_SENSORS_REGISTER_SENSOR(ThermalCameraSensor) 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/CMakeLists.txt b/test/integration/CMakeLists.txt index 2c5de9fe..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) @@ -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() diff --git a/test/integration/air_pressure_plugin.cc b/test/integration/air_pressure.cc similarity index 90% rename from test/integration/air_pressure_plugin.cc rename to test/integration/air_pressure.cc index a7b74fb4..347b2649 100644 --- a/test/integration/air_pressure_plugin.cc +++ b/test/integration/air_pressure.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.cc similarity index 91% rename from test/integration/altimeter_plugin.cc rename to test/integration/altimeter.cc index 4139ad0e..653f07df 100644 --- a/test/integration/altimeter_plugin.cc +++ b/test/integration/altimeter.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.cc similarity index 94% rename from test/integration/camera_plugin.cc rename to test/integration/camera.cc index ad86ee99..a8bc14bc 100644 --- a/test/integration/camera_plugin.cc +++ b/test/integration/camera.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); }; @@ -59,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.cc similarity index 99% rename from test/integration/depth_camera_plugin.cc rename to test/integration/depth_camera.cc index 991418fa..19e0377d 100644 --- a/test/integration/depth_camera_plugin.cc +++ b/test/integration/depth_camera.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/gpu_lidar_sensor_plugin.cc b/test/integration/gpu_lidar_sensor.cc similarity index 98% rename from test/integration/gpu_lidar_sensor_plugin.cc rename to test/integration/gpu_lidar_sensor.cc index d906e62e..f76ec4e2 100644 --- a/test/integration/gpu_lidar_sensor_plugin.cc +++ b/test/integration/gpu_lidar_sensor.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.cc similarity index 93% rename from test/integration/imu_plugin.cc rename to test/integration/imu.cc index f24fcff6..43c6a991 100644 --- a/test/integration/imu_plugin.cc +++ b/test/integration/imu.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.cc similarity index 92% rename from test/integration/logical_camera_plugin.cc rename to test/integration/logical_camera.cc index 61191220..d4ff2d3e 100644 --- a/test/integration/logical_camera_plugin.cc +++ b/test/integration/logical_camera.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.cc similarity index 92% rename from test/integration/magnetometer_plugin.cc rename to test/integration/magnetometer.cc index 2767515b..a96fe766 100644 --- a/test/integration/magnetometer_plugin.cc +++ b/test/integration/magnetometer.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/test/integration/rgbd_camera_plugin.cc b/test/integration/rgbd_camera.cc similarity index 99% rename from test/integration/rgbd_camera_plugin.cc rename to test/integration/rgbd_camera.cc index 2094e449..c7432ec8 100644 --- a/test/integration/rgbd_camera_plugin.cc +++ b/test/integration/rgbd_camera.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.cc similarity index 99% rename from test/integration/thermal_camera_plugin.cc rename to test/integration/thermal_camera.cc index 59c0843b..f7bf9298 100644 --- a/test/integration/thermal_camera_plugin.cc +++ b/test/integration/thermal_camera.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 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/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. diff --git a/tutorials/custom_sensors.md b/tutorials/custom_sensors.md new file mode 100644 index 00000000..c6cd5ac4 --- /dev/null +++ b/tutorials/custom_sensors.md @@ -0,0 +1,117 @@ +\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 with 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 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 +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). + +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: + +* 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: + +```xml + + 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. +* 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. +