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.
open-dynamic-robot-initiative · Aug 20, 2024 · 631530a · 631530a
1 parent ae3b768
commit 631530a
Show file tree

Hide file tree

Showing 11 changed files with 166 additions and 46 deletions.
diff --git 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
@@ -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
@@ -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
@@ -16,6 +16,7 @@
 #include <robot_interfaces/sensors/sensor_frontend.hpp>
 #include <robot_interfaces/sensors/sensor_log_reader.hpp>
 #include <robot_interfaces/sensors/sensor_logger.hpp>
+#include <robot_interfaces/utils.hpp>
 
 namespace robot_interfaces
 {
@@ -24,7 +25,7 @@ namespace robot_interfaces
  *
  * @tparam The ObservationType
  */
-template <typename ObservationType>
+template <typename ObservationType, typename InfoType = None>
 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<ObservationType> BaseData;
-    typedef SingleProcessSensorData<ObservationType> SingleProcData;
-    typedef MultiProcessSensorData<ObservationType> MultiProcData;
-    typedef SensorLogger<ObservationType> Logger;
+    typedef SensorData<ObservationType, InfoType> BaseData;
+    typedef SingleProcessSensorData<ObservationType, InfoType> SingleProcData;
+    typedef MultiProcessSensorData<ObservationType, InfoType> MultiProcData;
+    typedef SensorLogger<ObservationType, InfoType> Logger;
     typedef SensorLogReader<ObservationType> LogReader;
 
+    pybind11::class_<None, std::shared_ptr<None>>(
+        m, "None", pybind11::module_local());
+
     pybind11::class_<BaseData, std::shared_ptr<BaseData>>(m, "BaseData");
 
     pybind11::class_<SingleProcData, std::shared_ptr<SingleProcData>, BaseData>(
@@ -52,31 +56,34 @@ void create_sensor_bindings(pybind11::module& m)
              pybind11::arg("is_master"),
              pybind11::arg("history_size") = 1000);
 
-    pybind11::class_<SensorDriver<ObservationType>,
-                     std::shared_ptr<SensorDriver<ObservationType>>>(m,
-                                                                     "Driver");
+    pybind11::class_<SensorDriver<ObservationType, InfoType>,
+                     std::shared_ptr<SensorDriver<ObservationType, InfoType>>>(
+        m, "Driver");
 
-    pybind11::class_<SensorBackend<ObservationType>>(m, "Backend")
+    pybind11::class_<SensorBackend<ObservationType, InfoType>>(m, "Backend")
         .def(pybind11::init<
-             typename std::shared_ptr<SensorDriver<ObservationType>>,
+             typename std::shared_ptr<SensorDriver<ObservationType, InfoType>>,
              typename std::shared_ptr<BaseData>>())
         .def("shutdown",
-             &SensorBackend<ObservationType>::shutdown,
+             &SensorBackend<ObservationType, InfoType>::shutdown,
              pybind11::call_guard<pybind11::gil_scoped_release>());
 
-    pybind11::class_<SensorFrontend<ObservationType>>(m, "Frontend")
+    pybind11::class_<SensorFrontend<ObservationType, InfoType>>(m, "Frontend")
         .def(pybind11::init<typename std::shared_ptr<BaseData>>())
+        .def("get_sensor_info",
+             &SensorFrontend<ObservationType, InfoType>::get_sensor_info,
+             pybind11::call_guard<pybind11::gil_scoped_release>())
         .def("get_latest_observation",
-             &SensorFrontend<ObservationType>::get_latest_observation,
+             &SensorFrontend<ObservationType, InfoType>::get_latest_observation,
              pybind11::call_guard<pybind11::gil_scoped_release>())
         .def("get_observation",
-             &SensorFrontend<ObservationType>::get_observation,
+             &SensorFrontend<ObservationType, InfoType>::get_observation,
              pybind11::call_guard<pybind11::gil_scoped_release>())
         .def("get_timestamp_ms",
-             &SensorFrontend<ObservationType>::get_timestamp_ms,
+             &SensorFrontend<ObservationType, InfoType>::get_timestamp_ms,
              pybind11::call_guard<pybind11::gil_scoped_release>())
         .def("get_current_timeindex",
-             &SensorFrontend<ObservationType>::get_current_timeindex,
+             &SensorFrontend<ObservationType, InfoType>::get_current_timeindex,
              pybind11::call_guard<pybind11::gil_scoped_release>());
 
     pybind11::class_<Logger, std::shared_ptr<Logger>>(m, "Logger")

diff --git a/include/robot_interfaces/sensors/sensor_backend.hpp b/include/robot_interfaces/sensors/sensor_backend.hpp
@@ -15,6 +15,7 @@
 
 #include <robot_interfaces/sensors/sensor_data.hpp>
 #include <robot_interfaces/sensors/sensor_driver.hpp>
+#include <robot_interfaces/utils.hpp>
 
 namespace robot_interfaces
 {
@@ -27,24 +28,31 @@ namespace robot_interfaces
  *
  * @tparam ObservationType
  */
-template <typename ObservationType>
+template <typename ObservationType, typename InfoType = None>
 class SensorBackend
 {
 public:
-    typedef std::shared_ptr<SensorBackend<ObservationType>> Ptr;
-    typedef std::shared_ptr<const SensorBackend<ObservationType>> ConstPtr;
+    typedef std::shared_ptr<SensorBackend<ObservationType, InfoType>> Ptr;
+    typedef std::shared_ptr<const SensorBackend<ObservationType, InfoType>>
+        ConstPtr;
 
     /**
      * @param sensor_driver  Driver instance for the sensor.
      * @param sensor_data  Data is sent to/retrieved from here.
      */
-    SensorBackend(std::shared_ptr<SensorDriver<ObservationType>> sensor_driver,
-                  std::shared_ptr<SensorData<ObservationType>> sensor_data)
+    SensorBackend(
+        std::shared_ptr<SensorDriver<ObservationType, InfoType>> sensor_driver,
+        std::shared_ptr<SensorData<ObservationType, InfoType>> sensor_data)
         : sensor_driver_(sensor_driver),
           sensor_data_(sensor_data),
           shutdown_requested_(false)
     {
-        thread_ = std::thread(&SensorBackend<ObservationType>::loop, this);
+        // populate the sensor information field
+        InfoType info = sensor_driver_->get_sensor_info();
+        sensor_data_->sensor_info->append(info);
+
+        thread_ =
+            std::thread(&SensorBackend<ObservationType, InfoType>::loop, this);
     }
 
     // reinstate the implicit move constructor
@@ -67,8 +75,8 @@ class SensorBackend
     }
 
 private:
-    std::shared_ptr<SensorDriver<ObservationType>> sensor_driver_;
-    std::shared_ptr<SensorData<ObservationType>> sensor_data_;
+    std::shared_ptr<SensorDriver<ObservationType, InfoType>> sensor_driver_;
+    std::shared_ptr<SensorData<ObservationType, InfoType>> sensor_data_;
 
     bool shutdown_requested_;
 

diff --git 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,19 +15,30 @@
 #include <time_series/multiprocess_time_series.hpp>
 #include <time_series/time_series.hpp>
 
+#include <robot_interfaces/utils.hpp>
+
 namespace robot_interfaces
 {
 /**
  * @brief Contains the data coming from the sensors.
  *
  * @tparam Observation  Type of the sensor observation.
  */
-template <typename Observation>
+template <typename Observation, typename Info = None>
 class SensorData
 {
 public:
-    typedef std::shared_ptr<SensorData<Observation>> Ptr;
-    typedef std::shared_ptr<const SensorData<Observation>> ConstPtr;
+    typedef std::shared_ptr<SensorData<Observation, Info>> Ptr;
+    typedef std::shared_ptr<const SensorData<Observation, Info>> 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<time_series::TimeSeriesInterface<Info>> sensor_info;
 
     //! @brief Time series of the sensor observations.
     std::shared_ptr<time_series::TimeSeriesInterface<Observation>> observation;
@@ -48,12 +58,15 @@ class SensorData
  * @copydoc SensorData
  * @see MultiProcessSensorData
  */
-template <typename Observation>
-class SingleProcessSensorData : public SensorData<Observation>
+template <typename Observation, typename Info = None>
+class SingleProcessSensorData : public SensorData<Observation, Info>
 {
 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<time_series::TimeSeries<Info>>(1);
         this->observation =
             std::make_shared<time_series::TimeSeries<Observation>>(
                 history_length);
@@ -71,27 +84,44 @@ class SingleProcessSensorData : public SensorData<Observation>
  * @copydoc SensorData
  * @see SingleProcessSensorData
  */
-template <typename Observation>
-class MultiProcessSensorData : public SensorData<Observation>
+template <typename Observation, typename Info = None>
+class MultiProcessSensorData : public SensorData<Observation, Info>
 {
 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<Info>::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<Info>::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
@@ -10,6 +10,8 @@
 
 #include <iostream>
 
+#include <robot_interfaces/utils.hpp>
+
 namespace robot_interfaces
 {
 /**
@@ -18,7 +20,7 @@ namespace robot_interfaces
  *
  * @tparam ObservationType
  */
-template <typename ObservationType>
+template <typename ObservationType, typename InfoType = None>
 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
@@ -15,6 +15,7 @@
 #include <time_series/time_series.hpp>
 
 #include <robot_interfaces/sensors/sensor_data.hpp>
+#include <robot_interfaces/utils.hpp>
 
 namespace robot_interfaces
 {
@@ -26,23 +27,30 @@ namespace robot_interfaces
  *
  * @tparam ObservationType
  */
-template <typename ObservationType>
+template <typename ObservationType, typename InfoType = None>
 class SensorFrontend
 {
 public:
     template <typename Type>
     using Timeseries = time_series::TimeSeries<Type>;
 
-    typedef std::shared_ptr<SensorFrontend<ObservationType>> Ptr;
-    typedef std::shared_ptr<const SensorFrontend<ObservationType>> ConstPtr;
+    typedef std::shared_ptr<SensorFrontend<ObservationType, InfoType>> Ptr;
+    typedef std::shared_ptr<const SensorFrontend<ObservationType, InfoType>>
+        ConstPtr;
     typedef time_series::Timestamp TimeStamp;
     typedef time_series::Index TimeIndex;
 
-    SensorFrontend(std::shared_ptr<SensorData<ObservationType>> sensor_data)
+    SensorFrontend(
+        std::shared_ptr<SensorData<ObservationType, InfoType>> 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<SensorData<ObservationType>> sensor_data_;
+    std::shared_ptr<SensorData<ObservationType, InfoType>> sensor_data_;
 };
 
 }  // namespace robot_interfaces