Added starting of resources into CM and RM

controller_manager/src/controller_manager.cpp

@@ -71,6 +71,9 @@ ControllerManager::ControllerManager(
 
   resource_manager_->load_urdf(robot_description);
 
+  // TODO(all): Here we should start only "auto-start" resources
+  resource_manager_->start_components();
+
   init_services();
 }
 

hardware_interface/include/hardware_interface/components/actuator.hpp

@@ -16,6 +16,7 @@
 #define HARDWARE_INTERFACE__COMPONENTS__ACTUATOR_HPP_
 
 #include <memory>
+#include <string>
 #include <vector>
 
 #include "hardware_interface/handle.hpp"
@@ -58,6 +59,9 @@ class Actuator final
   HARDWARE_INTERFACE_PUBLIC
   return_type stop();
 
+  HARDWARE_INTERFACE_PUBLIC
+  std::string get_name() const;
+
   HARDWARE_INTERFACE_PUBLIC
   status get_status() const;
 

hardware_interface/include/hardware_interface/components/actuator_interface.hpp

@@ -16,6 +16,7 @@
 #define HARDWARE_INTERFACE__COMPONENTS__ACTUATOR_INTERFACE_HPP_
 
 #include <memory>
+#include <string>
 #include <vector>
 
 #include "hardware_interface/handle.hpp"
@@ -91,7 +92,15 @@ class ActuatorInterface
   return_type stop() = 0;
 
   /**
-   * \brief Get current state of the system hardware.
+   * \brief Get name of the actuator hardware.
+   *
+   * \return std::string name.
+   */
+  virtual
+  std::string get_name() const = 0;
+
+  /**
+   * \brief Get current state of the actuator hardware.
    *
    * \return status current status.
    */

hardware_interface/include/hardware_interface/components/base_interface.hpp

@@ -0,0 +1,63 @@
+// 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__COMPONENTS__BASE_INTERFACE_HPP_
+#define HARDWARE_INTERFACE__COMPONENTS__BASE_INTERFACE_HPP_
+
+#include <string>
+
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_status_values.hpp"
+
+namespace hardware_interface
+{
+namespace components
+{
+
+template<class InterfaceType>
+class BaseInterface : public InterfaceType
+{
+public:
+  return_type configure(const HardwareInfo & info) override
+  {
+    info_ = info;
+    status_ = status::CONFIGURED;
+    return return_type::OK;
+  }
+
+  return_type configure_default(const HardwareInfo & info)
+  {
+    return BaseInterface<InterfaceType>::configure(info);
+  }
+
+  std::string get_name() const final
+  {
+    return info_.name;
+  }
+
+  status get_status() const final
+  {
+    return status_;
+  }
+
+protected:
+  HardwareInfo info_;
+  status status_;
+};
+
+}  // namespace components
+}  // namespace hardware_interface
+
+#endif  // HARDWARE_INTERFACE__COMPONENTS__BASE_INTERFACE_HPP_

hardware_interface/include/hardware_interface/components/sensor.hpp

@@ -57,6 +57,9 @@ class Sensor final
   HARDWARE_INTERFACE_PUBLIC
   return_type stop();
 
+  HARDWARE_INTERFACE_PUBLIC
+  std::string get_name() const;
+
   HARDWARE_INTERFACE_PUBLIC
   status get_status() const;
 

hardware_interface/include/hardware_interface/components/sensor_interface.hpp

@@ -16,6 +16,7 @@
 #define HARDWARE_INTERFACE__COMPONENTS__SENSOR_INTERFACE_HPP_
 
 #include <memory>
+#include <string>
 #include <vector>
 
 #include "hardware_interface/handle.hpp"
@@ -80,7 +81,15 @@ class SensorInterface
   return_type stop() = 0;
 
   /**
-   * \brief Get current state of the system hardware.
+   * \brief Get name of the sensor hardware.
+   *
+   * \return std::string name.
+   */
+  virtual
+  std::string get_name() const = 0;
+
+  /**
+   * \brief Get current state of the sensor hardware.
    *
    * \return status current status.
    */

hardware_interface/include/hardware_interface/components/system.hpp

@@ -58,6 +58,9 @@ class System final
   HARDWARE_INTERFACE_PUBLIC
   return_type stop();
 
+  HARDWARE_INTERFACE_PUBLIC
+  std::string get_name() const;
+
   HARDWARE_INTERFACE_PUBLIC
   status get_status() const;
 

hardware_interface/include/hardware_interface/components/system_interface.hpp

@@ -16,6 +16,7 @@
 #define HARDWARE_INTERFACE__COMPONENTS__SYSTEM_INTERFACE_HPP_
 
 #include <memory>
+#include <string>
 #include <vector>
 
 #include "hardware_interface/handle.hpp"
@@ -93,6 +94,14 @@ class SystemInterface
   virtual
   return_type stop() = 0;
 
