Extended SensorInterface to work with hardware. Added Example for Dum…

…mySensor and HW in tests. Added lifecycle management for components ros-controls#107

hardware_interface/include/hardware_interface/component_interfaces/sensor_interface.hpp

@@ -19,6 +19,7 @@
 
 #include "hardware_interface/component_info.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_state_values.hpp"
 #include "hardware_interface/visibility_control.h"
 
 namespace hardware_interface
@@ -40,7 +41,11 @@ class SensorInterface
 
   HARDWARE_INTERFACE_PUBLIC
   virtual
-  std::string get_interface_name() const = 0;
+  return_type initalize(bool auto_start) = 0;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  return_type recover(bool auto_start) = 0;
 
   HARDWARE_INTERFACE_PUBLIC
   virtual
@@ -52,11 +57,19 @@ class SensorInterface
 
   HARDWARE_INTERFACE_PUBLIC
   virtual
-  bool is_started() const = 0;
+  return_type halt() = 0;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  component_state get_state() const = 0;
 
   HARDWARE_INTERFACE_PUBLIC
   virtual
   return_type read(double & data) = 0;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  std::string get_interface_name() const = 0;
 };
 
 }  // namespace hardware_interface

hardware_interface/include/hardware_interface/sensor.hpp

@@ -22,6 +22,7 @@
 
 #include "hardware_interface/component_info.hpp"
 #include "hardware_interface/component_interfaces/sensor_interface.hpp"
+#include "hardware_interface/types/hardware_interface_state_values.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/visibility_control.h"
 
@@ -33,18 +34,30 @@ class Sensor final
 public:
   Sensor() = default;
 
-  explicit Sensor(std::unique_ptr<SensorInterface> impl)
+  explicit Sensor(std::unique_ptr<SensorInterface> & impl)
   : impl_(std::move(impl))
   {}
 
-  virtual ~Sensor() = default;
+  ~Sensor() = default;
 
   HARDWARE_INTERFACE_PUBLIC
   return_type configure(const ComponentInfo & sensor_info)
   {
     return impl_->configure(sensor_info);
   }
 
+  HARDWARE_INTERFACE_PUBLIC
+  return_type initalize(bool auto_start)
+  {
+    return impl_->initalize(auto_start);
+  }
+
+  HARDWARE_INTERFACE_PUBLIC
+  return_type recover(bool auto_start)
+  {
+    return impl_->recover(auto_start);
+  }
+
   HARDWARE_INTERFACE_PUBLIC
   return_type start()
   {
@@ -57,6 +70,18 @@ class Sensor final
     return impl_->stop();
   }
 
+  HARDWARE_INTERFACE_PUBLIC
+  return_type halt()
+  {
+    return impl_->halt();
+  }
+
+  HARDWARE_INTERFACE_PUBLIC
+  component_state get_state()
+  {
+    return impl_->get_state();
+  }
+
   HARDWARE_INTERFACE_PUBLIC
   return_type read(double & data)
   {

hardware_interface/include/hardware_interface/types/hardware_interface_state_values.hpp

@@ -0,0 +1,34 @@
+// Copyright 2020 ros2_control Development Team
+//
+// 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 HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_STATE_VALUES_HPP_
+#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_STATE_VALUES_HPP_
+
+#include <cstdint>
+
+namespace hardware_interface
+{
+enum class component_state : std::uint8_t
+{
+  UNKNOWN = 0,
+  CONFIGURED = 1,
+  INITIALIZED = 2,
+  STARTED = 3,
+  STOPPED = 4,
+  HALTED = 5,
+};
+
+}  // namespace hardware_interface
+
+#endif  // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_STATE_VALUES_HPP_

hardware_interface/test/test_component_interfaces.cpp

@@ -15,6 +15,7 @@
 #include <gmock/gmock.h>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include "hardware_interface/component_info.hpp"
 #include "hardware_interface/component_interfaces/joint_interface.hpp"
 #include "hardware_interface/component_interfaces/sensor_interface.hpp"
@@ -23,6 +24,7 @@
 #include "hardware_interface/sensor.hpp"
 #include "hardware_interface/system.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_state_values.hpp"
 
 namespace hardware_interface
 {
@@ -85,8 +87,152 @@ class DummyJoint : public JointInterface
   bool started;
 };
 
+class DummySensorHardware
+{
+public:
+  DummySensorHardware() = default;
+
+  ~DummySensorHardware() = default;
+
+  return_type configure_hardware(std::unordered_map<std::string, std::string> & parameters)
+  {
+    if (parameters.count("start_value") > 0) {
+      value = stoi(parameters["start_value"]);
+      return return_type::OK;
+    }
+    return return_type::ERROR;
+  }
+
+  return_type initalize_hardware(bool auto_start)
+  {
+    (void) auto_start;
+    return return_type::OK;
+  }
+
+  return_type recover_hardware(bool auto_start)
+  {
+    (void) auto_start;
+    return return_type::OK;
+  }
+
+  return_type start_hardware()
+  {
+    return return_type::OK;
+  }
+
+  return_type stop_hardware()
+  {
+    return return_type::OK;
+  }
+
+  return_type halt_hardware()
+  {
+    return return_type::OK;
+  }
+
+  return_type read_from_hardware(int & data)
+  {
+    data = value + 128;
+    return return_type::OK;
+  }
+
+private:
+  int value = 0;
+};
+
 class DummySensor : public SensorInterface
 {
+  return_type configure(const ComponentInfo & sensor_info) override
+  {
+    return_type ret;
+    info = sensor_info;
+    binary_to_voltage_factor = stod(info.parameters["binary_to_voltage_factor"]);
+
+    // Normaly crate dynamically SensorHardware Object from sensor hardware_class_type
+    if (hardware.configure_hardware(info.hardware_parameters) == return_type::OK) {
+      state = component_state::CONFIGURED;
+      ret = return_type::OK;
+    } else {
+      state = component_state::UNKNOWN;
+      ret = return_type::ERROR;
+    }
+    return ret;
+  }
+
+  std::string get_interface_name() const override
+  {
+    return info.interface_names.front();
+  }
+
+  return_type initalize(bool auto_start) override
+  {
+    if (hardware.initalize_hardware(auto_start) == return_type::OK) {
+      if (auto_start) {
+        start();
+      } else {
+        state = component_state::INITIALIZED;
+      }
+    }
+    return return_type::OK;
+  }
+
+  return_type recover(bool auto_start) override
+  {
+    if (hardware.recover_hardware(auto_start) == return_type::OK) {
+      if (auto_start) {
+        start();
+      } else {
+        state = component_state::INITIALIZED;
+      }
+    }
+    return return_type::OK;
+  }
+
+  return_type start() override
+  {
+    if (hardware.start_hardware() == return_type::OK) {
+      state = component_state::STARTED;
+    }
+    return return_type::OK;
+  }
+
+  return_type stop() override
+  {
+    if (hardware.stop_hardware() == return_type::OK) {
+      state = component_state::STOPPED;
+    }
+    return return_type::OK;
+  }
+
+  return_type halt() override
+  {
+    if (hardware.halt_hardware() == return_type::OK) {
+      state = component_state::HALTED;
+    }
+    return return_type::OK;
+  }
+
+  component_state get_state() const override
+  {
+    return state;
+  }
+
+  return_type read(double & data) override
+  {
+    int hw_data;
+    hardware.read_from_hardware(hw_data);
+    data = hw_data * binary_to_voltage_factor;
+    return return_type::OK;
+  }
+
+private:
+  ComponentInfo info;
+
+  component_state state = component_state::UNKNOWN;
+
+  DummySensorHardware hardware;
+
+  double binary_to_voltage_factor;
 };
 
 class DummySystem : public SystemInterface
@@ -96,11 +242,16 @@ class DummySystem : public SystemInterface
 }  // namespace component_interfaces_test
 }  // namespace hardware_interface
 
