parse components

Signed-off-by: Karsten Knese <Karsten1987@users.noreply.github.com>

controller_manager/src/resource_manager.cpp

@@ -96,7 +96,9 @@ class ResourceStorage
     initialize_hardware<hardware_interface::components::Actuator,
       hardware_interface::components::ActuatorInterface>(
       hardware_info, actuator_loader_, actuators_);
-    actuators_.back().configure(hardware_info);
+    if (hardware_interface::return_type::OK != actuators_.back().configure(hardware_info)) {
+      throw std::runtime_error(std::string("failed to configure ") + hardware_info.name);
+    }
     import_state_handles(actuators_.back());
     import_command_handles(actuators_.back());
   }
@@ -170,6 +172,12 @@ std::vector<std::string> ResourceManager::state_handle_keys() const
   return keys;
 }
 
+bool ResourceManager::state_handle_exists(const std::string & key) const
+{
+  return resource_storage_->state_handle_map_.find(key) !=
+         resource_storage_->state_handle_map_.end();
+}
+
 std::vector<std::string> ResourceManager::command_handle_keys() const
 {
   std::vector<std::string> keys;
@@ -179,6 +187,12 @@ std::vector<std::string> ResourceManager::command_handle_keys() const
   return keys;
 }
 
+bool ResourceManager::command_handle_exists(const std::string & key) const
+{
+  return resource_storage_->command_handle_map_.find(key) !=
+         resource_storage_->command_handle_map_.end();
+}
+
 size_t ResourceManager::actuator_interfaces_size() const
 {
   return resource_storage_->actuators_.size();

controller_manager/src/resource_manager.hpp

@@ -35,14 +35,50 @@ class ResourceManager
 
   ~ResourceManager();
 
+  /// Returns all registered state handles keys.
+  /**
+   * The keys are collected from each loaded hardware component.
+   *
+   * \return vector of strings, containing all registered keys.
+   */
   std::vector<std::string> state_handle_keys() const;
 
+  /// Checks whether a handle is registered under the given key.
+  /**
+   * \return true if handle exist, false otherwise.
+   */
+  bool state_handle_exists(const std::string & key) const;
+
+  /// Returns all registered command handles keys.
+  /**
+   * The keys are collected from each loaded hardware component.
+   *
+   * \return vector of strings, containing all registered keys.
+   */
   std::vector<std::string> command_handle_keys() const;
 
+  /// Checks whether a handle is registered under the given key.
+  /**
+   * \return true if handle exist, false otherwise.
+   */
+  bool command_handle_exists(const std::string & key) const;
+
+  /// Return the number of loaded actuator components.
+  /**
+   * \return number of actuator components.
+   */
   size_t actuator_interfaces_size() const;
 
+  /// Return the number of loaded sensor components.
+  /**
+   * \return number of sensor components.
+   */
   size_t sensor_interfaces_size() const;
 
+  /// Return the number of loaded system components.
+  /**
+   * \return number of system components.
+   */
   size_t system_interfaces_size() const;
 
 private:

controller_manager/test/test_components/test_actuator.cpp

@@ -27,22 +27,29 @@ class TestActuator : public hardware_interface::components::ActuatorInterface
   return_type configure(const hardware_interface::HardwareInfo & actuator_info) override
   {
     actuator_info_ = actuator_info;
-    for (const auto & joint : actuator_info_.joints) {
-      fprintf(stderr, "joint info: %s\n", joint.name.c_str());
-      for (const auto & command_interface : joint.command_interfaces) {
-        fprintf(stderr, "\t%s\n", command_interface.name.c_str());
-      }
-    }
+    // can only control one joint
+    if (actuator_info_.joints.size() != 1) {return return_type::ERROR;}
+    // can only control in position
+    if (actuator_info_.joints[0].command_interfaces.size() != 1) {return return_type::ERROR;}
+    // can only give feedback state for position and velocity
+    if (actuator_info_.joints[0].state_interfaces.size() != 2) {return return_type::ERROR;}
+
     return return_type::OK;
   }
 
   std::vector<StateHandle> export_state_handles() override
   {
     std::vector<StateHandle> state_handles;
     state_handles.emplace_back(
-      hardware_interface::StateHandle("joint1", "position", &position_state_));
+      hardware_interface::StateHandle(
+        actuator_info_.joints[0].name,
+        actuator_info_.joints[0].state_interfaces[0].name,
+        &position_state_));
     state_handles.emplace_back(
-      hardware_interface::StateHandle("joint1", "velocity", &velocity_state_));
+      hardware_interface::StateHandle(
+        actuator_info_.joints[0].name,
+        actuator_info_.joints[0].state_interfaces[1].name,
+        &velocity_state_));
 
     return state_handles;
   }
@@ -51,7 +58,10 @@ class TestActuator : public hardware_interface::components::ActuatorInterface
   {
     std::vector<CommandHandle> command_handles;
     command_handles.emplace_back(
-      hardware_interface::CommandHandle("joint1", "velocity", &velocity_command_));
+      hardware_interface::CommandHandle(
+        actuator_info_.joints[0].name,
+        actuator_info_.joints[0].command_interfaces[0].name,
+        &velocity_command_));
 
     return command_handles;
   }

controller_manager/test/test_components/test_sensor.cpp

