diff --git a/src/description/ros2_control/science/science.killswitch.ros2_control.xacro b/src/description/ros2_control/science/science.killswitch.ros2_control.xacro new file mode 100644 index 0000000..822a298 --- /dev/null +++ b/src/description/ros2_control/science/science.killswitch.ros2_control.xacro @@ -0,0 +1,33 @@ + + + + + + + + + killswitch_ros2_control/KillswitchHardwareInterface + ${button_gpio_pin} + ${main_power_gpio_pin} + ${jetson_power_gpio_pin} + ${all_power_gpio_pin} + ${active_high} + + + + + + + + + + + + + + + + + + + diff --git a/src/description/ros2_control/science/science.laser.ros2_control.xacro b/src/description/ros2_control/science/science.laser.ros2_control.xacro new file mode 100644 index 0000000..f981886 --- /dev/null +++ b/src/description/ros2_control/science/science.laser.ros2_control.xacro @@ -0,0 +1,28 @@ + + + + + + + + + laser_ros2_control/LaserHardwareInterface + ${gpio_pin} + ${min_power} + ${max_power} + + + + + + + + + + + + + + + + diff --git a/src/description/ros2_control/science/science.led.ros2_control.xacro b/src/description/ros2_control/science/science.led.ros2_control.xacro new file mode 100644 index 0000000..cb96abc --- /dev/null +++ b/src/description/ros2_control/science/science.led.ros2_control.xacro @@ -0,0 +1,25 @@ + + + + + + + + + led_ros2_control/LEDHardwareInterface + ${gpio_pin} + ${default_state} + + + + + + + + + + + + + + diff --git a/src/description/urdf/athena_science.urdf.xacro b/src/description/urdf/athena_science.urdf.xacro index ca032f7..b88a75f 100644 --- a/src/description/urdf/athena_science.urdf.xacro +++ b/src/description/urdf/athena_science.urdf.xacro @@ -9,6 +9,9 @@ + + + @@ -30,7 +33,21 @@ - + + + + + + + + + + \ No newline at end of file diff --git a/src/hardware_interfaces/README.md b/src/hardware_interfaces/README.md index 2488a49..a083d58 100644 --- a/src/hardware_interfaces/README.md +++ b/src/hardware_interfaces/README.md @@ -4,8 +4,12 @@ This directory contains ROS2 Control hardware interface implementations for vari ## Available Interfaces -- `actuator_ros2_control/` - Generic actuator interface +- `killswitch_ros2_control/` - Killswitch hardware interface +- `laser_ros2_control/` - Laser hardware interface +- `led_ros2_control/` - LED hardware interface - `rmd_ros2_control/` - RMD motor controller interface +- `ros_odrive/` - ODrive motor controller interface +- `servo_ros2_control/` - Servo motor controller interface - `smc_ros2_control/` - SMC motor controller interface +- `stepper_ros2_control/` - Stepper motor controller interface - `talon_ros2_control/` - Talon motor controller interface -- `ros_odrive/` - ODrive motor controller interface diff --git a/src/hardware_interfaces/killswitch_ros2_control/CMakeLists.txt b/src/hardware_interfaces/killswitch_ros2_control/CMakeLists.txt new file mode 100644 index 0000000..e3eb609 --- /dev/null +++ b/src/hardware_interfaces/killswitch_ros2_control/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.8) +project(killswitch_ros2_control) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_auto REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +ament_auto_find_build_dependencies() + +include_directories(include) + +add_library( + killswitch_ros2_control + SHARED + src/killswitch_hardware_interface.cpp +) + +target_compile_features(killswitch_ros2_control PUBLIC cxx_std_20) +target_include_directories(killswitch_ros2_control PUBLIC +$ +$ +) + +ament_target_dependencies( + killswitch_ros2_control PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# Export hardware plugins +pluginlib_export_plugin_description_file(hardware_interface killswitch_hardware_interface.xml) + +# INSTALL +install( + DIRECTORY include/ + DESTINATION include/killswitch_ros2_control +) + +install(TARGETS killswitch_ros2_control + EXPORT export_killswitch_ros2_control + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +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() + +ament_export_targets(export_killswitch_ros2_control) +ament_export_include_directories(include) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/hardware_interfaces/killswitch_ros2_control/include/killswitch_ros2_control/killswitch_hardware_interface.hpp b/src/hardware_interfaces/killswitch_ros2_control/include/killswitch_ros2_control/killswitch_hardware_interface.hpp new file mode 100644 index 0000000..421e6c2 --- /dev/null +++ b/src/hardware_interfaces/killswitch_ros2_control/include/killswitch_ros2_control/killswitch_hardware_interface.hpp @@ -0,0 +1,134 @@ +// Copyright (c) 2024 UMD Loop +// +// 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 KILLSWITCH_ROS2_CONTROL__KILLSWITCH_HARDWARE_INTERFACE_HPP_ +#define KILLSWITCH_ROS2_CONTROL__KILLSWITCH_HARDWARE_INTERFACE_HPP_ + +#include +#include +#include + +#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 "rclcpp_lifecycle/state.hpp" + +namespace killswitch_ros2_control +{ + +// GPIO utility functions (Linux sysfs) +namespace gpio_utils +{ + int setup_gpio_input(int pin); + int setup_gpio_output(int pin); + void cleanup_gpio(int pin, int fd); + bool read_gpio(int fd); + bool write_gpio(int fd, bool value); +} + +/** + * @brief Hardware interface for drive killswitch system via ros2_control + * + * This is a SystemInterface that monitors a physical killswitch button + * and controls power kill relays for main power, Jetson power, and all power. + * + * State Interfaces (read by controllers): + * - button_state: Physical button state (0.0 = not pressed, 1.0 = pressed) + * - main_power_killed: Main power relay state (0.0 = on, 1.0 = killed) + * - jetson_power_killed: Jetson power relay state (0.0 = on, 1.0 = killed) + * - all_power_killed: All power relay state (0.0 = on, 1.0 = killed) + * - is_connected: Hardware ready status + * + * Command Interfaces (written by controllers): + * - kill_main_power: Set to 1.0 to kill main power, 0.0 to restore + * - kill_jetson_power: Set to 1.0 to kill Jetson power, 0.0 to restore + * - kill_all_power: Set to 1.0 to kill all power, 0.0 to restore + * + * Hardware Parameters (from URDF): + * - button_gpio_pin: GPIO pin for killswitch button input (required) + * - main_power_gpio_pin: GPIO pin for main power relay output (required) + * - jetson_power_gpio_pin: GPIO pin for Jetson power relay output (required) + * - all_power_gpio_pin: GPIO pin for all-power relay output (required) + * - active_high: If true, HIGH = button pressed (default: true) + */ +class KillswitchHardwareInterface : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(KillswitchHardwareInterface) + + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + std::vector export_state_interfaces() override; + + std::vector export_command_interfaces() override; + + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::return_type read( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; + + hardware_interface::return_type write( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; + +private: + // Configuration parameters (GPIO pins) + int button_gpio_pin_; + int main_power_gpio_pin_; + int jetson_power_gpio_pin_; + int all_power_gpio_pin_; + bool active_high_; + + // State variables (hardware → ros2_control) + double button_state_; // Physical button: 0.0 = not pressed, 1.0 = pressed + double main_power_killed_; // 0.0 = power on, 1.0 = killed + double jetson_power_killed_; // 0.0 = power on, 1.0 = killed + double all_power_killed_; // 0.0 = power on, 1.0 = killed + double is_connected_; // Hardware ready status + + // Command variables (ros2_control → hardware) + double cmd_kill_main_power_; + double cmd_kill_jetson_power_; + double cmd_kill_all_power_; + + // GPIO file descriptors + int button_fd_; + int main_power_fd_; + int jetson_power_fd_; + int all_power_fd_; + bool hw_connected_; +}; + +} // namespace killswitch_ros2_control + +#endif // KILLSWITCH_ROS2_CONTROL__KILLSWITCH_HARDWARE_INTERFACE_HPP_ + diff --git a/src/hardware_interfaces/killswitch_ros2_control/killswitch_hardware_interface.xml b/src/hardware_interfaces/killswitch_ros2_control/killswitch_hardware_interface.xml new file mode 100644 index 0000000..9fd8e26 --- /dev/null +++ b/src/hardware_interfaces/killswitch_ros2_control/killswitch_hardware_interface.xml @@ -0,0 +1,10 @@ + + + + Drive killswitch hardware interface via ros2_control. + Monitors physical killswitch button and controls power kill relays. + + + diff --git a/src/hardware_interfaces/killswitch_ros2_control/package.xml b/src/hardware_interfaces/killswitch_ros2_control/package.xml new file mode 100644 index 0000000..37f7409 --- /dev/null +++ b/src/hardware_interfaces/killswitch_ros2_control/package.xml @@ -0,0 +1,26 @@ + + + + killswitch_ros2_control + 0.1.0 + Killswitch hardware interface for ros2_control - GPIO-based drive killswitch system + Dwarakesh Baraneetharan + Apache-2.0 + + ament_cmake + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + ament_lint_auto + ament_lint_common + + ament_cmake + + + ament_cmake + + diff --git a/src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp b/src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp new file mode 100644 index 0000000..dedb805 --- /dev/null +++ b/src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp @@ -0,0 +1,483 @@ +// Copyright (c) 2024 UMD Loop +// +// 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 "killswitch_ros2_control/killswitch_hardware_interface.hpp" + +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace killswitch_ros2_control +{ + +// GPIO utility functions implementation +namespace gpio_utils +{ + +int setup_gpio_output(int pin) +{ + // Export GPIO + int fd = ::open("/sys/class/gpio/export", O_WRONLY); + if (fd >= 0) { + std::string pin_str = std::to_string(pin); + ::write(fd, pin_str.c_str(), pin_str.length()); + ::close(fd); + usleep(100000); // Wait for GPIO to be exported + } + + // Set direction to output + std::stringstream direction_path; + direction_path << "/sys/class/gpio/gpio" << pin << "/direction"; + fd = ::open(direction_path.str().c_str(), O_WRONLY); + if (fd < 0) { + return -1; + } + ::write(fd, "out", 3); + ::close(fd); + + // Open value file + std::stringstream value_path; + value_path << "/sys/class/gpio/gpio" << pin << "/value"; + int value_fd = ::open(value_path.str().c_str(), O_RDWR); + + return value_fd; +} + +int setup_gpio_input(int pin) +{ + // Export GPIO + int fd = ::open("/sys/class/gpio/export", O_WRONLY); + if (fd >= 0) { + std::string pin_str = std::to_string(pin); + ::write(fd, pin_str.c_str(), pin_str.length()); + ::close(fd); + usleep(100000); // Wait for GPIO to be exported + } + + // Set direction to input + std::stringstream direction_path; + direction_path << "/sys/class/gpio/gpio" << pin << "/direction"; + fd = ::open(direction_path.str().c_str(), O_WRONLY); + if (fd < 0) { + return -1; + } + ::write(fd, "in", 2); + ::close(fd); + + // Open value file + std::stringstream value_path; + value_path << "/sys/class/gpio/gpio" << pin << "/value"; + int value_fd = ::open(value_path.str().c_str(), O_RDONLY); + + return value_fd; +} + +void cleanup_gpio(int pin, int fd) +{ + if (fd >= 0) { + ::close(fd); + } + + // Unexport GPIO + int export_fd = ::open("/sys/class/gpio/unexport", O_WRONLY); + if (export_fd >= 0) { + std::string pin_str = std::to_string(pin); + ::write(export_fd, pin_str.c_str(), pin_str.length()); + ::close(export_fd); + } +} + +bool write_gpio(int fd, bool value) +{ + if (fd < 0) return false; + + char val = value ? '1' : '0'; + lseek(fd, 0, SEEK_SET); + return (::write(fd, &val, 1) == 1); +} + +bool read_gpio(int fd) +{ + if (fd < 0) return false; + + char value; + lseek(fd, 0, SEEK_SET); + if (::read(fd, &value, 1) == 1) { + return (value == '1'); + } + return false; +} + +} // namespace gpio_utils + +// Hardware Interface Implementation + +hardware_interface::CallbackReturn KillswitchHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // Initialize state variables (all power ON, button not pressed) + button_state_ = 0.0; + main_power_killed_ = 0.0; + jetson_power_killed_ = 0.0; + all_power_killed_ = 0.0; + is_connected_ = 0.0; + + // Initialize command variables (don't kill anything on startup) + cmd_kill_main_power_ = 0.0; + cmd_kill_jetson_power_ = 0.0; + cmd_kill_all_power_ = 0.0; + + // Parse hardware parameters + auto get_required_param = [&](const std::string& name) -> int { + if (!info_.hardware_parameters.count(name)) { + RCLCPP_ERROR( + rclcpp::get_logger("KillswitchHardwareInterface"), + "%s parameter is required", name.c_str()); + return -1; + } + return std::stoi(info_.hardware_parameters.at(name)); + }; + + button_gpio_pin_ = get_required_param("button_gpio_pin"); + main_power_gpio_pin_ = get_required_param("main_power_gpio_pin"); + jetson_power_gpio_pin_ = get_required_param("jetson_power_gpio_pin"); + all_power_gpio_pin_ = get_required_param("all_power_gpio_pin"); + + if (button_gpio_pin_ < 0 || main_power_gpio_pin_ < 0 || + jetson_power_gpio_pin_ < 0 || all_power_gpio_pin_ < 0) { + return hardware_interface::CallbackReturn::ERROR; + } + + // Active high/low for button (default: true) + if (info_.hardware_parameters.count("active_high")) { + active_high_ = (info_.hardware_parameters.at("active_high") == "true"); + } else { + active_high_ = true; + } + + // Initialize file descriptors + button_fd_ = -1; + main_power_fd_ = -1; + jetson_power_fd_ = -1; + all_power_fd_ = -1; + hw_connected_ = false; + + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Initialized killswitch system - Button: GPIO%d, Main: GPIO%d, Jetson: GPIO%d, All: GPIO%d", + button_gpio_pin_, main_power_gpio_pin_, jetson_power_gpio_pin_, all_power_gpio_pin_); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn KillswitchHardwareInterface::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Configuring killswitch hardware..."); + + // Setup GPIO input for button + button_fd_ = gpio_utils::setup_gpio_input(button_gpio_pin_); + if (button_fd_ < 0) { + RCLCPP_ERROR( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Failed to setup button GPIO input on pin %d", button_gpio_pin_); + return hardware_interface::CallbackReturn::ERROR; + } + + // Setup GPIO outputs for power relays (initially OFF = power ON) + main_power_fd_ = gpio_utils::setup_gpio_output(main_power_gpio_pin_); + if (main_power_fd_ < 0) { + RCLCPP_ERROR( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Failed to setup main power GPIO output on pin %d", main_power_gpio_pin_); + return hardware_interface::CallbackReturn::ERROR; + } + gpio_utils::write_gpio(main_power_fd_, false); // Initialize to LOW (power ON) + + jetson_power_fd_ = gpio_utils::setup_gpio_output(jetson_power_gpio_pin_); + if (jetson_power_fd_ < 0) { + RCLCPP_ERROR( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Failed to setup Jetson power GPIO output on pin %d", jetson_power_gpio_pin_); + return hardware_interface::CallbackReturn::ERROR; + } + gpio_utils::write_gpio(jetson_power_fd_, false); // Initialize to LOW (power ON) + + all_power_fd_ = gpio_utils::setup_gpio_output(all_power_gpio_pin_); + if (all_power_fd_ < 0) { + RCLCPP_ERROR( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Failed to setup all-power GPIO output on pin %d", all_power_gpio_pin_); + return hardware_interface::CallbackReturn::ERROR; + } + gpio_utils::write_gpio(all_power_fd_, false); // Initialize to LOW (power ON) + + hw_connected_ = true; + is_connected_ = 1.0; + + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Successfully configured killswitch hardware"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector +KillswitchHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + + // Use the joint name from URDF + const std::string& name = info_.joints[0].name; + + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "button_state", &button_state_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "main_power_killed", &main_power_killed_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "jetson_power_killed", &jetson_power_killed_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "all_power_killed", &all_power_killed_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "is_connected", &is_connected_)); + + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Exported %zu state interfaces", state_interfaces.size()); + + return state_interfaces; +} + +std::vector +KillswitchHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + + // Use the joint name from URDF + const std::string& name = info_.joints[0].name; + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(name, "kill_main_power", &cmd_kill_main_power_)); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(name, "kill_jetson_power", &cmd_kill_jetson_power_)); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(name, "kill_all_power", &cmd_kill_all_power_)); + + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Exported %zu command interfaces", command_interfaces.size()); + + return command_interfaces; +} + +hardware_interface::CallbackReturn KillswitchHardwareInterface::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Activating killswitch hardware..."); + + // Reset commands to safe state (don't kill anything) + cmd_kill_main_power_ = 0.0; + cmd_kill_jetson_power_ = 0.0; + cmd_kill_all_power_ = 0.0; + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn KillswitchHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Deactivating killswitch hardware..."); + + // Note: We intentionally do NOT kill power on deactivate + // The killswitch should only respond to explicit commands + // GPIO cleanup is done in on_cleanup + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn KillswitchHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Cleaning up killswitch hardware..."); + + // Cleanup GPIO - unexport and close file descriptors + if (button_fd_ >= 0) { + gpio_utils::cleanup_gpio(button_gpio_pin_, button_fd_); + button_fd_ = -1; + } + if (main_power_fd_ >= 0) { + gpio_utils::cleanup_gpio(main_power_gpio_pin_, main_power_fd_); + main_power_fd_ = -1; + } + if (jetson_power_fd_ >= 0) { + gpio_utils::cleanup_gpio(jetson_power_gpio_pin_, jetson_power_fd_); + jetson_power_fd_ = -1; + } + if (all_power_fd_ >= 0) { + gpio_utils::cleanup_gpio(all_power_gpio_pin_, all_power_fd_); + all_power_fd_ = -1; + } + + hw_connected_ = false; + is_connected_ = 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Killswitch hardware cleanup complete"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn KillswitchHardwareInterface::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Shutting down killswitch hardware..."); + + // Delegate to cleanup to release resources + return on_cleanup(previous_state); +} + +hardware_interface::return_type KillswitchHardwareInterface::read( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +{ + if (!hw_connected_) { + return hardware_interface::return_type::OK; + } + + // Read physical button state + if (button_fd_ >= 0) { + bool gpio_value = gpio_utils::read_gpio(button_fd_); + bool is_pressed = active_high_ ? gpio_value : !gpio_value; + + double previous_state = button_state_; + button_state_ = is_pressed ? 1.0 : 0.0; + + // Log state changes + if (is_pressed && previous_state < 0.5) { + RCLCPP_WARN( + rclcpp::get_logger("KillswitchHardwareInterface"), + "KILLSWITCH BUTTON PRESSED!"); + } else if (!is_pressed && previous_state > 0.5) { + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Killswitch button released"); + } + } + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type KillswitchHardwareInterface::write( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +{ + if (!hw_connected_) { + return hardware_interface::return_type::OK; + } + + // Process kill_main_power command + if (main_power_fd_ >= 0) { + bool should_kill = (cmd_kill_main_power_ > 0.5); + bool currently_killed = (main_power_killed_ > 0.5); + + if (should_kill != currently_killed) { + gpio_utils::write_gpio(main_power_fd_, should_kill); + main_power_killed_ = should_kill ? 1.0 : 0.0; + + if (should_kill) { + RCLCPP_WARN( + rclcpp::get_logger("KillswitchHardwareInterface"), + "MAIN POWER KILLED"); + } else { + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Main power restored"); + } + } + } + + // Process kill_jetson_power command + if (jetson_power_fd_ >= 0) { + bool should_kill = (cmd_kill_jetson_power_ > 0.5); + bool currently_killed = (jetson_power_killed_ > 0.5); + + if (should_kill != currently_killed) { + gpio_utils::write_gpio(jetson_power_fd_, should_kill); + jetson_power_killed_ = should_kill ? 1.0 : 0.0; + + if (should_kill) { + RCLCPP_WARN( + rclcpp::get_logger("KillswitchHardwareInterface"), + "JETSON POWER KILLED"); + } else { + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Jetson power restored"); + } + } + } + + // Process kill_all_power command + if (all_power_fd_ >= 0) { + bool should_kill = (cmd_kill_all_power_ > 0.5); + bool currently_killed = (all_power_killed_ > 0.5); + + if (should_kill != currently_killed) { + gpio_utils::write_gpio(all_power_fd_, should_kill); + all_power_killed_ = should_kill ? 1.0 : 0.0; + + if (should_kill) { + RCLCPP_ERROR( + rclcpp::get_logger("KillswitchHardwareInterface"), + "ALL POWER KILLED"); + } else { + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "All power restored"); + } + } + } + + return hardware_interface::return_type::OK; +} + +} // namespace killswitch_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + killswitch_ros2_control::KillswitchHardwareInterface, + hardware_interface::SystemInterface) + diff --git a/src/hardware_interfaces/laser_ros2_control/CMakeLists.txt b/src/hardware_interfaces/laser_ros2_control/CMakeLists.txt new file mode 100644 index 0000000..cca55ab --- /dev/null +++ b/src/hardware_interfaces/laser_ros2_control/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.8) +project(laser_ros2_control) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_auto REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +ament_auto_find_build_dependencies() + +include_directories(include) + +add_library( + laser_ros2_control + SHARED + src/laser_hardware_interface.cpp +) + +target_compile_features(laser_ros2_control PUBLIC cxx_std_20) +target_include_directories(laser_ros2_control PUBLIC +$ +$ +) + +ament_target_dependencies( + laser_ros2_control PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# Export hardware plugins +pluginlib_export_plugin_description_file(hardware_interface laser_hardware_interface.xml) + +# INSTALL +install( + DIRECTORY include/ + DESTINATION include/laser_ros2_control +) + +install(TARGETS laser_ros2_control + EXPORT export_laser_ros2_control + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +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() + +ament_export_targets(export_laser_ros2_control) +ament_export_include_directories(include) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/hardware_interfaces/laser_ros2_control/include/laser_ros2_control/laser_hardware_interface.hpp b/src/hardware_interfaces/laser_ros2_control/include/laser_ros2_control/laser_hardware_interface.hpp new file mode 100644 index 0000000..2144799 --- /dev/null +++ b/src/hardware_interfaces/laser_ros2_control/include/laser_ros2_control/laser_hardware_interface.hpp @@ -0,0 +1,118 @@ +// Copyright (c) 2024 UMD Loop +// +// 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 LASER_ROS2_CONTROL__LASER_HARDWARE_INTERFACE_HPP_ +#define LASER_ROS2_CONTROL__LASER_HARDWARE_INTERFACE_HPP_ + +#include +#include +#include + +#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 "rclcpp_lifecycle/state.hpp" + +namespace laser_ros2_control +{ + +// GPIO utility functions (Linux sysfs) +namespace gpio_utils +{ + int setup_gpio_output(int pin); + void cleanup_gpio(int pin, int fd); + bool write_gpio(int fd, bool value); +} + +/** + * @brief Hardware interface for GPIO-controlled laser via ros2_control + * + * This interface controls a spectrometry laser through GPIO pins. + * + * State Interfaces (read by controllers): + * - laser_state: 0.0 = OFF, 1.0 = ON + * - power_level: Current power level (0.0 to 1.0) + * - is_ready: Is hardware connected and ready (0.0 or 1.0) + * + * Command Interfaces (written by controllers): + * - laser_command: 0.0 = turn OFF, 1.0 = turn ON + * - power_command: Desired power level (0.0 to 1.0) + * + * Hardware Parameters (from URDF): + * - gpio_pin: GPIO pin number for laser control (required) + * - min_power: Minimum power level (default: 0.0) + * - max_power: Maximum power level (default: 1.0) + */ +class LaserHardwareInterface : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(LaserHardwareInterface) + + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + std::vector export_state_interfaces() override; + + std::vector export_command_interfaces() override; + + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::return_type read( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; + + hardware_interface::return_type write( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; + +private: + // Configuration parameters + int gpio_pin_; + double min_power_; + double max_power_; + + // State variables (hardware → ros2_control) + double laser_state_; // Current state: 0.0 = OFF, 1.0 = ON + double power_level_; // Current power level + double is_ready_; // Hardware ready status + + // Command variables (ros2_control → hardware) + double laser_command_; // Commanded state + double power_command_; // Commanded power level + + // GPIO interface + int gpio_fd_; + bool hw_connected_; +}; + +} // namespace laser_ros2_control + +#endif // LASER_ROS2_CONTROL__LASER_HARDWARE_INTERFACE_HPP_ + diff --git a/src/hardware_interfaces/laser_ros2_control/laser_hardware_interface.xml b/src/hardware_interfaces/laser_ros2_control/laser_hardware_interface.xml new file mode 100644 index 0000000..c0488a2 --- /dev/null +++ b/src/hardware_interfaces/laser_ros2_control/laser_hardware_interface.xml @@ -0,0 +1,10 @@ + + + + Hardware interface for spectrometry laser control via ros2_control. + Supports on/off control and power level via GPIO. + + + diff --git a/src/hardware_interfaces/laser_ros2_control/package.xml b/src/hardware_interfaces/laser_ros2_control/package.xml new file mode 100644 index 0000000..c3f5f95 --- /dev/null +++ b/src/hardware_interfaces/laser_ros2_control/package.xml @@ -0,0 +1,26 @@ + + + + laser_ros2_control + 0.1.0 + Laser hardware interface for ros2_control - GPIO-based spectrometry laser control + Dwarakesh Baraneetharan + Apache-2.0 + + ament_cmake + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + ament_lint_auto + ament_lint_common + + ament_cmake + + + ament_cmake + + diff --git a/src/hardware_interfaces/laser_ros2_control/src/laser_hardware_interface.cpp b/src/hardware_interfaces/laser_ros2_control/src/laser_hardware_interface.cpp new file mode 100644 index 0000000..15376f3 --- /dev/null +++ b/src/hardware_interfaces/laser_ros2_control/src/laser_hardware_interface.cpp @@ -0,0 +1,338 @@ +// Copyright (c) 2024 UMD Loop +// +// 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 "laser_ros2_control/laser_hardware_interface.hpp" + +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace laser_ros2_control +{ + +// GPIO utility functions implementation +namespace gpio_utils +{ + +int setup_gpio_output(int pin) +{ + // Export GPIO + int fd = ::open("/sys/class/gpio/export", O_WRONLY); + if (fd >= 0) { + std::string pin_str = std::to_string(pin); + ::write(fd, pin_str.c_str(), pin_str.length()); + ::close(fd); + usleep(100000); // Wait for GPIO to be exported + } + + // Set direction to output + std::stringstream direction_path; + direction_path << "/sys/class/gpio/gpio" << pin << "/direction"; + fd = ::open(direction_path.str().c_str(), O_WRONLY); + if (fd < 0) { + return -1; + } + ::write(fd, "out", 3); + ::close(fd); + + // Open value file + std::stringstream value_path; + value_path << "/sys/class/gpio/gpio" << pin << "/value"; + int value_fd = ::open(value_path.str().c_str(), O_RDWR); + + return value_fd; +} + +void cleanup_gpio(int pin, int fd) +{ + if (fd >= 0) { + ::close(fd); + } + + // Unexport GPIO + int export_fd = ::open("/sys/class/gpio/unexport", O_WRONLY); + if (export_fd >= 0) { + std::string pin_str = std::to_string(pin); + ::write(export_fd, pin_str.c_str(), pin_str.length()); + ::close(export_fd); + } +} + +bool write_gpio(int fd, bool value) +{ + if (fd < 0) return false; + + char val = value ? '1' : '0'; + lseek(fd, 0, SEEK_SET); + return (::write(fd, &val, 1) == 1); +} + +} // namespace gpio_utils + +// Hardware Interface Implementation + +hardware_interface::CallbackReturn LaserHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // Initialize state variables + laser_state_ = 0.0; // OFF + power_level_ = 0.0; + is_ready_ = 0.0; + + // Initialize command variables + laser_command_ = 0.0; // OFF + power_command_ = 0.0; + + // Parse hardware parameters + if (!info_.hardware_parameters.count("gpio_pin")) { + RCLCPP_ERROR( + rclcpp::get_logger("LaserHardwareInterface"), + "gpio_pin parameter is required"); + return hardware_interface::CallbackReturn::ERROR; + } + gpio_pin_ = std::stoi(info_.hardware_parameters.at("gpio_pin")); + + if (info_.hardware_parameters.count("min_power")) { + min_power_ = std::stod(info_.hardware_parameters.at("min_power")); + } else { + min_power_ = 0.0; + } + + if (info_.hardware_parameters.count("max_power")) { + max_power_ = std::stod(info_.hardware_parameters.at("max_power")); + } else { + max_power_ = 1.0; + } + + gpio_fd_ = -1; + hw_connected_ = false; + + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Initialized laser on GPIO pin %d", gpio_pin_); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LaserHardwareInterface::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Configuring laser hardware..."); + + // Setup GPIO output + gpio_fd_ = gpio_utils::setup_gpio_output(gpio_pin_); + if (gpio_fd_ < 0) { + RCLCPP_ERROR( + rclcpp::get_logger("LaserHardwareInterface"), + "Failed to setup GPIO output on pin %d", gpio_pin_); + return hardware_interface::CallbackReturn::ERROR; + } + + hw_connected_ = true; + is_ready_ = 1.0; + + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Successfully configured laser hardware"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector +LaserHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + + // Use the joint name from URDF + const std::string& name = info_.joints[0].name; + + // Laser state (ON/OFF) + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "laser_state", &laser_state_)); + + // Power level + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "power_level", &power_level_)); + + // Hardware ready status + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "is_ready", &is_ready_)); + + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Exported %zu state interfaces", state_interfaces.size()); + + return state_interfaces; +} + +std::vector +LaserHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + + // Use the joint name from URDF + const std::string& name = info_.joints[0].name; + + // Laser command (ON/OFF) + command_interfaces.emplace_back( + hardware_interface::CommandInterface(name, "laser_command", &laser_command_)); + + // Power command + command_interfaces.emplace_back( + hardware_interface::CommandInterface(name, "power_command", &power_command_)); + + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Exported %zu command interfaces", command_interfaces.size()); + + return command_interfaces; +} + +hardware_interface::CallbackReturn LaserHardwareInterface::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Activating laser hardware..."); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LaserHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Deactivating laser hardware..."); + + // Safety: Turn laser OFF on deactivation + if (hw_connected_ && gpio_fd_ >= 0) { + gpio_utils::write_gpio(gpio_fd_, false); + laser_state_ = 0.0; + power_level_ = 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Laser turned OFF (safety)"); + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LaserHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Cleaning up laser hardware..."); + + // Ensure laser is OFF before cleanup + if (gpio_fd_ >= 0) { + gpio_utils::write_gpio(gpio_fd_, false); + } + + // Cleanup GPIO - unexport and close file descriptor + if (gpio_fd_ >= 0) { + gpio_utils::cleanup_gpio(gpio_pin_, gpio_fd_); + gpio_fd_ = -1; + } + + hw_connected_ = false; + is_ready_ = 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Laser hardware cleanup complete"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LaserHardwareInterface::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Shutting down laser hardware..."); + + // Delegate to cleanup to release resources + return on_cleanup(previous_state); +} + +hardware_interface::return_type LaserHardwareInterface::read( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +{ + // Laser state is known from commands, no need to read from GPIO + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type LaserHardwareInterface::write( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +{ + if (!hw_connected_ || gpio_fd_ < 0) { + return hardware_interface::return_type::OK; + } + + // Check if laser command changed + bool commanded_on = (laser_command_ > 0.5); + bool currently_on = (laser_state_ > 0.5); + + if (commanded_on != currently_on) { + if (gpio_utils::write_gpio(gpio_fd_, commanded_on)) { + laser_state_ = commanded_on ? 1.0 : 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Laser turned %s", commanded_on ? "ON" : "OFF"); + } + } + + // Update power level (clamped to min/max) + double commanded_power = power_command_; + if (commanded_power < min_power_) commanded_power = min_power_; + if (commanded_power > max_power_) commanded_power = max_power_; + + if (std::abs(commanded_power - power_level_) > 0.01) { + power_level_ = commanded_power; + + RCLCPP_DEBUG( + rclcpp::get_logger("LaserHardwareInterface"), + "Power level set to %.2f", power_level_); + } + + return hardware_interface::return_type::OK; +} + +} // namespace laser_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + laser_ros2_control::LaserHardwareInterface, + hardware_interface::SystemInterface) + diff --git a/src/hardware_interfaces/led_ros2_control/CMakeLists.txt b/src/hardware_interfaces/led_ros2_control/CMakeLists.txt new file mode 100644 index 0000000..0e3b4e4 --- /dev/null +++ b/src/hardware_interfaces/led_ros2_control/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.8) +project(led_ros2_control) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_auto REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +ament_auto_find_build_dependencies() + +include_directories(include) + +add_library( + led_ros2_control + SHARED + src/led_hardware_interface.cpp +) + +target_compile_features(led_ros2_control PUBLIC cxx_std_20) +target_include_directories(led_ros2_control PUBLIC +$ +$ +) + +ament_target_dependencies( + led_ros2_control PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# Export hardware plugins +pluginlib_export_plugin_description_file(hardware_interface led_hardware_interface.xml) + +# INSTALL +install( + DIRECTORY include/ + DESTINATION include/led_ros2_control +) + +install(TARGETS led_ros2_control + EXPORT export_led_ros2_control + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +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() + +ament_export_targets(export_led_ros2_control) +ament_export_include_directories(include) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/hardware_interfaces/led_ros2_control/include/led_ros2_control/led_hardware_interface.hpp b/src/hardware_interfaces/led_ros2_control/include/led_ros2_control/led_hardware_interface.hpp new file mode 100644 index 0000000..7919ed5 --- /dev/null +++ b/src/hardware_interfaces/led_ros2_control/include/led_ros2_control/led_hardware_interface.hpp @@ -0,0 +1,112 @@ +// Copyright (c) 2024 UMD Loop +// +// 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 LED_ROS2_CONTROL__LED_HARDWARE_INTERFACE_HPP_ +#define LED_ROS2_CONTROL__LED_HARDWARE_INTERFACE_HPP_ + +#include +#include +#include + +#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 "rclcpp_lifecycle/state.hpp" + +namespace led_ros2_control +{ + +// GPIO utility functions (Linux sysfs) +namespace gpio_utils +{ + int setup_gpio_output(int pin); + void cleanup_gpio(int pin, int fd); + bool write_gpio(int fd, bool value); +} + +/** + * @brief Hardware interface for LED control via ros2_control + * + * This is a SystemInterface for controlling status LEDs through GPIO. + * + * State Interfaces (read by controllers): + * - led_state: Current LED state (0.0 = OFF, 1.0 = ON) + * - is_connected: Is hardware connected and ready (0.0 or 1.0) + * + * Command Interfaces (written by controllers): + * - led_command: LED command (0.0 = turn OFF, 1.0 = turn ON) + * + * Hardware Parameters (from URDF): + * - gpio_pin: GPIO pin number for LED output (required) + * - default_state: "on" or "off" (default: "off") + */ +class LEDHardwareInterface : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(LEDHardwareInterface) + + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + std::vector export_state_interfaces() override; + + std::vector export_command_interfaces() override; + + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::return_type read( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; + + hardware_interface::return_type write( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; + +private: + // Configuration parameters + int gpio_pin_; + bool default_state_; + + // State variables (hardware → ros2_control) + double led_state_; // Current state: 0.0 = OFF, 1.0 = ON + double is_connected_; // Hardware ready status + + // Command variables (ros2_control → hardware) + double led_command_; // Commanded state + + // Hardware interface + int gpio_fd_; + bool hw_connected_; +}; + +} // namespace led_ros2_control + +#endif // LED_ROS2_CONTROL__LED_HARDWARE_INTERFACE_HPP_ + diff --git a/src/hardware_interfaces/led_ros2_control/led_hardware_interface.xml b/src/hardware_interfaces/led_ros2_control/led_hardware_interface.xml new file mode 100644 index 0000000..eac679c --- /dev/null +++ b/src/hardware_interfaces/led_ros2_control/led_hardware_interface.xml @@ -0,0 +1,10 @@ + + + + LED status indicator hardware interface via ros2_control. + GPIO output for controlling status LEDs. + + + diff --git a/src/hardware_interfaces/led_ros2_control/package.xml b/src/hardware_interfaces/led_ros2_control/package.xml new file mode 100644 index 0000000..10258b2 --- /dev/null +++ b/src/hardware_interfaces/led_ros2_control/package.xml @@ -0,0 +1,26 @@ + + + + led_ros2_control + 0.1.0 + LED hardware interface for ros2_control - GPIO-based status LED control + Dwarakesh Baraneetharan + Apache-2.0 + + ament_cmake + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + ament_lint_auto + ament_lint_common + + ament_cmake + + + ament_cmake + + diff --git a/src/hardware_interfaces/led_ros2_control/src/led_hardware_interface.cpp b/src/hardware_interfaces/led_ros2_control/src/led_hardware_interface.cpp new file mode 100644 index 0000000..7413fe6 --- /dev/null +++ b/src/hardware_interfaces/led_ros2_control/src/led_hardware_interface.cpp @@ -0,0 +1,312 @@ +// Copyright (c) 2024 UMD Loop +// +// 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 "led_ros2_control/led_hardware_interface.hpp" + +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace led_ros2_control +{ + +// GPIO utility functions implementation +namespace gpio_utils +{ + +int setup_gpio_output(int pin) +{ + // Export GPIO + int fd = ::open("/sys/class/gpio/export", O_WRONLY); + if (fd >= 0) { + std::string pin_str = std::to_string(pin); + ::write(fd, pin_str.c_str(), pin_str.length()); + ::close(fd); + usleep(100000); // Wait for GPIO to be exported + } + + // Set direction to output + std::stringstream direction_path; + direction_path << "/sys/class/gpio/gpio" << pin << "/direction"; + fd = ::open(direction_path.str().c_str(), O_WRONLY); + if (fd < 0) { + return -1; + } + ::write(fd, "out", 3); + ::close(fd); + + // Open value file + std::stringstream value_path; + value_path << "/sys/class/gpio/gpio" << pin << "/value"; + int value_fd = ::open(value_path.str().c_str(), O_RDWR); + + return value_fd; +} + +void cleanup_gpio(int pin, int fd) +{ + if (fd >= 0) { + ::close(fd); + } + + // Unexport GPIO + int export_fd = ::open("/sys/class/gpio/unexport", O_WRONLY); + if (export_fd >= 0) { + std::string pin_str = std::to_string(pin); + ::write(export_fd, pin_str.c_str(), pin_str.length()); + ::close(export_fd); + } +} + +bool write_gpio(int fd, bool value) +{ + if (fd < 0) return false; + + char val = value ? '1' : '0'; + lseek(fd, 0, SEEK_SET); + return (::write(fd, &val, 1) == 1); +} + +} // namespace gpio_utils + +// Hardware Interface Implementation + +hardware_interface::CallbackReturn LEDHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // Parse hardware parameters + if (!info_.hardware_parameters.count("gpio_pin")) { + RCLCPP_ERROR( + rclcpp::get_logger("LEDHardwareInterface"), + "gpio_pin parameter is required"); + return hardware_interface::CallbackReturn::ERROR; + } + gpio_pin_ = std::stoi(info_.hardware_parameters.at("gpio_pin")); + + // Default state + if (info_.hardware_parameters.count("default_state")) { + default_state_ = (info_.hardware_parameters.at("default_state") == "on"); + } else { + default_state_ = false; // OFF by default + } + + // Initialize state variables + led_state_ = default_state_ ? 1.0 : 0.0; + is_connected_ = 0.0; + + // Initialize command variables + led_command_ = default_state_ ? 1.0 : 0.0; + + gpio_fd_ = -1; + hw_connected_ = false; + + RCLCPP_INFO( + rclcpp::get_logger("LEDHardwareInterface"), + "Initialized LED on GPIO pin %d (default: %s)", + gpio_pin_, default_state_ ? "ON" : "OFF"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LEDHardwareInterface::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("LEDHardwareInterface"), + "Configuring LED hardware..."); + + // Setup GPIO output + gpio_fd_ = gpio_utils::setup_gpio_output(gpio_pin_); + if (gpio_fd_ < 0) { + RCLCPP_ERROR( + rclcpp::get_logger("LEDHardwareInterface"), + "Failed to setup GPIO output on pin %d", gpio_pin_); + return hardware_interface::CallbackReturn::ERROR; + } + + hw_connected_ = true; + is_connected_ = 1.0; + + // Set initial state + gpio_utils::write_gpio(gpio_fd_, default_state_); + + RCLCPP_INFO( + rclcpp::get_logger("LEDHardwareInterface"), + "Successfully configured LED hardware"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector +LEDHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + + // Use the joint name from URDF + const std::string& name = info_.joints[0].name; + + // LED state + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "led_state", &led_state_)); + + // Connection status + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "is_connected", &is_connected_)); + + RCLCPP_INFO( + rclcpp::get_logger("LEDHardwareInterface"), + "Exported %zu state interfaces", state_interfaces.size()); + + return state_interfaces; +} + +std::vector +LEDHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + + // Use the joint name from URDF + const std::string& name = info_.joints[0].name; + + // LED command + command_interfaces.emplace_back( + hardware_interface::CommandInterface(name, "led_command", &led_command_)); + + RCLCPP_INFO( + rclcpp::get_logger("LEDHardwareInterface"), + "Exported %zu command interfaces", command_interfaces.size()); + + return command_interfaces; +} + +hardware_interface::CallbackReturn LEDHardwareInterface::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("LEDHardwareInterface"), + "Activating LED hardware..."); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LEDHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("LEDHardwareInterface"), + "Deactivating LED hardware..."); + + // Turn LED off on deactivation (safety) + if (hw_connected_ && gpio_fd_ >= 0) { + gpio_utils::write_gpio(gpio_fd_, false); + led_state_ = 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("LEDHardwareInterface"), + "LED turned OFF (deactivation)"); + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LEDHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("LEDHardwareInterface"), + "Cleaning up LED hardware..."); + + // Ensure LED is OFF before cleanup + if (gpio_fd_ >= 0) { + gpio_utils::write_gpio(gpio_fd_, false); + } + + // Cleanup GPIO - unexport and close file descriptor + if (gpio_fd_ >= 0) { + gpio_utils::cleanup_gpio(gpio_pin_, gpio_fd_); + gpio_fd_ = -1; + } + + hw_connected_ = false; + is_connected_ = 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("LEDHardwareInterface"), + "LED hardware cleanup complete"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LEDHardwareInterface::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + RCLCPP_INFO( + rclcpp::get_logger("LEDHardwareInterface"), + "Shutting down LED hardware..."); + + // Delegate to cleanup to release resources + return on_cleanup(previous_state); +} + +hardware_interface::return_type LEDHardwareInterface::read( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +{ + // LED state is known from commands, no need to read from GPIO + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type LEDHardwareInterface::write( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +{ + if (!hw_connected_ || gpio_fd_ < 0) { + return hardware_interface::return_type::OK; + } + + // Check if LED command changed + bool commanded_on = (led_command_ > 0.5); + bool currently_on = (led_state_ > 0.5); + + if (commanded_on != currently_on) { + if (gpio_utils::write_gpio(gpio_fd_, commanded_on)) { + led_state_ = commanded_on ? 1.0 : 0.0; + + RCLCPP_DEBUG( + rclcpp::get_logger("LEDHardwareInterface"), + "LED turned %s", commanded_on ? "ON" : "OFF"); + } + } + + return hardware_interface::return_type::OK; +} + +} // namespace led_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + led_ros2_control::LEDHardwareInterface, + hardware_interface::SystemInterface) + diff --git a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml index af691e7..f355cd9 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -36,6 +36,27 @@ controller_manager: science_controller: type: science_controllers/ScienceManual + # Laser Controllers + laser_onoff_controller: + type: forward_command_controller/ForwardCommandController + + laser_power_controller: + type: forward_command_controller/ForwardCommandController + + # Killswitch Controllers (one per relay) + killswitch_main_controller: + type: forward_command_controller/ForwardCommandController + + killswitch_jetson_controller: + type: forward_command_controller/ForwardCommandController + + killswitch_all_controller: + type: forward_command_controller/ForwardCommandController + + # LED Controller + led_controller: + type: forward_command_controller/ForwardCommandController + joint_group_position_controller: ros__parameters: joints: @@ -85,4 +106,43 @@ science_controller: cap: "cap" interface_name: position command_interfaces: [position, velocity] - state_interfaces: [position, velocity] \ No newline at end of file + state_interfaces: [position, velocity] + +# Laser Controller Configurations +laser_onoff_controller: + ros__parameters: + joints: + - spectrometry_laser + interface_name: laser_command + +laser_power_controller: + ros__parameters: + joints: + - spectrometry_laser + interface_name: power_command + +# Killswitch Controller Configurations +killswitch_main_controller: + ros__parameters: + joints: + - drive_killswitch + interface_name: kill_main_power + +killswitch_jetson_controller: + ros__parameters: + joints: + - drive_killswitch + interface_name: kill_jetson_power + +killswitch_all_controller: + ros__parameters: + joints: + - drive_killswitch + interface_name: kill_all_power + +# LED Controller Configuration +led_controller: + ros__parameters: + joints: + - status_led + interface_name: led_command \ No newline at end of file