From 20aa343f2917ccf655738bdad8f3f44030c8df13 Mon Sep 17 00:00:00 2001 From: Ar-Ray-code Date: Sat, 2 Dec 2023 00:46:46 +0900 Subject: [PATCH] add auto_driver_hardware --- auto_driver_hardware/CMakeLists.txt | 26 ++++ auto_driver_hardware/auto_driver_hardware.xml | 10 ++ .../auto_driver_hardware.hpp | 36 +++++ auto_driver_hardware/package.xml | 22 +++ .../src/auto_driver_hardware.cpp | 142 ++++++++++++++++++ 5 files changed, 236 insertions(+) create mode 100644 auto_driver_hardware/CMakeLists.txt create mode 100644 auto_driver_hardware/auto_driver_hardware.xml create mode 100644 auto_driver_hardware/include/auto_driver_hardware/auto_driver_hardware.hpp create mode 100644 auto_driver_hardware/package.xml create mode 100644 auto_driver_hardware/src/auto_driver_hardware.cpp diff --git a/auto_driver_hardware/CMakeLists.txt b/auto_driver_hardware/CMakeLists.txt new file mode 100644 index 0000000..83c4b88 --- /dev/null +++ b/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() diff --git a/auto_driver_hardware/auto_driver_hardware.xml b/auto_driver_hardware/auto_driver_hardware.xml new file mode 100644 index 0000000..51b11e6 --- /dev/null +++ b/auto_driver_hardware/auto_driver_hardware.xml @@ -0,0 +1,10 @@ + + + + CoRE-1 Auto Driver hardware interface + + + \ No newline at end of file diff --git a/auto_driver_hardware/include/auto_driver_hardware/auto_driver_hardware.hpp b/auto_driver_hardware/include/auto_driver_hardware/auto_driver_hardware.hpp new file mode 100644 index 0000000..5556b1e --- /dev/null +++ b/auto_driver_hardware/include/auto_driver_hardware/auto_driver_hardware.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include + +#include +#include +#include + +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 export_state_interfaces() override; + std::vector 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 position_states_; + std::vector velocity_states_; + std::vector position_commands_; + std::vector velocity_commands_; + double shoot_command_; + double shoot_state_; +}; +} // namespace auto_driver_hardware diff --git a/auto_driver_hardware/package.xml b/auto_driver_hardware/package.xml new file mode 100644 index 0000000..1dfcea9 --- /dev/null +++ b/auto_driver_hardware/package.xml @@ -0,0 +1,22 @@ + + + + auto_driver_hardware + 0.0.0 + CoRE-1 Auto Driver hardware interface + Ar-Ray-code + Apache-2.0 + + ament_cmake_auto + + hardware_interface + pluginlib + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/auto_driver_hardware/src/auto_driver_hardware.cpp b/auto_driver_hardware/src/auto_driver_hardware.cpp new file mode 100644 index 0000000..55a6809 --- /dev/null +++ b/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::quiet_NaN()); + this->velocity_states_.resize( + this->info_.joints.size(), std::numeric_limits::quiet_NaN()); + this->position_commands_.resize( + this->info_.joints.size(), std::numeric_limits::quiet_NaN()); + this->velocity_commands_.resize( + this->info_.joints.size(), std::numeric_limits::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 AutoDriverHardware::export_state_interfaces() +{ + std::vector 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 AutoDriverHardware::export_command_interfaces() +{ + std::vector 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_EXPORT_CLASS( + auto_driver_hardware::AutoDriverHardware, + hardware_interface::SystemInterface)