+  /**
+   * \brief Get name of the system hardware.
+   *
+   * \return std::string name.
+   */
+  virtual
+  std::string get_name() const = 0;
+
   /**
    * \brief Get current state of the system hardware.
    *

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -24,6 +24,7 @@
 #include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/types/hardware_interface_status_values.hpp"
 
 namespace hardware_interface
 {
@@ -190,6 +191,18 @@ class ResourceManager
    */
   void import_component(std::unique_ptr<components::SystemInterface> system);
 
+  /// Return status for all components.
+  /**
+   * \return map of hardware names and their status
+   */
+  std::unordered_map<std::string, status> get_components_status();
+
+  /// Start all loaded hardware components.
+  void start_components();
+
+  /// Stops all loaded hardware components.
+  void stop_components();
+
   /// Reads all loaded hardware components.
   void read();
 

hardware_interface/src/components/actuator.cpp

@@ -62,6 +62,11 @@ return_type Actuator::stop()
   return impl_->stop();
 }
 
+std::string Actuator::get_name() const
+{
+  return impl_->get_name();
+}
+
 status Actuator::get_status() const
 {
   return impl_->get_status();

hardware_interface/src/components/sensor.cpp

@@ -54,6 +54,11 @@ return_type Sensor::stop()
   return impl_->stop();
 }
 
+std::string Sensor::get_name() const
+{
+  return impl_->get_name();
+}
+
 status Sensor::get_status() const
 {
   return impl_->get_status();

hardware_interface/src/components/system.cpp

@@ -59,6 +59,11 @@ return_type System::stop()
   return impl_->stop();
 }
 
+std::string System::get_name() const
+{
+  return impl_->get_name();
+}
+
 status System::get_status() const
 {
   return impl_->get_status();

hardware_interface/src/resource_manager.cpp

@@ -62,8 +62,10 @@ class ResourceStorage
     // e.g.: <package_vendor>/<system_type>
     auto interface = std::unique_ptr<HardwareInterfaceT>(
       loader.createUnmanagedInstance(hardware_info.hardware_class_type));
-    HardwareT actuator(std::move(interface));
-    container.emplace_back(std::move(actuator));
+    HardwareT hardware(std::move(interface));
+    container.emplace_back(std::move(hardware));
+    hardware_status_map_.emplace(
+      std::make_pair(container.back().get_name(), container.back().get_status()));
   }
 
   template<class HardwareT>
@@ -137,6 +139,8 @@ class ResourceStorage
   std::vector<components::Sensor> sensors_;
   std::vector<components::System> systems_;
 
+  std::unordered_map<std::string, status> hardware_status_map_;
+
   std::unordered_map<std::string, StateInterface> state_interface_map_;
   std::unordered_map<std::string, CommandInterface> command_interface_map_;
 };
@@ -291,6 +295,47 @@ size_t ResourceManager::system_components_size() const
   return resource_storage_->systems_.size();
 }
 
+std::unordered_map<std::string, status> ResourceManager::get_components_status()
+{
+  for (auto & component : resource_storage_->actuators_) {
+    resource_storage_->hardware_status_map_[component.get_name()] = component.get_status();
+  }
+  for (auto & component : resource_storage_->sensors_) {
+    resource_storage_->hardware_status_map_[component.get_name()] = component.get_status();
+  }
+  for (auto & component : resource_storage_->systems_) {
+    resource_storage_->hardware_status_map_[component.get_name()] = component.get_status();
+  }
+
+  return resource_storage_->hardware_status_map_;
+}
+
+void ResourceManager::start_components()
+{
+  for (auto & component : resource_storage_->actuators_) {
+    component.start();
+  }
+  for (auto & component : resource_storage_->sensors_) {
+    component.start();
+  }
+  for (auto & component : resource_storage_->systems_) {
+    component.start();
+  }
+}
+
+void ResourceManager::stop_components()
+{
+  for (auto & component : resource_storage_->actuators_) {
+    component.stop();
+  }
+  for (auto & component : resource_storage_->sensors_) {
+    component.stop();
+  }
+  for (auto & component : resource_storage_->systems_) {
+    component.stop();
+  }
+}
+
 void ResourceManager::read()
 {
   for (auto & component : resource_storage_->actuators_) {

hardware_interface/test/test_component_interfaces.cpp

@@ -80,6 +80,11 @@ class DummyActuator : public hardware_interface::components::ActuatorInterface
     return hardware_interface::return_type::OK;
   }
 
+  std::string get_name() const override
+  {
+    return "DummyActuator";
+  }
+
   hardware_interface::status get_status() const override
   {
     return hardware_interface::status::UNKNOWN;
@@ -134,6 +139,11 @@ class DummySensor : public hardware_interface::components::SensorInterface
     return hardware_interface::return_type::OK;
   }
 
+  std::string get_name() const override
+  {
+    return "DummySensor";
+  }
+
   hardware_interface::status get_status() const override
   {
     return hardware_interface::status::UNKNOWN;
@@ -211,6 +221,11 @@ class DummySystem : public hardware_interface::components::SystemInterface
     return hardware_interface::return_type::OK;
   }
 
+  std::string get_name() const override
+  {
+    return "DummySystem";
+  }
+
   hardware_interface::status get_status() const override
   {
     return hardware_interface::status::UNKNOWN;