Merge branch '10-hardware-interface' into 'main'

add initial servo hardware

Closes #10

See merge request robossembler/servo!4

rbs_servo_hardware/CMakeLists.txt

@@ -0,0 +1,67 @@
+cmake_minimum_required(VERSION 3.16)
+project(rbs_servo_hardware LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+# find dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+
+## COMPILE
+add_library(
+  rbs_servo_hardware
+  SHARED
+  src/rbs_servo_actuator.cpp
+)
+target_compile_features(rbs_servo_hardware PUBLIC cxx_std_17)
+target_include_directories(rbs_servo_hardware PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/rbs_servo_hardware>
+)
+ament_target_dependencies(
+  rbs_servo_hardware PUBLIC
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(${PROJECT_NAME} PRIVATE "rbs_servo_hardware_BUILDING_DLL")
+
+# Export hardware plugins
+pluginlib_export_plugin_description_file(hardware_interface rbs_servo_hardware.xml)
+
+# INSTALL
+install(
+  DIRECTORY include/
+  DESTINATION include/
+)
+
+install(TARGETS rbs_servo_hardware
+  EXPORT export_rbs_servo_hardware
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+endif()
+
+## EXPORTS
+ament_export_targets(export_rbs_servo_hardware HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

rbs_servo_hardware/README.md

@@ -0,0 +1,5 @@
+# rbs_servo_hardware
+
+   The example shows how to implement robot hardware with actuators not providing states and with additional sensors.
+
+Find the documentation in [doc/userdoc.rst](doc/userdoc.rst) or on [control.ros.org](https://control.ros.org/master/doc/ros2_control_demos/example_14/doc/userdoc.html).

rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp

@@ -0,0 +1,76 @@
+#ifndef rbs_servo_hardware__RBS_ACTUATOR_HPP_
+#define rbs_servo_hardware__RBS_ACTUATOR_HPP_
+// Libs for fake connection 
+// WARN: Delete it in the future
+#include <netinet/in.h>
+#include <sys/socket.h>
+// System libs
+#include <memory>
+#include <string>
+#include <vector>
+// ROS specific libs
+#include "hardware_interface/actuator_interface.hpp"
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "rclcpp/macros.hpp"
+#include "rbs_servo_hardware/visibility_control.h"
+
+namespace rbs_servo_hardware
+{
+class RbsServoActuator : public hardware_interface::ActuatorInterface
+{
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(RbsServoActuator);
+
+  rbs_servo_hardware_PUBLIC
+  hardware_interface::CallbackReturn on_init(
+    const hardware_interface::HardwareInfo & info) override;
+
+  rbs_servo_hardware_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  rbs_servo_hardware_PUBLIC
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  rbs_servo_hardware_PUBLIC
+  hardware_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  rbs_servo_hardware_PUBLIC
+  hardware_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  rbs_servo_hardware_PUBLIC
+  hardware_interface::CallbackReturn on_shutdown(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  rbs_servo_hardware_PUBLIC
+  hardware_interface::return_type read(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  rbs_servo_hardware_PUBLIC
+  hardware_interface::return_type write(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+private:
+  // Parameters for the rbs_servo_hardware simulation
+  double hw_start_sec_;
+  double hw_stop_sec_;
+
+  // Store the command for the robot
+  double hw_joint_command_;
+  // Store the states for the robot
+  double hw_joint_state_;
+
+  // Fake "mechanical connection" between actuator and sensor using sockets
+  struct sockaddr_in address_;
+  int socket_port_;
+  int sockoptval_ = 1;
+  int sock_;
+};
+
+}  // namespace rbs_servo_hardware
+
+#endif  // rbs_servo_hardware__RBS_ACTUATOR_HPP_

rbs_servo_hardware/include/rbs_servo_hardware/visibility_control.h

@@ -0,0 +1,56 @@
+// Copyright 2021 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.
+
+/* This header must be included by all rclcpp headers which declare symbols
+ * which are defined in the rclcpp library. When not building the rclcpp
+ * library, i.e. when using the headers in other package's code, the contents
+ * of this header change the visibility of certain symbols which the rclcpp
+ * library cannot have, but the consuming code must have inorder to link.
+ */
+
+#ifndef rbs_servo_hardware__VISIBILITY_CONTROL_H_
+#define rbs_servo_hardware__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define rbs_servo_hardware_EXPORT __attribute__((dllexport))
+#define rbs_servo_hardware_IMPORT __attribute__((dllimport))
+#else
+#define rbs_servo_hardware_EXPORT __declspec(dllexport)
+#define rbs_servo_hardware_IMPORT __declspec(dllimport)
+#endif
+#ifdef rbs_servo_hardware_BUILDING_DLL
+#define rbs_servo_hardware_PUBLIC rbs_servo_hardware_EXPORT
+#else
+#define rbs_servo_hardware_PUBLIC rbs_servo_hardware_IMPORT
+#endif
+#define rbs_servo_hardware_PUBLIC_TYPE rbs_servo_hardware_PUBLIC
+#define rbs_servo_hardware_LOCAL
+#else
+#define rbs_servo_hardware_EXPORT __attribute__((visibility("default")))
+#define rbs_servo_hardware_IMPORT
+#if __GNUC__ >= 4
+#define rbs_servo_hardware_PUBLIC __attribute__((visibility("default")))
+#define rbs_servo_hardware_LOCAL __attribute__((visibility("hidden")))
+#else
+#define rbs_servo_hardware_PUBLIC
+#define rbs_servo_hardware_LOCAL
+#endif
+#define rbs_servo_hardware_PUBLIC_TYPE
+#endif
+
+#endif  // rbs_servo_hardware__VISIBILITY_CONTROL_H_

rbs_servo_hardware/package.xml

@@ -0,0 +1,38 @@
+<?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>rbs_servo_hardware</name>
+  <version>0.0.0</version>
+  <description>The hardware_interface for rbs_servo_hardware from RobossemblerArm</description>
+
+  <maintainer email="ur.narmak@gmail.com">Bill Finger</maintainer>
+
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>forward_command_controller</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>ros2_controllers_test_nodes</exec_depend>
+  <exec_depend>ros2controlcli</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>launch_testing_ros</test_depend>
+  <test_depend>liburdfdom-tools</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

rbs_servo_hardware/rbs_servo_hardware.xml

@@ -0,0 +1,9 @@
+<library path="rbs_servo_hardware">
+  <class name="rbs_servo_hardware/RbsServo"
+         type="rbs_servo_hardware::RbsServo"
+         base_class_type="hardware_interface::ActuatorInterface">
+    <description>
+      Hardware interface for rbs_servo_hardware
+    </description>
+  </class>
+</library>

rbs_servo_hardware/src/rbs_servo_actuator.cpp

@@ -0,0 +1,205 @@
+#include "rbs_servo_hardware/rbs_servo_actuator.hpp"
+// Libs for fake connection 
+// WARN: Delete it in the future
+#include <netdb.h>
+// System libs
+#include <chrono>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <vector>
+// ROS specific libs
+#include <rclcpp/logger.hpp>
+#include "hardware_interface/actuator_interface.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace rbs_servo_hardware {
+hardware_interface::CallbackReturn
+RbsServoActuator::on_init(const hardware_interface::HardwareInfo &info) {
+  if (hardware_interface::ActuatorInterface::on_init(info) !=
+      hardware_interface::CallbackReturn::SUCCESS) {
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+  // START: This part here is for exemplary purposes - Please do not copy to
+  // your production code
+  hw_start_sec_ = std::stod(
+      info_.hardware_parameters["example_param_hw_start_duration_sec"]);
+  hw_stop_sec_ = std::stod(
+      info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
+  socket_port_ =
+      std::stoi(info_.hardware_parameters["example_param_socket_port"]);
+  // END: This part here is for exemplary purposes - Please do not copy to your
+  // production code
+
+  hw_joint_command_ = std::numeric_limits<double>::quiet_NaN();
+
+  const hardware_interface::ComponentInfo &joint = info_.joints[0];
+  // RbsServoActuator has exactly one command interface and one joint
+  if (joint.command_interfaces.size() != 1) {
+    RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"),
+                 "Joint '%s' has %zu command interfaces found. 1 expected.",
+                 joint.name.c_str(), joint.command_interfaces.size());
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
+  if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) {
+    RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"),
+                 "Joint '%s' have %s command interfaces found. '%s' expected.",
+                 joint.name.c_str(), joint.command_interfaces[0].name.c_str(),
+                 hardware_interface::HW_IF_VELOCITY);
+    return hardware_interface::CallbackReturn::ERROR;
+  } else if (joint.state_interfaces[0].name !=
+             hardware_interface::HW_IF_VELOCITY) {
+    RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"),
+                 "Joint '%s' have %s state interface found. '%s' expected.",
+                 joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
+                 hardware_interface::HW_IF_VELOCITY);
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
+  // START: This part here is for exemplary purposes - Please do not copy to
+  // your production code Initialize objects for fake mechanical connection
+  sock_ = socket(AF_INET, SOCK_STREAM, 0);
+  if (sock_ < 0) {
+    RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"),
+                 "Creating socket failed.");
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
+  auto server = gethostbyname("localhost");
+
+  address_.sin_family = AF_INET;
+  bcopy(reinterpret_cast<char *>(server->h_addr),
+        reinterpret_cast<char *>(&address_.sin_addr.s_addr), server->h_length);
+  address_.sin_port = htons(socket_port_);
+
+  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"),
+              "Trying to connect to port %d.", socket_port_);
+  if (connect(sock_, (struct sockaddr *)&address_, sizeof(address_)) < 0) {
+    RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"),
+                 "Connection over socket failed.");
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Connected to socket");
+  // END: This part here is for exemplary purposes - Please do not copy to your
+  // production code
+
+  return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn RbsServoActuator::on_shutdown(
+    const rclcpp_lifecycle::State & /*previous_state*/) {
+  shutdown(sock_, SHUT_RDWR); // shutdown socket
+
+  return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+std::vector<hardware_interface::StateInterface>
+RbsServoActuator::export_state_interfaces() {
+  std::vector<hardware_interface::StateInterface> state_interfaces;
+
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+      info_.joints[0].name, hardware_interface::HW_IF_VELOCITY,
+      &hw_joint_state_));
+
+  return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface>
+RbsServoActuator::export_command_interfaces() {
+  std::vector<hardware_interface::CommandInterface> command_interfaces;
+
+  command_interfaces.emplace_back(hardware_interface::CommandInterface(
+      info_.joints[0].name, hardware_interface::HW_IF_VELOCITY,
+      &hw_joint_command_));
+
+  return command_interfaces;
+}
+
+hardware_interface::CallbackReturn RbsServoActuator::on_activate(
+    const rclcpp_lifecycle::State & /*previous_state*/) {
+  // START: This part here is for exemplary purposes - Please do not copy to
+  // your production code
+  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"),
+              "Activating ...please wait...");
+
+  for (int i = 0; i < hw_start_sec_; i++) {
+    rclcpp::sleep_for(std::chrono::seconds(1));
+    RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "%.1f seconds left...",
+                hw_start_sec_ - i);
+  }
+  // END: This part here is for exemplary purposes - Please do not copy to your
+  // production code
+
+  // set some default values for joints
+  if (std::isnan(hw_joint_command_)) {
+    hw_joint_command_ = 0;
+    hw_joint_state_ = 0;
+  }
+
+  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"),
+              "Successfully activated!");
+
+  return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn RbsServoActuator::on_deactivate(
+    const rclcpp_lifecycle::State & /*previous_state*/) {
+  // START: This part here is for exemplary purposes - Please do not copy to
+  // your production code
+  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"),
+              "Deactivating ...please wait...");
+
+  for (int i = 0; i < hw_stop_sec_; i++) {
+    rclcpp::sleep_for(std::chrono::seconds(1));
+    RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "%.1f seconds left...",
+                hw_stop_sec_ - i);
+  }
+
+  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"),
+              "Successfully deactivated!");
+  // END: This part here is for exemplary purposes - Please do not copy to your
+  // production code
+
+  return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::return_type
+RbsServoActuator::read(const rclcpp::Time & /*time*/,
+                       const rclcpp::Duration & /*period*/) {
+  return hardware_interface::return_type::OK;
+  // TODO: Implementation of reading process
+  // see write() below for example of command
+  // interface
+}
+
+hardware_interface::return_type
+rbs_servo_hardware::RbsServoActuator::write(const rclcpp::Time & /*time*/,
+                                   const rclcpp::Duration & /*period*/) {
+  // START: This part here is for exemplary purposes - Please do not copy to
+  // your production code
+  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Writing command: %f",
+              hw_joint_command_);
+
+  // Simulate sending commands to the hardware
+  std::ostringstream data;
+  data << hw_joint_command_;
+  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"),
+              "Sending data command: %s", data.str().c_str());
+  send(sock_, data.str().c_str(), strlen(data.str().c_str()), 0);
+
+  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"),
+              "Joints successfully written!");
+  // END: This part here is for exemplary purposes - Please do not copy to your
+  // production code
+
+  return hardware_interface::return_type::OK;
+}
+
+} // namespace rbs_servo_hardware
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(rbs_servo_hardware::RbsServoActuator,
+                       hardware_interface::ActuatorInterface)