+using hardware_interface::return_type;
 using hardware_interface::ComponentInfo;
 using hardware_interface::Joint;
 using hardware_interface::JointInterface;
+using hardware_interface::Sensor;
+using hardware_interface::SensorInterface;
+
 using hardware_interface::component_interfaces_test::DummyJoint;
-using hardware_interface::return_type;
+using hardware_interface::component_interfaces_test::DummySensor;
+using hardware_interface::component_state;
 
 TEST(TestJointInterface, joint_interfce_works)
 {
@@ -113,7 +264,7 @@ TEST(TestJointInterface, joint_interfce_works)
   joint_info.parameters["min_value"] = "-1";
   joint_info.parameters["max_value"] = "1";
 
-  joint.configure(joint_info);
+  EXPECT_EQ(joint.configure(joint_info), return_type::OK);
   EXPECT_EQ(joint.get_interface_name(), "dummy");
   EXPECT_EQ(joint.is_started(), false);
   EXPECT_EQ(joint.start(), return_type::OK);
@@ -125,3 +276,41 @@ TEST(TestJointInterface, joint_interfce_works)
   EXPECT_EQ(joint.read(output), return_type::OK);
   EXPECT_DOUBLE_EQ(output, 0.5);
 }
+
+TEST(TestSensorInterfaceWithHardware, sensor_interface_with_hardware_works)
+{
+  std::unique_ptr<SensorInterface> sensor_interface(new DummySensor);
+  Sensor sensor(sensor_interface);
+
+  ComponentInfo sensor_info;
+  sensor_info.name = "DummySensor";
+  sensor_info.interface_names.push_back("dummyS");
+  sensor_info.parameters["binary_to_voltage_factor"] = "0.0048828125";
+  sensor_info.hardware_class_type = "DummySensorHardware";
+
+  EXPECT_EQ(sensor.configure(sensor_info), return_type::ERROR);
+  EXPECT_EQ(sensor.get_state(), component_state::UNKNOWN);
+
+  sensor_info.hardware_parameters["start_value"] = "128";
+
+  EXPECT_EQ(sensor.configure(sensor_info), return_type::OK);
+  EXPECT_EQ(sensor.get_state(), component_state::CONFIGURED);
+  EXPECT_EQ(sensor.get_interface_name(), "dummyS");
+  EXPECT_EQ(sensor.initalize(false), return_type::OK);
+  EXPECT_EQ(sensor.get_state(), component_state::INITIALIZED);
+  EXPECT_EQ(sensor.initalize(true), return_type::OK);
+  EXPECT_EQ(sensor.get_state(), component_state::STARTED);
+  EXPECT_EQ(sensor.stop(), return_type::OK);
+  EXPECT_EQ(sensor.get_state(), component_state::STOPPED);
+  EXPECT_EQ(sensor.start(), return_type::OK);
+  EXPECT_EQ(sensor.get_state(), component_state::STARTED);
+  EXPECT_EQ(sensor.halt(), return_type::OK);
+  EXPECT_EQ(sensor.get_state(), component_state::HALTED);
+  EXPECT_EQ(sensor.recover(false), return_type::OK);
+  EXPECT_EQ(sensor.get_state(), component_state::INITIALIZED);
+  EXPECT_EQ(sensor.start(), return_type::OK);
+  EXPECT_EQ(sensor.get_state(), component_state::STARTED);
+  double output;
+  EXPECT_EQ(sensor.read(output), return_type::OK);
+  EXPECT_DOUBLE_EQ(output, 0.0048828125 * (128 + 128));
+}