WIP: Implementation of components for controlling robots

hardware_interface/CMakeLists.txt

@@ -12,8 +12,11 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(control_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rcpputils REQUIRED)
+find_package(tinyxml2_vendor REQUIRED)
+find_package(TinyXML2 REQUIRED)
 
 add_library(
   hardware_interface
@@ -26,26 +29,41 @@ add_library(
 target_include_directories(
   hardware_interface
   PUBLIC
-  include)
+  include
+)
 ament_target_dependencies(
   hardware_interface
   control_msgs
   rclcpp
   rcpputils
 )
-
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL")
 
+add_library(
+  component_parser
+  src/utils/component_parser.cpp
+)
+target_include_directories(
+  component_parser
+  PUBLIC
+  include
+)
+ament_target_dependencies(
+  component_parser
+  TinyXML2
+)
+
+target_compile_definitions(component_parser PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL")
+
 install(
   DIRECTORY include/
   DESTINATION include
 )
 
 install(
-  TARGETS
-  hardware_interface
+  TARGETS hardware_interface component_parser
   RUNTIME DESTINATION bin
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib)
@@ -64,6 +82,11 @@ if(BUILD_TESTING)
   target_include_directories(test_robot_hardware_interfaces PRIVATE include)
   target_link_libraries(test_robot_hardware_interfaces hardware_interface)
 
+
+  ament_add_gmock(test_component_parser test/test_component_parser.cpp)
+  target_link_libraries(test_component_parser component_parser)
+  ament_target_dependencies(test_component_parser TinyXML2)
+
   ament_add_gmock(test_register_actuators test/test_register_actuators.cpp)
   target_include_directories(test_register_actuators PRIVATE include)
   target_link_libraries(test_register_actuators hardware_interface)
@@ -73,14 +96,19 @@ if(BUILD_TESTING)
   target_include_directories(test_actuator_handle PRIVATE include)
   target_link_libraries(test_actuator_handle hardware_interface)
   ament_target_dependencies(test_actuator_handle rcpputils)
-
 endif()
 
+ament_export_dependencies(
+  rclcpp
+  rcpputils
+)
 ament_export_include_directories(
   include
 )
 ament_export_libraries(
-  hardware_interface)
+  hardware_interface
+  component_parser
+)
 ament_export_dependencies(
   control_msgs)
 ament_package()

hardware_interface/include/hardware_interface/component_info.hpp

@@ -0,0 +1,103 @@
+// 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__COMPONENT_INFO_HPP_
+#define HARDWARE_INTERFACE__COMPONENT_INFO_HPP_
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace hardware_interface
+{
+
+/**
+  * \brief constants for types of components.
+  */
+constexpr const auto robotType = "robot";
+constexpr const auto actuatorType = "actuator";
+constexpr const auto sensorType = "sensor";
+
+/**
+ * \brief This structure stores information about components defined in a robot's URDF.
+ */
+struct ComponentInfo
+{
+  /**
+   * \brief name of the component.
+   */
+  std::string name;
+  /**
+   * \brief type of the component: sensor or actuator.
+   */
+  std::string type;
+  /**
+   * \brief component's class, which will be dynamically loaded.
+   */
+  std::string class_type;
+  /**
+   * \brief joint where component is placed.
+   */
+  std::string joint;
+  /**
+   * \brief name of the interface, e.g. "position", "velocity", etc. for meaning of data this component holds.
+   */
+  std::vector<std::string> interface_names;
+  /**
+   * \brief (optional) key-value pairs of components parameters.
+   */
+  std::unordered_map<std::string, std::string> parameters;
+
+  /**
+   * \brief (optional) hardware class of the component that will be dynamically loaded. If not defined, the system's hardware class has to be defined.
+   */
+  std::string hardware_class_type;
+  /**
+   * \brief (optional) key-value pairs for components hardware.
+   */
+  std::unordered_map<std::string, std::string> hardware_parameters;
+};
+
+/**
+ * \brief This structure stores informations about system defined in robot's URDF, i.e. "ros2_control"-tag.
+ */
+struct SystemInfo
+{
+  /**
+   * \brief name of the system.
+   */
+  std::string name;
+  /**
+   * \brief type of the system: robot, actuator or sensor. Note: URDF always needs a "robot" tag, nevertheless in terms of ros2_control, it can hold a definition for an actuator or sensor.
+   */
+  std::string type;
+  /**
+   * \brief (optional) hardware class of the system, which will be dynamically loaded. If not defined, a hardware class for each subcomponent has to be defined.
+   */
+  std::string hardware_class_type;
+  /**
+   * \brief (optional) key-value pairs for systems hardware.
+   */
+  std::unordered_map<std::string, std::string> hardware_parameters;
+
+  /**
+   * \brief list of subcomponents in the system, i.e., list of sensors and actuators.
+   */
+  std::vector<ComponentInfo> subcomponents;
+};
+
+}  // namespace hardware_interface
+
+#endif  // HARDWARE_INTERFACE__COMPONENT_INFO_HPP_

hardware_interface/include/hardware_interface/component_interfaces/joint_interface.hpp

@@ -0,0 +1,70 @@
+// 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__COMPONENT_INTERFACES__JOINT_INTERFACE_HPP_
+#define HARDWARE_INTERFACE__COMPONENT_INTERFACES__JOINT_INTERFACE_HPP_
+
+#include <algorithm>
+#include <array>
+#include <string>
+#include <vector>
+
+#include "hardware_interface/component_info.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/visibility_control.h"
+
+namespace hardware_interface
+{
+
+class JointInterface
+{
+public:
+  HARDWARE_INTERFACE_PUBLIC
+  JointInterface() = default;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  ~JointInterface() = default;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  return_type configure(const ComponentInfo & actuator_info) = 0;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  std::string get_interface_name() const = 0;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  return_type start() = 0;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  return_type stop() = 0;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  bool is_started() const = 0;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  return_type read(double & data) = 0;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  return_type write(const double & data) = 0;
+};
+
+}  // namespace hardware_interface
+#endif  // HARDWARE_INTERFACE__COMPONENT_INTERFACES__JOINT_INTERFACE_HPP_

hardware_interface/include/hardware_interface/component_interfaces/sensor_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__COMPONENT_INTERFACES__SENSOR_INTERFACE_HPP_
+#define HARDWARE_INTERFACE__COMPONENT_INTERFACES__SENSOR_INTERFACE_HPP_
+
+#include <string>
+
+#include "hardware_interface/component_info.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/visibility_control.h"
+
+namespace hardware_interface
+{
+
+class SensorInterface
+{
+public:
+  HARDWARE_INTERFACE_PUBLIC
+  SensorInterface() = default;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  ~SensorInterface() = default;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  return_type configure(const ComponentInfo & sensor_info) = 0;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  std::string get_interface_name() const = 0;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  return_type start() = 0;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  return_type stop() = 0;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  bool is_started() const = 0;
+
+  HARDWARE_INTERFACE_PUBLIC
+  virtual
+  return_type read(double & data) = 0;
+};
+
+}  // namespace hardware_interface
+#endif  // HARDWARE_INTERFACE__COMPONENT_INTERFACES__SENSOR_INTERFACE_HPP_