ROS2 Control Components - PositionJoint

hardware_interface/src/components/component_lists_management.hpp → hardware_interface/include/hardware_interface/components/component_lists_management.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_
-#define COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_
+#ifndef HARDWARE_INTERFACE__COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_
+#define HARDWARE_INTERFACE__COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_
 
 #include <algorithm>
 #include <string>
@@ -118,6 +118,58 @@ inline return_type set_internal_values(
   return return_type::OK;
 }
 
+/**
+ * \brief set values for queried_interfaces to the int_values considering the value limits.
+ * int_values, lower_limits and upper_limits data structure matches int_interfaces vector.
+ *
+ * \param values values to set.
+ * \param queried_interfaces interfaces for which values are queried.
+ * \param int_interfaces full list of interfaces of a component.
+ * \param int_values internal values of a component.
+ * \param lower_limits list of lower limits.
+ * \param upper_limits list of upper limits.
+ * \return return return_type::INTERFACE_NOT_PROVIDED if
+ * queried_interfaces list is is empty; return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if values and
+ * queried_interfaces arguments do not have the same length; return_type::VALUE_OUT_OF_LIMITS if a
+ * value is not in the limits; return_type::INTERFACE_NOT_FOUND if
+ * one of queried_interfaces is not defined in int_interfaces; return_type::OK otherwise.
+ *
+ * \todo The error handling in this function could lead to incosistant command or state variables
+ * for different interfaces. This should be changed in the future.
+ * (see: https://github.com/ros-controls/ros2_control/issues/129)
+ */
+inline return_type set_internal_values_with_limits(
+  const std::vector<double> & values, const std::vector<std::string> & queried_interfaces,
+  const std::vector<std::string> & int_interfaces, std::vector<double> & int_values,
+  const std::vector<double> & lower_limits, const std::vector<double> & upper_limits)
+{
+  if (queried_interfaces.size() == 0) {
+    return return_type::INTERFACE_NOT_PROVIDED;
+  }
+  if (values.size() != queried_interfaces.size()) {
+    return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL;
+  }
+
+  for (auto q_it = queried_interfaces.begin(); q_it != queried_interfaces.end(); ++q_it) {
+    auto it = std::find(int_interfaces.begin(), int_interfaces.end(), *q_it);
+    if (it != int_interfaces.end()) {
+      if (values[std::distance(queried_interfaces.begin(), q_it)] >=
+        lower_limits[std::distance(int_interfaces.begin(), it)] &&
+        values[std::distance(queried_interfaces.begin(), q_it)] <=
+        upper_limits[std::distance(int_interfaces.begin(), it)])
+      {
+        int_values[std::distance(int_interfaces.begin(), it)] =
+          values[std::distance(queried_interfaces.begin(), q_it)];
+      } else {
+        return return_type::VALUE_OUT_OF_LIMITS;
+      }
+    } else {
+      return return_type::INTERFACE_NOT_FOUND;
+    }
+  }
+  return return_type::OK;
+}
+
 /**
  * \brief set all values to compoenents internal values.
  *
@@ -137,6 +189,35 @@ inline return_type set_internal_values(
   return return_type::OK;
 }
 
+/**
+ * \brief set all values to compoenents internal values considering limits.
+ * int_values, lower_limits and upper_limits have the same data structure.
+ *
+ * \param values values to set.
+ * \param int_values internal values of a component.
+ * \param lower_limits list of lower limits.
+ * \param upper_limits list of upper limits.
+ * \return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL if the size of the arguments is not equal;
+ * return_type::VALUE_OUT_OF_LIMITS if a value is not in the limits; return_type::OK otherwise.
+ */
+inline return_type set_internal_values_with_limits(
+  const std::vector<double> & values, std::vector<double> & int_values,
+  const std::vector<double> & lower_limits, const std::vector<double> & upper_limits)
+{
+  if (values.size() == int_values.size()) {
+    for (uint i = 0; i < int_values.size(); i++) {
+      if (values[i] >= lower_limits[i] && values[i] <= upper_limits[i]) {
+        int_values[i] = values[i];
+      } else {
+        return return_type::VALUE_OUT_OF_LIMITS;
+      }
+    }
+  } else {
+    return return_type::INTERFACE_VALUE_SIZE_NOT_EQUAL;
+  }
+  return return_type::OK;
+}
+
 }  // namespace components
 }  // namespace hardware_interface