@@ -26,12 +26,19 @@ class TestSensor : public hardware_interface::components::SensorInterface
   return_type configure(const hardware_interface::HardwareInfo & sensor_info) override
   {
     sensor_info_ = sensor_info;
+    // can only give feedback state for velocity
+    if (sensor_info_.sensors[0].state_interfaces.size() != 1) {return return_type::ERROR;}
     return return_type::OK;
   }
 
   std::vector<StateHandle> export_state_handles() override
   {
     std::vector<StateHandle> state_handles;
+    state_handles.emplace_back(
+      hardware_interface::StateHandle(
+        sensor_info_.sensors[0].name,
+        sensor_info_.sensors[0].state_interfaces[0].name,
+        &velocity_state_));
 
     return state_handles;
   }
@@ -57,6 +64,7 @@ class TestSensor : public hardware_interface::components::SensorInterface
   }
 
 private:
+  double velocity_state_ = 0.0;
   hardware_interface::HardwareInfo sensor_info_;
 };
 

controller_manager/test/test_components/test_system.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <array>
 #include <memory>
 #include <vector>
 
@@ -33,13 +34,23 @@ class TestSystem : public hardware_interface::components::SystemInterface
   std::vector<StateHandle> export_state_handles() override
   {
     std::vector<StateHandle> state_handles;
+    for (auto i = 0u; i < system_info_.joints.size(); ++i) {
+      state_handles.emplace_back(
+        hardware_interface::StateHandle(
+          system_info_.joints[i].name, "position", &position_state_[i]));
+    }
 
     return state_handles;
   }
 
   std::vector<CommandHandle> export_command_handles() override
   {
     std::vector<CommandHandle> command_handles;
+    for (auto i = 0u; i < system_info_.joints.size(); ++i) {
+      command_handles.emplace_back(
+        hardware_interface::CommandHandle(
+          system_info_.joints[i].name, "velocity", &velocity_command_[i]));
+    }
 
     return command_handles;
   }
@@ -70,6 +81,8 @@ class TestSystem : public hardware_interface::components::SystemInterface
   }
 
 private:
+  std::array<double, 2> velocity_command_ = {0.0, 0.0};
+  std::array<double, 2> position_state_ = {0.0, 0.0};
   hardware_interface::HardwareInfo system_info_;
 };
 

controller_manager/test/test_resource_manager.cpp

@@ -90,15 +90,17 @@ class TestResourceManager : public ::testing::Test
       R"(
   <ros2_control name="TestActuatorHardware" type="actuator">
     <hardware>
-      <classType>test_actuator</classType>
+      <plugin>test_actuator</plugin>
     </hardware>
     <joint name="joint1">
       <command_interface name="position"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
     </joint>
   </ros2_control>
   <ros2_control name="TestSensorHardware" type="sensor">
     <hardware>
-      <classType>test_sensor</classType>
+      <plugin>test_sensor</plugin>
       <param name="example_param_write_for_sec">2</param>
       <param name="example_param_read_for_sec">2</param>
     </hardware>
@@ -108,7 +110,7 @@ class TestResourceManager : public ::testing::Test
   </ros2_control>
   <ros2_control name="TestSystemHardware" type="system">
     <hardware>
-      <classType>test_system</classType>
+      <plugin>test_system</plugin>
       <param name="example_param_write_for_sec">2</param>
       <param name="example_param_read_for_sec">2</param>
     </hardware>
@@ -145,17 +147,16 @@ TEST_F(TestResourceManager, initialization_with_urdf) {
   EXPECT_EQ(1u, rm.system_interfaces_size());
 
   auto state_handle_keys = rm.state_handle_keys();
-  // extracting a list from an unordered_map doesn't yield deterministic results
-  // sort the list to make comparison clear.
-  std::sort(state_handle_keys.begin(), state_handle_keys.end());
-  ASSERT_EQ(2u, state_handle_keys.size());
-  EXPECT_EQ("joint1/position", state_handle_keys[0]);
-  EXPECT_EQ("joint1/velocity", state_handle_keys[1]);
+  ASSERT_EQ(5u, state_handle_keys.size());
+  EXPECT_TRUE(rm.state_handle_exists("joint1/position"));
+  EXPECT_TRUE(rm.state_handle_exists("joint1/velocity"));
+  EXPECT_TRUE(rm.state_handle_exists("sensor1/velocity"));
+  EXPECT_TRUE(rm.state_handle_exists("joint2/position"));
+  EXPECT_TRUE(rm.state_handle_exists("joint3/position"));
 
   auto command_handle_keys = rm.command_handle_keys();
-  // extracting a list from an unordered_map doesn't yield deterministic results
-  // sort the list to make comparison clear.
-  std::sort(command_handle_keys.begin(), command_handle_keys.end());
-  ASSERT_EQ(1u, command_handle_keys.size());
-  EXPECT_EQ("joint1/velocity", command_handle_keys[0]);
+  ASSERT_EQ(3u, command_handle_keys.size());
+  EXPECT_TRUE(rm.command_handle_exists("joint1/position"));
+  EXPECT_TRUE(rm.command_handle_exists("joint2/velocity"));
+  EXPECT_TRUE(rm.command_handle_exists("joint3/velocity"));
 }

hardware_interface/src/component_parser.cpp

@@ -141,10 +141,10 @@ std::unordered_map<std::string, std::string> parse_parameters_from_xml(
  * \return std::vector< std::__cxx11::string > list of interface types
  * \throws std::runtime_error if the interfaceType text not set in a tag
  */
-components::InterfaceInfo parse_interfaces_from_xml(
+InterfaceInfo parse_interfaces_from_xml(
   const tinyxml2::XMLElement * interfaces_it)
 {
-  hardware_interface::components::InterfaceInfo interface;
+  InterfaceInfo interface;
 
   const std::string interface_name = get_attribute_value(
     interfaces_it, kNameAttribute, interfaces_it->Name());