add auto_driver_hardware

auto_driver_hardware/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(auto_driver_hardware)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(
+  ${PROJECT_NAME}
+  SHARED
+  src/auto_driver_hardware.cpp
+)
+pluginlib_export_plugin_description_file(hardware_interface auto_driver_hardware.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+include_directories(include)
+ament_auto_package()

auto_driver_hardware/auto_driver_hardware.xml

@@ -0,0 +1,10 @@
+<library path="auto_driver_hardware">
+    <class
+        name="auto_driver_hardware/AutoDriverHardware"
+        type="auto_driver_hardware::AutoDriverHardware"
+        base_class_type="hardware_interface::SystemInterface">
+      <description>
+        CoRE-1 Auto Driver hardware interface
+      </description>
+    </class>
+  </library>

auto_driver_hardware/include/auto_driver_hardware/auto_driver_hardware.hpp

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <vector>
+
+#include <hardware_interface/system_interface.hpp>
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace auto_driver_hardware
+{
+using hardware_interface::CallbackReturn;
+using rclcpp_lifecycle::State;
+using hardware_interface::return_type;
+
+class AutoDriverHardware : public hardware_interface::SystemInterface
+{
+public:
+  CallbackReturn on_init(const hardware_interface::HardwareInfo &) override;
+  CallbackReturn on_activate(const State &) override;
+  CallbackReturn on_deactivate(const State &) override;
+
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  return_type read(const rclcpp::Time &, const rclcpp::Duration &) override;
+  return_type write(const rclcpp::Time &, const rclcpp::Duration &) override;
+
+private:
+  std::vector<double> position_states_;
+  std::vector<double> velocity_states_;
+  std::vector<double> position_commands_;
+  std::vector<double> velocity_commands_;
+  double shoot_command_;
+  double shoot_state_;
+};
+}  // namespace auto_driver_hardware

auto_driver_hardware/package.xml

@@ -0,0 +1,22 @@
+<?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>auto_driver_hardware</name>
+  <version>0.0.0</version>
+  <description>CoRE-1 Auto Driver hardware interface</description>
+  <maintainer email="[email protected]">Ar-Ray-code</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

auto_driver_hardware/src/auto_driver_hardware.cpp

@@ -0,0 +1,142 @@
+#include "auto_driver_hardware/auto_driver_hardware.hpp"
+
+namespace auto_driver_hardware
+{
+AutoDriverHardware::CallbackReturn AutoDriverHardware::on_init(
+  const hardware_interface::HardwareInfo & info)
+{
+  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
+    return CallbackReturn::ERROR;
+  }
+
+  // TODO : Initialize AutoDriverInterface
+
+  this->position_states_.resize(
+    this->info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
+  this->velocity_states_.resize(
+    this->info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
+  this->position_commands_.resize(
+    this->info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
+  this->velocity_commands_.resize(
+    this->info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
+
+  this->shoot_command_ = 0;
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn AutoDriverHardware::on_activate(const State & state)
+{
+  (void)state;
+  for (size_t i = 0; i < this->info_.joints.size(); ++i) {
+    if (std::isnan(this->position_states_.at(i))) {
+      this->position_states_.at(i) = 0.0;
+    }
+    if (std::isnan(this->velocity_states_.at(i))) {
+      this->velocity_states_.at(i) = 0.0;
+    }
+    if (std::isnan(this->velocity_commands_.at(i))) {
+      this->velocity_commands_.at(i) = 0.0;
+    }
+  }
+
+  this->shoot_command_ = 0;
+
+  // TODO : on_activate AutoDriverInterface
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn AutoDriverHardware::on_deactivate(const State & state)
+{
+  (void)state;
+  // TODO : on_deactivate AutoDriverInterface
+  return CallbackReturn::SUCCESS;
+}
+
+std::vector<hardware_interface::StateInterface> AutoDriverHardware::export_state_interfaces()
+{
+  std::vector<hardware_interface::StateInterface> state_interfaces;
+
+  // joint states
+  for (size_t i = 0; i < info_.joints.size(); ++i) {
+    state_interfaces.emplace_back(
+      hardware_interface::StateInterface(
+        this->info_.joints.at(i).name, hardware_interface::HW_IF_POSITION,
+        &position_states_.at(i)));
+    state_interfaces.emplace_back(
+      hardware_interface::StateInterface(
+        this->info_.joints.at(i).name, hardware_interface::HW_IF_VELOCITY,
+        &velocity_states_.at(i)));
+  }
+
+  // shoot state
+  state_interfaces.emplace_back(
+    hardware_interface::StateInterface(
+      this->info_.gpios[0].name, "shoot_state", &this->shoot_state_));
+
+  return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface> AutoDriverHardware::export_command_interfaces()
+{
+  std::vector<hardware_interface::CommandInterface> command_interfaces;
+
+  // joint commands
+  for (size_t i = 0; i < this->info_.joints.size(); ++i) {
+    command_interfaces.emplace_back(
+      hardware_interface::CommandInterface(
+        this->info_.joints.at(i).name, hardware_interface::HW_IF_VELOCITY,
+        &this->velocity_commands_.at(i)));
+  }
+
+  // shoot command
+  command_interfaces.emplace_back(
+    hardware_interface::CommandInterface(
+      this->info_.gpios[0].name, "shoot_command", &this->shoot_command_));
+
+  return command_interfaces;
+}
+
+return_type AutoDriverHardware::read(const rclcpp::Time &, const rclcpp::Duration &)
+{
+
+  const double axis_velocity_0 = 0.0;
+  const double axis_velocity_1 = 0.0;
+  // TODO : read axis_0_velocity and axis_1_velocity from AutoDriverInterface
+
+  if (!std::isnan(axis_velocity_0)) {
+    // TODO : set this->velocity_states_[0]
+  }
+  if (!std::isnan(axis_velocity_1)) {
+    // TODO : set this->velocity_states_[1]
+  }
+
+  const double axis_0_position = 0.0;
+  const double axis_1_position = 0.0;
+  // TODO : read axis_0 and axis_1 from AutoDriverInterface
+
+  if (!std::isnan(axis_0_position)) {
+    // TODO : set this->position_states_[0]
+  }
+  if (!std::isnan(axis_1_position)) {
+    // TODO : set this->position_states_[1]
+  }
+
+  // TODO : cleanup
+  return return_type::OK;
+}
+
+return_type AutoDriverHardware::write(const rclcpp::Time &, const rclcpp::Duration &)
+{
+  // TODO : use this->velocity_commands_.at(0) and this->velocity_commands_.at(1)
+
+  // TODO : write this->shoot_command_ for shooting
+
+  return return_type::OK;
+}
+} // namespace auto_driver_hardware
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(
+  auto_driver_hardware::AutoDriverHardware,
+  hardware_interface::SystemInterface)