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