Add first example robot (rrbot as system)

ros2_control_demo_hardware/CMakeLists.txt

@@ -12,7 +12,35 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
 
+## COMPILE
+add_library(
+  ${PROJECT_NAME}
+  SHARED
+  src/rrbot_system_position_only.cpp
+)
+target_include_directories(
+  ${PROJECT_NAME}
+  PRIVATE
+  include
+)
+ament_target_dependencies(
+  ${PROJECT_NAME}
+  hardware_interface
+  pluginlib
+  rclcpp
+)
+
+pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_hardware.xml)
+
+# INSTALL
+install(
+  TARGETS ${PROJECT_NAME}
+  DESTINATION lib
+)
 install(
   DIRECTORY include/
   DESTINATION include
@@ -22,10 +50,16 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
 endif()
 
+## EXPORTS
 ament_export_include_directories(
   include
 )
 ament_export_libraries(
   ${PROJECT_NAME}
 )
+ament_export_dependencies(
+  hardware_interface
+  pluginlib
+  rclcpp
+)
 ament_package()

ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp

@@ -0,0 +1,97 @@
+// 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 ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_
+#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/macros.hpp"
+
+#include "hardware_interface/system_hardware_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_status_values.hpp"
+#include "ros2_control_demo_hardware/visibility_control.h"
+
+using hardware_interface::components::Joint;
+using hardware_interface::components::Sensor;
+using hardware_interface::HardwareInfo;
+using hardware_interface::hardware_interface_status;
+using hardware_interface::return_type;
+
+namespace hardware_interface
+{
+
+// TODO(all): This could be templated for Joint, Sensor and System Interface
+class BaseSystemHardwareInterface : public SystemHardwareInterface
+{
+public:
+  return_type configure(const HardwareInfo & system_info) override
+  {
+    info_ = system_info;
+    status_ = hardware_interface_status::CONFIGURED;
+    return return_type::OK;
+  }
+
+  std::string get_name() const final
+  {
+    return info_.name;
+  }
+
+  hardware_interface_status get_status() const final
+  {
+    return status_;
+  }
+
+protected:
+  HardwareInfo info_;
+  hardware_interface_status status_;
+};
+
+}  // namespace hardware_interface
+
+namespace ros2_control_demo_hardware
+{
+class RRBotSystemPositionOnlyHardware : public hardware_interface::BaseSystemHardwareInterface
+{
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware);
+
+  return_type configure(const HardwareInfo & system_info) override;
+
+  return_type start() override;
+
+  return_type stop() override;
+
+  // TODO(all): Add a new "sensor not exitst" error?
+  return_type read_sensors(std::vector<std::shared_ptr<Sensor>> & /*sensors*/) const override
+  {
+    return return_type::ERROR;
+  }
+
+  return_type read_joints(std::vector<std::shared_ptr<Joint>> & joints) const override;
+
+  return_type write_joints(const std::vector<std::shared_ptr<Joint>> & joints) override;
+
+private:
+  double hw_write_time_, hw_read_time_;
+  std::vector<double> hw_values_;
+};
+
+}  // namespace ros2_control_demo_hardware
+
+#endif  // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_

ros2_control_demo_hardware/package.xml

@@ -2,14 +2,17 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ros2_control_demo_hardware</name>
-  <version>0.0.0</version>
+  <version>0.0.1</version>
   <description>Demo package of `ros2_control` Hardware. This package provides example on how to define hardware of your own robot for ROS2 control. The package is used with `demo_robot` from package `ros2_control_demo_robot`.</description>
 
   <maintainer email="[email protected]">Denis Štogl</maintainer>
 
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>hardware_interface</buildtool_depend>
+  <buildtool_depend>pluginlib</buildtool_depend>
+  <buildtool_depend>rclcpp</buildtool_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
 

ros2_control_demo_hardware/ros2_control_demo_hardware.xml

@@ -0,0 +1,9 @@
+<library path="ros2_control_demo_hardware">
+  <class name="ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware"
+         type="ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware"
+         base_class_type="hardware_interface::SystemHardwareInterface">
+    <description>
+      The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1.
+    </description>
+  </class>
+</library>

ros2_control_demo_hardware/src/rrbot_system_position_only.cpp

