From 631530addd86b469c2578e8138850e50283fc7ee Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Mon, 5 Aug 2024 11:14:48 +0200 Subject: [PATCH] Add optional sensor_info to sensor interface Add a `sensor_info` field to `SensorData` which can be used to provide static information about the sensor to the user. A method `get_sensor_info()` is added to `SensorDriver` to fill the field in the backend. It can then be accessed via a method of the same name in `SensorFrontend`. By default, it provides an empty struct for backward compatibility. Use this for data that doesn't change between observations (e.g. calibration parameters of frame rate). Changing values should be added to the observation instead. --- CHANGELOG.md | 4 ++ doc/custom_sensor.rst | 23 ++++++++ doc_mainpage.rst | 1 + .../sensors/pybind_sensors.hpp | 39 ++++++++------ .../sensors/sensor_backend.hpp | 24 ++++++--- .../robot_interfaces/sensors/sensor_data.hpp | 52 +++++++++++++++---- .../sensors/sensor_driver.hpp | 15 +++++- .../sensors/sensor_frontend.hpp | 18 +++++-- .../sensors/sensor_logger.hpp | 6 +-- include/robot_interfaces/utils.hpp | 25 +++++++++ tests/test_sensor_logger.cpp | 5 +- 11 files changed, 166 insertions(+), 46 deletions(-) create mode 100644 doc/custom_sensor.rst create mode 100644 include/robot_interfaces/utils.hpp diff --git a/CHANGELOG.md b/CHANGELOG.md index 4cce608..1686dd5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,10 @@ classes). - Method `RobotDriver::get_idle_action()` that is expected to return an action that is safe to apply while the robot is idle. +- Option to provide static information about a sensor (e.g. camera calibration + coefficients, frame rate, etc.). For this, a method `get_sensor_info()` is added to + `SensorFrontend`. It defaults to an empty struct for backward compatibility. To use + it, implement the method with the same name in `SensorDriver`. ### Changed - The return type of `RobotDriver::get_error()` is changed to diff --git a/doc/custom_sensor.rst b/doc/custom_sensor.rst new file mode 100644 index 0000000..3b37699 --- /dev/null +++ b/doc/custom_sensor.rst @@ -0,0 +1,23 @@ +**************** +Sensor Interface +**************** + +``robot_interfaces`` also provides base classes for a pure sensor interface. In +contrast to a robot, a sensor only provides observations but does not take actions. +Apart from that, the overall structure is mostly the same as for a robot. That is, +there are :cpp:class:`~robot_interfaces::SensorBackend` and +:cpp:class:`~robot_interfaces::SensorFrontend` which communicate via +:cpp:class:`~robot_interfaces::SensorData` (either single- or multi-process). + +For implementing an interface for your sensor, you only need to implement an observation +type and a driver class based on :cpp:class:`~robot_interfaces::SensorDriver`. Then +simply create data/backend/frontend using those custom types as template arguments. + +Optionally, your driver class can also provide a "sensor info" object by implementing +:cpp:func:`~robot_interfaces::SensorDriver::get_sensor_info`. This object is then +accessible by the user via +:cpp:func:`~robot_interfaces::SensorFrontend::get_sensor_info`. You may use this, to +provide static information about the sensor, that does not change over time (e.g. frame +rate of a camera). +If you don't implement the corresponding method in the driver, the front end will return +an empty struct as placeholder. diff --git a/doc_mainpage.rst b/doc_mainpage.rst index ae32fdc..b7619a1 100644 --- a/doc_mainpage.rst +++ b/doc_mainpage.rst @@ -15,6 +15,7 @@ paper_ on the open-source version of the TriFinger robot. doc/real_time.rst doc/quick_start_example.rst doc/custom_driver.rst + doc/custom_sensor.rst .. toctree:: :caption: Topic Guides diff --git a/include/robot_interfaces/sensors/pybind_sensors.hpp b/include/robot_interfaces/sensors/pybind_sensors.hpp index e55e64a..77421ef 100644 --- a/include/robot_interfaces/sensors/pybind_sensors.hpp +++ b/include/robot_interfaces/sensors/pybind_sensors.hpp @@ -16,6 +16,7 @@ #include #include #include +#include namespace robot_interfaces { @@ -24,7 +25,7 @@ namespace robot_interfaces * * @tparam The ObservationType */ -template +template void create_sensor_bindings(pybind11::module& m) { pybind11::options options; @@ -33,12 +34,15 @@ void create_sensor_bindings(pybind11::module& m) options.disable_function_signatures(); // some typedefs to keep code below shorter - typedef SensorData BaseData; - typedef SingleProcessSensorData SingleProcData; - typedef MultiProcessSensorData MultiProcData; - typedef SensorLogger Logger; + typedef SensorData BaseData; + typedef SingleProcessSensorData SingleProcData; + typedef MultiProcessSensorData MultiProcData; + typedef SensorLogger Logger; typedef SensorLogReader LogReader; + pybind11::class_>( + m, "None", pybind11::module_local()); + pybind11::class_>(m, "BaseData"); pybind11::class_, BaseData>( @@ -52,31 +56,34 @@ void create_sensor_bindings(pybind11::module& m) pybind11::arg("is_master"), pybind11::arg("history_size") = 1000); - pybind11::class_, - std::shared_ptr>>(m, - "Driver"); + pybind11::class_, + std::shared_ptr>>( + m, "Driver"); - pybind11::class_>(m, "Backend") + pybind11::class_>(m, "Backend") .def(pybind11::init< - typename std::shared_ptr>, + typename std::shared_ptr>, typename std::shared_ptr>()) .def("shutdown", - &SensorBackend::shutdown, + &SensorBackend::shutdown, pybind11::call_guard()); - pybind11::class_>(m, "Frontend") + pybind11::class_>(m, "Frontend") .def(pybind11::init>()) + .def("get_sensor_info", + &SensorFrontend::get_sensor_info, + pybind11::call_guard()) .def("get_latest_observation", - &SensorFrontend::get_latest_observation, + &SensorFrontend::get_latest_observation, pybind11::call_guard()) .def("get_observation", - &SensorFrontend::get_observation, + &SensorFrontend::get_observation, pybind11::call_guard()) .def("get_timestamp_ms", - &SensorFrontend::get_timestamp_ms, + &SensorFrontend::get_timestamp_ms, pybind11::call_guard()) .def("get_current_timeindex", - &SensorFrontend::get_current_timeindex, + &SensorFrontend::get_current_timeindex, pybind11::call_guard()); pybind11::class_>(m, "Logger") diff --git a/include/robot_interfaces/sensors/sensor_backend.hpp b/include/robot_interfaces/sensors/sensor_backend.hpp index ade2c07..4aaf07a 100644 --- a/include/robot_interfaces/sensors/sensor_backend.hpp +++ b/include/robot_interfaces/sensors/sensor_backend.hpp @@ -15,6 +15,7 @@ #include #include +#include namespace robot_interfaces { @@ -27,24 +28,31 @@ namespace robot_interfaces * * @tparam ObservationType */ -template +template class SensorBackend { public: - typedef std::shared_ptr> Ptr; - typedef std::shared_ptr> ConstPtr; + typedef std::shared_ptr> Ptr; + typedef std::shared_ptr> + ConstPtr; /** * @param sensor_driver Driver instance for the sensor. * @param sensor_data Data is sent to/retrieved from here. */ - SensorBackend(std::shared_ptr> sensor_driver, - std::shared_ptr> sensor_data) + SensorBackend( + std::shared_ptr> sensor_driver, + std::shared_ptr> sensor_data) : sensor_driver_(sensor_driver), sensor_data_(sensor_data), shutdown_requested_(false) { - thread_ = std::thread(&SensorBackend::loop, this); + // populate the sensor information field + InfoType info = sensor_driver_->get_sensor_info(); + sensor_data_->sensor_info->append(info); + + thread_ = + std::thread(&SensorBackend::loop, this); } // reinstate the implicit move constructor @@ -67,8 +75,8 @@ class SensorBackend } private: - std::shared_ptr> sensor_driver_; - std::shared_ptr> sensor_data_; + std::shared_ptr> sensor_driver_; + std::shared_ptr> sensor_data_; bool shutdown_requested_; diff --git a/include/robot_interfaces/sensors/sensor_data.hpp b/include/robot_interfaces/sensors/sensor_data.hpp index 9e2da50..3f147ea 100644 --- a/include/robot_interfaces/sensors/sensor_data.hpp +++ b/include/robot_interfaces/sensors/sensor_data.hpp @@ -1,4 +1,3 @@ - /** * @file * @brief To store all the data from all the sensors in use @@ -16,6 +15,8 @@ #include #include +#include + namespace robot_interfaces { /** @@ -23,12 +24,21 @@ namespace robot_interfaces * * @tparam Observation Type of the sensor observation. */ -template +template class SensorData { public: - typedef std::shared_ptr> Ptr; - typedef std::shared_ptr> ConstPtr; + typedef std::shared_ptr> Ptr; + typedef std::shared_ptr> ConstPtr; + + /** + * @brief Static information about the sensor + * + * Note: A time series is used here for convenience to handle the shared + * memory aspect. However, this is intended to only hold one element that + * doesn't change over time. + */ + std::shared_ptr> sensor_info; //! @brief Time series of the sensor observations. std::shared_ptr> observation; @@ -48,12 +58,15 @@ class SensorData * @copydoc SensorData * @see MultiProcessSensorData */ -template -class SingleProcessSensorData : public SensorData +template +class SingleProcessSensorData : public SensorData { public: SingleProcessSensorData(size_t history_length = 1000) { + // sensor_info only contains a single static element, so length is set + // to 1 + this->sensor_info = std::make_shared>(1); this->observation = std::make_shared>( history_length); @@ -71,27 +84,44 @@ class SingleProcessSensorData : public SensorData * @copydoc SensorData * @see SingleProcessSensorData */ -template -class MultiProcessSensorData : public SensorData +template +class MultiProcessSensorData : public SensorData { public: MultiProcessSensorData(const std::string &shared_memory_id, bool is_master, size_t history_length = 1000) { + // each time series needs its own shared memory ID, so add unique + // suffixes to the given ID. + const std::string shm_id_info = shared_memory_id + "_info"; + const std::string shm_id_observation = + shared_memory_id + "_observation"; + if (is_master) { // the master instance is in charge of cleaning the memory - time_series::clear_memory(shared_memory_id); + time_series::clear_memory(shm_id_info); + time_series::clear_memory(shm_id_observation); + + // sensor_info only contains a single static element, so length is + // set to 1 + this->sensor_info = + time_series::MultiprocessTimeSeries::create_leader_ptr( + shm_id_info, 1); this->observation = time_series::MultiprocessTimeSeries< - Observation>::create_leader_ptr(shared_memory_id, + Observation>::create_leader_ptr(shm_id_observation, history_length); } else { + this->sensor_info = + time_series::MultiprocessTimeSeries::create_follower_ptr( + shm_id_info); + this->observation = time_series::MultiprocessTimeSeries< - Observation>::create_follower_ptr(shared_memory_id); + Observation>::create_follower_ptr(shm_id_observation); } } }; diff --git a/include/robot_interfaces/sensors/sensor_driver.hpp b/include/robot_interfaces/sensors/sensor_driver.hpp index 0302b61..dbdeca7 100644 --- a/include/robot_interfaces/sensors/sensor_driver.hpp +++ b/include/robot_interfaces/sensors/sensor_driver.hpp @@ -10,6 +10,8 @@ #include +#include + namespace robot_interfaces { /** @@ -18,7 +20,7 @@ namespace robot_interfaces * * @tparam ObservationType */ -template +template class SensorDriver { public: @@ -27,6 +29,17 @@ class SensorDriver { } + /** + * @brief Return static information about the sensor. + * + * This information is expected to be constructed during initialization and + * to not change later on. + */ + virtual InfoType get_sensor_info() + { + return InfoType(); + } + /** * @brief return the observation * @return depends on the observation structure diff --git a/include/robot_interfaces/sensors/sensor_frontend.hpp b/include/robot_interfaces/sensors/sensor_frontend.hpp index 6161fed..7ec4f0d 100644 --- a/include/robot_interfaces/sensors/sensor_frontend.hpp +++ b/include/robot_interfaces/sensors/sensor_frontend.hpp @@ -15,6 +15,7 @@ #include #include +#include namespace robot_interfaces { @@ -26,23 +27,30 @@ namespace robot_interfaces * * @tparam ObservationType */ -template +template class SensorFrontend { public: template using Timeseries = time_series::TimeSeries; - typedef std::shared_ptr> Ptr; - typedef std::shared_ptr> ConstPtr; + typedef std::shared_ptr> Ptr; + typedef std::shared_ptr> + ConstPtr; typedef time_series::Timestamp TimeStamp; typedef time_series::Index TimeIndex; - SensorFrontend(std::shared_ptr> sensor_data) + SensorFrontend( + std::shared_ptr> sensor_data) : sensor_data_(sensor_data) { } + InfoType get_sensor_info() const + { + return sensor_data_->sensor_info->newest_element(); + } + ObservationType get_observation(const TimeIndex t) const { return (*sensor_data_->observation)[t]; @@ -63,7 +71,7 @@ class SensorFrontend } private: - std::shared_ptr> sensor_data_; + std::shared_ptr> sensor_data_; }; } // namespace robot_interfaces diff --git a/include/robot_interfaces/sensors/sensor_logger.hpp b/include/robot_interfaces/sensors/sensor_logger.hpp index ab38915..e2e3b90 100644 --- a/include/robot_interfaces/sensors/sensor_logger.hpp +++ b/include/robot_interfaces/sensors/sensor_logger.hpp @@ -43,11 +43,11 @@ namespace robot_interfaces * * @tparam Observation Typ of the observation that is recorded. */ -template +template class SensorLogger { public: - typedef std::shared_ptr> DataPtr; + typedef std::shared_ptr> DataPtr; typedef typename std::tuple StampedObservation; /** @@ -89,7 +89,7 @@ class SensorLogger { enabled_ = true; buffer_thread_ = - std::thread(&SensorLogger::loop, this); + std::thread(&SensorLogger::loop, this); } } diff --git a/include/robot_interfaces/utils.hpp b/include/robot_interfaces/utils.hpp new file mode 100644 index 0000000..58a1f9c --- /dev/null +++ b/include/robot_interfaces/utils.hpp @@ -0,0 +1,25 @@ +/** + * @file + * @copyright 2020, New York University, Max Planck Gesellschaft. All rights + * reserved. + * @license BSD-3-clause + */ +#pragma once + +#include + +namespace robot_interfaces +{ +//! @brief Empty struct that can be used as placeholder. +struct None +{ + template + void serialize(Archive& archive) + { + // need to serialize some dummy value here, as an actual empty type will + // cause trouble for our shared_memory implementation + uint8_t dummy = 0; + archive(dummy); + } +}; +} // namespace robot_interfaces diff --git a/tests/test_sensor_logger.cpp b/tests/test_sensor_logger.cpp index 21fe4df..e928359 100644 --- a/tests/test_sensor_logger.cpp +++ b/tests/test_sensor_logger.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include "dummy_sensor_driver.hpp" @@ -50,7 +51,7 @@ TEST_F(TestSensorLogger, write_and_read_log) auto driver = std::make_shared(); auto frontend = SensorFrontend(data); - auto logger = SensorLogger(data, BUFFER_LIMIT); + auto logger = SensorLogger(data, BUFFER_LIMIT); logger.start(); // create backend last to ensure no message is missed @@ -90,7 +91,7 @@ TEST_F(TestSensorLogger, buffer_limit) auto frontend = SensorFrontend(data); // set buffer - auto logger = SensorLogger(data, BUFFER_LIMIT); + auto logger = SensorLogger(data, BUFFER_LIMIT); logger.start(); // create backend last to ensure no message is missed