-#endif  // COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_
+#endif  // HARDWARE_INTERFACE__COMPONENTS__COMPONENT_LISTS_MANAGEMENT_HPP_

hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp

@@ -36,7 +36,12 @@ enum class return_type : std::uint8_t
   INTERFACE_VALUE_SIZE_NOT_EQUAL = 31,
   INTERFACE_NOT_PROVIDED = 32,
 
-  COMMAND_OUT_OF_LIMITS = 40,
+  VALUE_OUT_OF_LIMITS = 40,
+
+  COMPONENT_TOO_MANY_INTERFACES = 51,
+  COMPONENT_WRONG_INTERFACE = 52,
+  COMPONENT_ONLY_STATE_DEFINED = 53,
+  COMPONENT_MISSING_PARAMETER = 54,
 };
 
 using hardware_interface_ret_t = return_type;

hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp

@@ -17,8 +17,17 @@
 
 namespace hardware_interface
 {
+/**
+  * Constant defining position interface
+  */
 constexpr const auto HW_IF_POSITION = "position";
+/**
+ * Constant defining velocity interface
+ */
 constexpr const auto HW_IF_VELOCITY = "velocity";
+/**
+ * Constant defining effort interface
+ */
 constexpr const auto HW_IF_EFFORT = "effort";
 }  // namespace hardware_interface
 

hardware_interface/src/components/joint.cpp

@@ -17,10 +17,9 @@
 
 #include "hardware_interface/components/joint.hpp"
 #include "hardware_interface/components/component_info.hpp"