@@ -0,0 +1,175 @@
+// 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 <chrono>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <vector>
+
+#include "ros2_control_demo_hardware/rrbot_system_position_only.hpp"
+
+#include "hardware_interface/components/component_info.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace ros2_control_demo_hardware
+{
+
+return_type RRBotSystemPositionOnlyHardware::configure(const HardwareInfo & system_info)
+{
+  if (hardware_interface::BaseSystemHardwareInterface::configure(system_info) != return_type::OK) {
+    return return_type::ERROR;
+  }
+
+  hw_read_time_ = stod(info_.hardware_parameters["example_param_read_for_sec"]);
+  hw_write_time_ = stod(info_.hardware_parameters["example_param_write_for_sec"]);
+  hw_values_.resize(info_.joints.size());
+  for (uint i = 0; i < hw_values_.size(); i++) {
+    hw_values_[i] = std::numeric_limits<double>::quiet_NaN();
+  }
+  for (auto joint : info_.joints) {
+    if (joint.class_type.compare("ros2_control_components/PositionJoint") != 0) {
+      status_ = hardware_interface_status::UNKNOWN;
+      // TODO(all): should we return specizalized error?
+      return return_type::ERROR;
+    }
+  }
+
+  status_ = hardware_interface_status::CONFIGURED;
+  return return_type::OK;
+}
+
+return_type RRBotSystemPositionOnlyHardware::start()
+{
+  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
+    "Starting ...please wait...");
+
+  for (int i = 0; i < 3; i++) {
+    rclcpp::sleep_for(std::chrono::seconds(1));
+    RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%d seconds left...", 3 - i);
+  }
+
+  // set some default values
+  for (uint i = 0; i < hw_values_.size(); i++) {
+    if (std::isnan(hw_values_[i])) {
+      hw_values_[i] = 0;
+    }
+  }
+
+  status_ = hardware_interface_status::STARTED;
+
+  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
+    "System Sucessfully started!");
+
+  return return_type::OK;
+}
+
+return_type RRBotSystemPositionOnlyHardware::stop()
+{
+  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
+    "Stopping ...please wait...");
+
+  for (int i = 0; i < 4; i++) {
+    rclcpp::sleep_for(std::chrono::seconds(1));
+    RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%d seconds left...", 3 - i);
+  }
+
+  status_ = hardware_interface_status::STOPPED;
+
+  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
+    "System sucessfully stopped!");
+
+  return return_type::OK;
+}
+
+return_type RRBotSystemPositionOnlyHardware::read_joints(
+  std::vector<std::shared_ptr<Joint>> & joints) const
+{
+  if (joints.size() != hw_values_.size()) {
+    // TODO(all): return "wrong number of joints" error?
+    return return_type::ERROR;
+  }
+
+  return_type ret = return_type::OK;
+
+  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
+    "Reading ...please wait...");
+
+  for (int i = 0; i < hw_read_time_; i++) {
+    rclcpp::sleep_for(std::chrono::seconds(1));
+    RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
+      "%.1f seconds for reading left...", hw_read_time_ - i);
+  }
+
+  // TODO(all): Should we check here joint names for the proper order?
+  std::vector<double> values;
+  values.resize(1);
+  for (uint i = 0; i < joints.size(); i++) {
+    values[0] = hw_values_[i] + 0.04;  // Added number for "randomness" in the data
+    ret = joints[i]->set_state(values);
+    if (ret != return_type::OK) {
+      break;
+    }
+  }
+
+  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
+    "Joints sucessfully read!");
+
+  return ret;
+}
+
+return_type RRBotSystemPositionOnlyHardware::write_joints(
+  const std::vector<std::shared_ptr<Joint>> & joints)
+{
+  if (joints.size() != hw_values_.size()) {
+    // TODO(all): return wrong number of joints
+    return return_type::ERROR;
+  }
+
+  return_type ret = return_type::OK;
+
+  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
+    "Writing ...please wait...");
+
+  for (int i = 0; i < hw_write_time_; i++) {
+    rclcpp::sleep_for(std::chrono::seconds(1));
+    RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
+      "%.1f seconds for writing left...", hw_write_time_ - i);
+  }
+
+  // TODO(anyone): here crashes the plugin, why...
+  // TODO(all): Should we check here joint names for the proper order?
+  std::vector<double> values;
+  for (uint i = 0; i < joints.size(); i++) {
+    ret = joints[i]->get_command(values);
+    if (ret != return_type::OK) {
+      break;
+    }
+    hw_values_[i] = values[0] + 0.023;  // Added number for "randomness" in the data
+  }
+
+  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
+    "Joints sucessfully written!");
+  return ret;
+}
+
+}  // namespace ros2_control_demo_hardware
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware,
+  hardware_interface::SystemHardwareInterface
+)