Merge pull request #133 from open-dynamic-robot-initiative/fkloss/sen…

…sor_info

Add optional sensor_info to sensor interface

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

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.

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

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")

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_;
 

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);
         }
     }
 };

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

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