+#include "hardware_interface/components/component_lists_management.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 
-#include "./component_lists_management.hpp"
-
 namespace hardware_interface
 {
 namespace components

hardware_interface/src/components/sensor.cpp

@@ -17,10 +17,9 @@
 
 #include "hardware_interface/components/sensor.hpp"
 #include "hardware_interface/components/component_info.hpp"
+#include "hardware_interface/components/component_lists_management.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 
-#include "./component_lists_management.hpp"
-
 namespace hardware_interface
 {
 namespace components

hardware_interface/test/test_component_interfaces.cpp

@@ -43,7 +43,7 @@ namespace hardware_interfaces_components_test
 class DummyPositionJoint : public components::Joint
 {
 public:
-  return_type configure(const components::ComponentInfo & joint_info)
+  return_type configure(const components::ComponentInfo & joint_info) override
   {
     if (Joint::configure(joint_info) != return_type::OK) {
       return return_type::ERROR;
@@ -73,7 +73,7 @@ class DummyPositionJoint : public components::Joint
 class DummyMultiJoint : public components::Joint
 {
 public:
-  return_type configure(const components::ComponentInfo & joint_info)
+  return_type configure(const components::ComponentInfo & joint_info) override
   {
     if (Joint::configure(joint_info) != return_type::OK) {
       return return_type::ERROR;
@@ -98,7 +98,7 @@ class DummyMultiJoint : public components::Joint
 class DummyForceTorqueSensor : public components::Sensor
 {
 public:
-  return_type configure(const components::ComponentInfo & sensor_info)
+  return_type configure(const components::ComponentInfo & sensor_info) override
   {
     if (Sensor::configure(sensor_info) != return_type::OK) {
       return return_type::ERROR;

ros2_control/package.xml

@@ -14,6 +14,7 @@
   <exec_depend>controller_interface</exec_depend>
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>hardware_interface</exec_depend>
+  <exec_depend>ros2_control_components</exec_depend>
   <exec_depend>transmission_interface</exec_depend>
 
   <test_depend>test_robot_hardware</test_depend>

ros2_control_components/CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 3.5)
+project(ros2_control_components)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+
+add_library(
+  ros2_control_components
+  SHARED
+  # joints
+  src/position_joint.cpp
+  # sensors
+)
+ament_target_dependencies(
+  ros2_control_components
+  hardware_interface
+  pluginlib
+)
+# prevent pluginlib from using boost
+target_compile_definitions(ros2_control_components PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+pluginlib_export_plugin_description_file(
+  hardware_interface ros2_control_components_plugins.xml)
+
+install(TARGETS
+  ros2_control_components
+  DESTINATION lib
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gmock REQUIRED)
+
+  ament_add_gmock(test_position_joint test/test_position_joint.cpp)
+  ament_target_dependencies(test_position_joint hardware_interface pluginlib)
+  # prevent pluginlib from using boost
+  target_compile_definitions(test_position_joint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+endif()
+
+ament_export_libraries(
+  ros2_control_components
+)
+ament_export_dependencies(
+  control_components
+  hardware_interface
+  pluginlib
+)
+ament_package()

ros2_control_components/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ros2_control_components</name>
+  <version>0.0.2</version>
+  <description>The package implements control components, i.e., joints and sensors, the logical building blocks of ros2_control system.
+  </description>
+  <maintainer email="denis@stogl.de">Denis Štogl</maintainer>
+  <license>Apache License 2.0</license>
+
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

ros2_control_components/ros2_control_components_plugins.xml

@@ -0,0 +1,7 @@
+<library path="ros2_control_components">
+  <class name="ros2_control_components/PositionJoint" type="ros2_control_components::PositionJoint" base_class_type="hardware_interface::components::Joint">
+    <description>
+      The position joint provides functionality of position-only joint with minimum and maximum limits.
+    </description>
+  </class>
+</library>

ros2_control_components/src/position_joint.cpp

@@ -0,0 +1,106 @@
+// 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.
+
+#include <string>
+#include <vector>
+
+#include "hardware_interface/components/joint.hpp"
+#include "hardware_interface/components/component_lists_management.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+
+using hardware_interface::components::ComponentInfo;
+using hardware_interface::components::Joint;
+using hardware_interface::return_type;
+
+namespace ros2_control_components
+{
+
+class PositionJoint : public Joint
+{
+public:
+  return_type configure(const ComponentInfo & joint_info) override
+  {
+    if (Joint::configure(joint_info) != return_type::OK) {
+      return return_type::ERROR;
+    }
+
+    // has to provide exactly one command interface if defined
+    if (info_.command_interfaces.size() > 1 || info_.state_interfaces.size() > 1) {
+      return return_type::COMPONENT_TOO_MANY_INTERFACES;
+    }
+
+    if (info_.command_interfaces.size() == 1 &&
+      info_.command_interfaces[0].compare(hardware_interface::HW_IF_POSITION))
+    {
+      return return_type::COMPONENT_WRONG_INTERFACE;
+    }
+
+    if (info_.state_interfaces.size() == 1 &&
+      info_.state_interfaces[0].compare(hardware_interface::HW_IF_POSITION))
+    {
+      return return_type::COMPONENT_WRONG_INTERFACE;
+    }
+
+    // set default values is not interface defined in URDF.
+    // Set state interface to default only if command interface is also not defined, otherwise
+    // return error code.
+    if (info_.command_interfaces.size() == 0) {
+      info_.command_interfaces = {hardware_interface::HW_IF_POSITION};
+      commands_.resize(1);
+      if (info_.state_interfaces.size() == 0) {
+        info_.state_interfaces = {hardware_interface::HW_IF_POSITION};
+        states_.resize(1);
+      } else {
+        return return_type::COMPONENT_ONLY_STATE_DEFINED;
+      }
+    }
+
+    if (info_.parameters.find("min") == info_.parameters.end()) {
+      return return_type::COMPONENT_MISSING_PARAMETER;
+    }
+    if (info_.parameters.find("max") == info_.parameters.end()) {
+      return return_type::COMPONENT_MISSING_PARAMETER;
+    }
+    lower_limits_.resize(1);
+    lower_limits_[0] = stod(info_.parameters["min"]);
+    upper_limits_.resize(1);
+    upper_limits_[0] = stod(info_.parameters["max"]);
+
+    return return_type::OK;
+  }
+
+  return_type set_command(
+    const std::vector<double> & command,
+    const std::vector<std::string> & interfaces) override
+  {
+    return hardware_interface::components::set_internal_values_with_limits(
+      command, interfaces, info_.command_interfaces, commands_, lower_limits_, upper_limits_);
+  }
+
+  return_type set_command(const std::vector<double> & command) override
+  {
+    return hardware_interface::components::set_internal_values_with_limits(
+      command, commands_, lower_limits_, upper_limits_);
+  }
+
+private:
+  std::vector<double> lower_limits_, upper_limits_;
+};
+
+}  // namespace ros2_control_components
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(
+  ros2_control_components::PositionJoint, hardware_interface::components::Joint)