From 0d58371cf80ed46478e7d5ff80d756995700d26f Mon Sep 17 00:00:00 2001 From: Dwarakesh Baraneetharan Date: Tue, 6 Jan 2026 16:15:16 -0500 Subject: [PATCH 1/3] Add laser, killswitch, and LED hardware interfaces --- .../science.killswitch.ros2_control.xacro | 33 ++ .../science/science.laser.ros2_control.xacro | 28 + .../science/science.led.ros2_control.xacro | 25 + .../urdf/athena_science.urdf.xacro | 19 +- src/hardware_interfaces/README.md | 8 +- .../killswitch_ros2_control/CMakeLists.txt | 69 +++ .../killswitch_hardware_interface.hpp | 134 +++++ .../killswitch_hardware_interface.xml | 10 + .../killswitch_ros2_control/package.xml | 26 + .../src/killswitch_hardware_interface.cpp | 483 ++++++++++++++++++ .../laser_ros2_control/CMakeLists.txt | 69 +++ .../laser_hardware_interface.hpp | 118 +++++ .../laser_hardware_interface.xml | 10 + .../laser_ros2_control/package.xml | 26 + .../src/laser_hardware_interface.cpp | 338 ++++++++++++ .../led_ros2_control/CMakeLists.txt | 69 +++ .../led_hardware_interface.hpp | 112 ++++ .../led_hardware_interface.xml | 10 + .../led_ros2_control/package.xml | 26 + .../src/led_hardware_interface.cpp | 312 +++++++++++ .../config/athena_science_controllers.yaml | 62 ++- 21 files changed, 1983 insertions(+), 4 deletions(-) create mode 100644 src/description/ros2_control/science/science.killswitch.ros2_control.xacro create mode 100644 src/description/ros2_control/science/science.laser.ros2_control.xacro create mode 100644 src/description/ros2_control/science/science.led.ros2_control.xacro create mode 100644 src/hardware_interfaces/killswitch_ros2_control/CMakeLists.txt create mode 100644 src/hardware_interfaces/killswitch_ros2_control/include/killswitch_ros2_control/killswitch_hardware_interface.hpp create mode 100644 src/hardware_interfaces/killswitch_ros2_control/killswitch_hardware_interface.xml create mode 100644 src/hardware_interfaces/killswitch_ros2_control/package.xml create mode 100644 src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp create mode 100644 src/hardware_interfaces/laser_ros2_control/CMakeLists.txt create mode 100644 src/hardware_interfaces/laser_ros2_control/include/laser_ros2_control/laser_hardware_interface.hpp create mode 100644 src/hardware_interfaces/laser_ros2_control/laser_hardware_interface.xml create mode 100644 src/hardware_interfaces/laser_ros2_control/package.xml create mode 100644 src/hardware_interfaces/laser_ros2_control/src/laser_hardware_interface.cpp create mode 100644 src/hardware_interfaces/led_ros2_control/CMakeLists.txt create mode 100644 src/hardware_interfaces/led_ros2_control/include/led_ros2_control/led_hardware_interface.hpp create mode 100644 src/hardware_interfaces/led_ros2_control/led_hardware_interface.xml create mode 100644 src/hardware_interfaces/led_ros2_control/package.xml create mode 100644 src/hardware_interfaces/led_ros2_control/src/led_hardware_interface.cpp 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 2965b2c..15e4412 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: @@ -92,4 +113,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 From fce8acc4eaa3a990f0d30c5f4204d76c8f313fb7 Mon Sep 17 00:00:00 2001 From: Dwarakesh Baraneetharan Date: Mon, 26 Jan 2026 16:39:08 -0500 Subject: [PATCH 2/3] refactor: Laser/Killswitch to CAN, move LED/Killswitch to drive Address team lead feedback: Laser and Killswitch now use CAN protocol instead of GPIO. LED stays GPIO. All use gpio tags and gpio_command_controller. LED and Killswitch moved from science to drive subsystem. Added simulation fallback for development. --- .../drive/drive.killswitch.ros2_control.xacro | 28 ++ .../drive/drive.led.ros2_control.xacro | 23 + .../science.killswitch.ros2_control.xacro | 33 -- .../science/science.laser.ros2_control.xacro | 17 +- .../science/science.led.ros2_control.xacro | 25 -- src/description/urdf/athena_drive.urdf.xacro | 12 + .../urdf/athena_science.urdf.xacro | 17 +- .../killswitch_ros2_control/CMakeLists.txt | 10 +- .../killswitch_hardware_interface.hpp | 101 +++-- .../killswitch_hardware_interface.xml | 4 +- .../killswitch_ros2_control/package.xml | 6 +- .../src/killswitch_hardware_interface.cpp | 404 +++++------------- .../laser_ros2_control/CMakeLists.txt | 10 +- .../laser_hardware_interface.hpp | 61 +-- .../laser_hardware_interface.xml | 2 +- .../laser_ros2_control/package.xml | 6 +- .../src/laser_hardware_interface.cpp | 224 ++++------ .../led_ros2_control/CMakeLists.txt | 1 + .../led_hardware_interface.hpp | 1 + .../src/led_hardware_interface.cpp | 49 ++- .../config/athena_drive_controllers.yaml | 42 ++ .../config/athena_science_controllers.yaml | 72 +--- 22 files changed, 456 insertions(+), 692 deletions(-) create mode 100644 src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro create mode 100644 src/description/ros2_control/drive/drive.led.ros2_control.xacro delete mode 100644 src/description/ros2_control/science/science.killswitch.ros2_control.xacro delete mode 100644 src/description/ros2_control/science/science.led.ros2_control.xacro diff --git a/src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro b/src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro new file mode 100644 index 0000000..334b857 --- /dev/null +++ b/src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro @@ -0,0 +1,28 @@ + + + + + + + + + killswitch_ros2_control/KillswitchHardwareInterface + ${can_interface} + ${can_id} + + + + + + + + + + + + + + + + + diff --git a/src/description/ros2_control/drive/drive.led.ros2_control.xacro b/src/description/ros2_control/drive/drive.led.ros2_control.xacro new file mode 100644 index 0000000..b560b89 --- /dev/null +++ b/src/description/ros2_control/drive/drive.led.ros2_control.xacro @@ -0,0 +1,23 @@ + + + + + + + + led_ros2_control/LEDHardwareInterface + ${gpio_pin} + ${default_state} + + + + + + + + + + + + + diff --git a/src/description/ros2_control/science/science.killswitch.ros2_control.xacro b/src/description/ros2_control/science/science.killswitch.ros2_control.xacro deleted file mode 100644 index 822a298..0000000 --- a/src/description/ros2_control/science/science.killswitch.ros2_control.xacro +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - 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 index f981886..50719bb 100644 --- a/src/description/ros2_control/science/science.laser.ros2_control.xacro +++ b/src/description/ros2_control/science/science.laser.ros2_control.xacro @@ -1,24 +1,23 @@ - + + laser_ros2_control/LaserHardwareInterface - ${gpio_pin} - ${min_power} - ${max_power} + ${can_interface} + ${can_id} - + - - + + - - + diff --git a/src/description/ros2_control/science/science.led.ros2_control.xacro b/src/description/ros2_control/science/science.led.ros2_control.xacro deleted file mode 100644 index cb96abc..0000000 --- a/src/description/ros2_control/science/science.led.ros2_control.xacro +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - led_ros2_control/LEDHardwareInterface - ${gpio_pin} - ${default_state} - - - - - - - - - - - - - - diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index 9deb819..d2d0a0c 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -36,6 +36,12 @@ + + + + + + @@ -66,6 +72,12 @@ + + + + + + diff --git a/src/description/urdf/athena_science.urdf.xacro b/src/description/urdf/athena_science.urdf.xacro index b88a75f..80fc531 100644 --- a/src/description/urdf/athena_science.urdf.xacro +++ b/src/description/urdf/athena_science.urdf.xacro @@ -10,8 +10,6 @@ - - @@ -35,19 +33,8 @@ - - - - - - - - + + \ No newline at end of file diff --git a/src/hardware_interfaces/killswitch_ros2_control/CMakeLists.txt b/src/hardware_interfaces/killswitch_ros2_control/CMakeLists.txt index e3eb609..4a42db5 100644 --- a/src/hardware_interfaces/killswitch_ros2_control/CMakeLists.txt +++ b/src/hardware_interfaces/killswitch_ros2_control/CMakeLists.txt @@ -10,10 +10,12 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib rclcpp rclcpp_lifecycle + umdloop_can_library ) # find dependencies find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(ament_cmake_auto REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -35,9 +37,15 @@ $ $ ) +# Link CAN library +target_link_libraries(killswitch_ros2_control PUBLIC umdloop_can_library::umdloop_can_library) + ament_target_dependencies( killswitch_ros2_control PUBLIC - ${THIS_PACKAGE_INCLUDE_DEPENDS} + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle ) # Export hardware plugins 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 index 421e6c2..4636862 100644 --- 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 @@ -19,51 +19,47 @@ #include #include #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/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -namespace killswitch_ros2_control -{ +#include "umdloop_can_library/SocketCanBus.hpp" +#include "umdloop_can_library/CanFrame.hpp" -// GPIO utility functions (Linux sysfs) -namespace gpio_utils +namespace killswitch_ros2_control { - 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 + * @brief Hardware interface for CAN-controlled killswitch 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. + * This interface sends kill commands to the voltage monitoring board via CAN. + * + * CAN Protocol (ID: 0x100): + * - Kill All Power: DATA[0] = 0x01 + * - Kill Main Power: DATA[0] = 0x03 + * - Kill Jetson Power: DATA[0] = 0x05 * * 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 + * - kill_all_sent: 1.0 if kill all command was sent + * - kill_main_sent: 1.0 if kill main command was sent + * - kill_jetson_sent: 1.0 if kill jetson command was sent + * - is_connected: Is CAN connected (0.0 or 1.0) * * 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 + * - kill_all: Set to 1.0 to kill all power + * - kill_main: Set to 1.0 to kill main power + * - kill_jetson: Set to 1.0 to kill jetson power * * 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) + * - can_interface: CAN interface name (default: "can0") + * - can_id: CAN ID for killswitch commands (default: 0x100) */ class KillswitchHardwareInterface : public hardware_interface::SystemInterface { @@ -101,34 +97,35 @@ class KillswitchHardwareInterface : public hardware_interface::SystemInterface 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_; + // CAN message handler + void onCanMessage(const CANLib::CanFrame& frame); + + // Configuration parameters + std::string can_interface_; + uint32_t can_id_; + + // CAN bus + CANLib::SocketCanBus canBus_; + CANLib::CanFrame can_tx_frame_; + bool can_connected_; + + // State variables (track what was sent) + double kill_all_sent_; + double kill_main_sent_; + double kill_jetson_sent_; + double is_connected_; + + // Command variables + double cmd_kill_all_; + double cmd_kill_main_; + double cmd_kill_jetson_; + + // CAN command bytes + static constexpr uint8_t CMD_KILL_ALL = 0x01; + static constexpr uint8_t CMD_KILL_MAIN = 0x03; + static constexpr uint8_t CMD_KILL_JETSON = 0x05; }; } // 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 index 9fd8e26..9162004 100644 --- a/src/hardware_interfaces/killswitch_ros2_control/killswitch_hardware_interface.xml +++ b/src/hardware_interfaces/killswitch_ros2_control/killswitch_hardware_interface.xml @@ -3,8 +3,8 @@ type="killswitch_ros2_control::KillswitchHardwareInterface" base_class_type="hardware_interface::SystemInterface"> - Drive killswitch hardware interface via ros2_control. - Monitors physical killswitch button and controls power kill relays. + Killswitch hardware interface via ros2_control. + CAN-based power kill commands for main, jetson, and all systems. diff --git a/src/hardware_interfaces/killswitch_ros2_control/package.xml b/src/hardware_interfaces/killswitch_ros2_control/package.xml index 37f7409..afa1bc2 100644 --- a/src/hardware_interfaces/killswitch_ros2_control/package.xml +++ b/src/hardware_interfaces/killswitch_ros2_control/package.xml @@ -3,7 +3,7 @@ killswitch_ros2_control 0.1.0 - Killswitch hardware interface for ros2_control - GPIO-based drive killswitch system + Killswitch hardware interface for ros2_control - CAN-based power killswitch system Dwarakesh Baraneetharan Apache-2.0 @@ -15,6 +15,10 @@ rclcpp rclcpp_lifecycle + + umdloop_can_library + umdloop_can_library + ament_lint_auto ament_lint_common 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 index dedb805..972889c 100644 --- a/src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp +++ b/src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp @@ -15,118 +15,12 @@ #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) { @@ -136,57 +30,36 @@ hardware_interface::CallbackReturn KillswitchHardwareInterface::on_init( 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; + // Initialize state variables + kill_all_sent_ = 0.0; + kill_main_sent_ = 0.0; + kill_jetson_sent_ = 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; + // Initialize command variables + cmd_kill_all_ = 0.0; + cmd_kill_main_ = 0.0; + cmd_kill_jetson_ = 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; + if (info_.hardware_parameters.count("can_interface")) { + can_interface_ = info_.hardware_parameters.at("can_interface"); + } else { + can_interface_ = "can0"; } - // Active high/low for button (default: true) - if (info_.hardware_parameters.count("active_high")) { - active_high_ = (info_.hardware_parameters.at("active_high") == "true"); + if (info_.hardware_parameters.count("can_id")) { + can_id_ = static_cast(std::stoi(info_.hardware_parameters.at("can_id"))); } else { - active_high_ = true; + can_id_ = 0x100; // Default killswitch CAN ID } - // Initialize file descriptors - button_fd_ = -1; - main_power_fd_ = -1; - jetson_power_fd_ = -1; - all_power_fd_ = -1; - hw_connected_ = false; + can_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_); + "Initialized killswitch on CAN interface %s with ID 0x%X", + can_interface_.c_str(), can_id_); return hardware_interface::CallbackReturn::SUCCESS; } @@ -198,69 +71,51 @@ hardware_interface::CallbackReturn KillswitchHardwareInterface::on_configure( 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( + // Open CAN bus + if (!canBus_.open(can_interface_, + std::bind(&KillswitchHardwareInterface::onCanMessage, this, std::placeholders::_1))) + { + RCLCPP_WARN( 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( + "Failed to open CAN interface %s - running in SIMULATION mode", + can_interface_.c_str()); + can_connected_ = false; + } else { + can_connected_ = true; + RCLCPP_INFO( rclcpp::get_logger("KillswitchHardwareInterface"), - "Failed to setup all-power GPIO output on pin %d", all_power_gpio_pin_); - return hardware_interface::CallbackReturn::ERROR; + "Successfully opened CAN interface %s", can_interface_.c_str()); } - gpio_utils::write_gpio(all_power_fd_, false); // Initialize to LOW (power ON) - hw_connected_ = true; - is_connected_ = 1.0; + is_connected_ = can_connected_ ? 1.0 : 0.0; RCLCPP_INFO( rclcpp::get_logger("KillswitchHardwareInterface"), - "Successfully configured killswitch hardware"); + "Killswitch hardware configured (%s)", can_connected_ ? "CAN MODE" : "SIMULATION"); return hardware_interface::CallbackReturn::SUCCESS; } +void KillswitchHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) +{ + // Handle incoming CAN messages from voltage monitoring board if needed + (void)frame; // Currently not expecting responses +} + std::vector KillswitchHardwareInterface::export_state_interfaces() { std::vector state_interfaces; - // Use the joint name from URDF - const std::string& name = info_.joints[0].name; + // Use the gpio name from URDF + const std::string& name = info_.gpios[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_)); + hardware_interface::StateInterface(name, "kill_all_sent", &kill_all_sent_)); state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "jetson_power_killed", &jetson_power_killed_)); + hardware_interface::StateInterface(name, "kill_main_sent", &kill_main_sent_)); state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "all_power_killed", &all_power_killed_)); + hardware_interface::StateInterface(name, "kill_jetson_sent", &kill_jetson_sent_)); state_interfaces.emplace_back( hardware_interface::StateInterface(name, "is_connected", &is_connected_)); @@ -276,15 +131,15 @@ KillswitchHardwareInterface::export_command_interfaces() { std::vector command_interfaces; - // Use the joint name from URDF - const std::string& name = info_.joints[0].name; + // Use the gpio name from URDF + const std::string& name = info_.gpios[0].name; command_interfaces.emplace_back( - hardware_interface::CommandInterface(name, "kill_main_power", &cmd_kill_main_power_)); + hardware_interface::CommandInterface(name, "kill_all", &cmd_kill_all_)); command_interfaces.emplace_back( - hardware_interface::CommandInterface(name, "kill_jetson_power", &cmd_kill_jetson_power_)); + hardware_interface::CommandInterface(name, "kill_main", &cmd_kill_main_)); command_interfaces.emplace_back( - hardware_interface::CommandInterface(name, "kill_all_power", &cmd_kill_all_power_)); + hardware_interface::CommandInterface(name, "kill_jetson", &cmd_kill_jetson_)); RCLCPP_INFO( rclcpp::get_logger("KillswitchHardwareInterface"), @@ -300,10 +155,10 @@ hardware_interface::CallbackReturn KillswitchHardwareInterface::on_activate( 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; + // Reset commands to safe state + cmd_kill_all_ = 0.0; + cmd_kill_main_ = 0.0; + cmd_kill_jetson_ = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } @@ -315,9 +170,8 @@ hardware_interface::CallbackReturn KillswitchHardwareInterface::on_deactivate( 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 + // Note: We intentionally do NOT send kill commands on deactivate + // Killswitch should only respond to explicit commands return hardware_interface::CallbackReturn::SUCCESS; } @@ -329,25 +183,11 @@ hardware_interface::CallbackReturn KillswitchHardwareInterface::on_cleanup( 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; + if (can_connected_) { + canBus_.close(); } - hw_connected_ = false; + can_connected_ = false; is_connected_ = 0.0; RCLCPP_INFO( @@ -364,7 +204,6 @@ hardware_interface::CallbackReturn KillswitchHardwareInterface::on_shutdown( rclcpp::get_logger("KillswitchHardwareInterface"), "Shutting down killswitch hardware..."); - // Delegate to cleanup to release resources return on_cleanup(previous_state); } @@ -372,30 +211,7 @@ 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"); - } - } - + // CAN messages are handled asynchronously via callback return hardware_interface::return_type::OK; } @@ -403,71 +219,58 @@ 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"); - } + // Check kill_all command (rising edge detection) + if (cmd_kill_all_ > 0.5 && kill_all_sent_ < 0.5) { + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_id_; + can_tx_frame_.dlc = 1; + can_tx_frame_.data[0] = CMD_KILL_ALL; + canBus_.send(can_tx_frame_); } + + kill_all_sent_ = 1.0; + RCLCPP_ERROR( + rclcpp::get_logger("KillswitchHardwareInterface"), + "KILL ALL POWER COMMAND SENT%s", can_connected_ ? "" : " (simulated)"); + } else if (cmd_kill_all_ < 0.5) { + kill_all_sent_ = 0.0; // Reset when command goes low } - // 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"); - } + // Check kill_main command (rising edge detection) + if (cmd_kill_main_ > 0.5 && kill_main_sent_ < 0.5) { + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_id_; + can_tx_frame_.dlc = 1; + can_tx_frame_.data[0] = CMD_KILL_MAIN; + canBus_.send(can_tx_frame_); } + + kill_main_sent_ = 1.0; + RCLCPP_WARN( + rclcpp::get_logger("KillswitchHardwareInterface"), + "KILL MAIN POWER COMMAND SENT%s", can_connected_ ? "" : " (simulated)"); + } else if (cmd_kill_main_ < 0.5) { + kill_main_sent_ = 0.0; // Reset when command goes low } - // 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"); - } + // Check kill_jetson command (rising edge detection) + if (cmd_kill_jetson_ > 0.5 && kill_jetson_sent_ < 0.5) { + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_id_; + can_tx_frame_.dlc = 1; + can_tx_frame_.data[0] = CMD_KILL_JETSON; + canBus_.send(can_tx_frame_); } + + kill_jetson_sent_ = 1.0; + RCLCPP_WARN( + rclcpp::get_logger("KillswitchHardwareInterface"), + "KILL JETSON POWER COMMAND SENT%s", can_connected_ ? "" : " (simulated)"); + } else if (cmd_kill_jetson_ < 0.5) { + kill_jetson_sent_ = 0.0; // Reset when command goes low } return hardware_interface::return_type::OK; @@ -480,4 +283,3 @@ hardware_interface::return_type KillswitchHardwareInterface::write( 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 index cca55ab..3c301d5 100644 --- a/src/hardware_interfaces/laser_ros2_control/CMakeLists.txt +++ b/src/hardware_interfaces/laser_ros2_control/CMakeLists.txt @@ -10,10 +10,12 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib rclcpp rclcpp_lifecycle + umdloop_can_library ) # find dependencies find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(ament_cmake_auto REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -35,9 +37,15 @@ $ $ ) +# Link CAN library +target_link_libraries(laser_ros2_control PUBLIC umdloop_can_library::umdloop_can_library) + ament_target_dependencies( laser_ros2_control PUBLIC - ${THIS_PACKAGE_INCLUDE_DEPENDS} + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle ) # Export hardware plugins 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 index 2144799..65206ab 100644 --- 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 @@ -19,43 +19,45 @@ #include #include #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/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -namespace laser_ros2_control -{ +#include "umdloop_can_library/SocketCanBus.hpp" +#include "umdloop_can_library/CanFrame.hpp" -// GPIO utility functions (Linux sysfs) -namespace gpio_utils +namespace laser_ros2_control { - 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 + * @brief Hardware interface for CAN-controlled spectrometry laser via ros2_control + * + * This interface controls a spectrometry laser through CAN bus. * - * This interface controls a spectrometry laser through GPIO pins. + * CAN Protocol (ID: 0x130): + * - ON: DATA[0] = 0x60 + * - OFF: DATA[0] = 0x80 + * - Read Temperature: DATA[0] = 0x85 + * - Read Wavelength: DATA[0] = 0x90 * * 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) + * - temperature: Laser temperature (if available) + * - is_connected: Is CAN connected (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) + * - can_interface: CAN interface name (default: "can0") + * - can_id: CAN ID for laser commands (default: 0x130) */ class LaserHardwareInterface : public hardware_interface::SystemInterface { @@ -93,26 +95,33 @@ class LaserHardwareInterface : public hardware_interface::SystemInterface const rclcpp::Duration & period) override; private: + // CAN message handler + void onCanMessage(const CANLib::CanFrame& frame); + // Configuration parameters - int gpio_pin_; - double min_power_; - double max_power_; + std::string can_interface_; + uint32_t can_id_; + + // CAN bus + CANLib::SocketCanBus canBus_; + CANLib::CanFrame can_tx_frame_; + bool can_connected_; // 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 + double temperature_; // Laser temperature + double is_connected_; // CAN connected 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_; + // CAN command bytes + static constexpr uint8_t CMD_LASER_ON = 0x60; + static constexpr uint8_t CMD_LASER_OFF = 0x80; + static constexpr uint8_t CMD_READ_TEMP = 0x85; + static constexpr uint8_t CMD_READ_WAVELENGTH = 0x90; }; } // 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 index c0488a2..6e16a20 100644 --- a/src/hardware_interfaces/laser_ros2_control/laser_hardware_interface.xml +++ b/src/hardware_interfaces/laser_ros2_control/laser_hardware_interface.xml @@ -4,7 +4,7 @@ base_class_type="hardware_interface::SystemInterface"> Hardware interface for spectrometry laser control via ros2_control. - Supports on/off control and power level via GPIO. + CAN-based on/off control with temperature readback. diff --git a/src/hardware_interfaces/laser_ros2_control/package.xml b/src/hardware_interfaces/laser_ros2_control/package.xml index c3f5f95..a72e298 100644 --- a/src/hardware_interfaces/laser_ros2_control/package.xml +++ b/src/hardware_interfaces/laser_ros2_control/package.xml @@ -3,7 +3,7 @@ laser_ros2_control 0.1.0 - Laser hardware interface for ros2_control - GPIO-based spectrometry laser control + Laser hardware interface for ros2_control - CAN-based spectrometry laser control Dwarakesh Baraneetharan Apache-2.0 @@ -15,6 +15,10 @@ rclcpp rclcpp_lifecycle + + umdloop_can_library + umdloop_can_library + ament_lint_auto ament_lint_common 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 index 15376f3..e392b0d 100644 --- a/src/hardware_interfaces/laser_ros2_control/src/laser_hardware_interface.cpp +++ b/src/hardware_interfaces/laser_ros2_control/src/laser_hardware_interface.cpp @@ -15,78 +15,12 @@ #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) { @@ -98,40 +32,31 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_init( // Initialize state variables laser_state_ = 0.0; // OFF - power_level_ = 0.0; - is_ready_ = 0.0; + temperature_ = 0.0; + is_connected_ = 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")); + if (info_.hardware_parameters.count("can_interface")) { + can_interface_ = info_.hardware_parameters.at("can_interface"); } else { - min_power_ = 0.0; + can_interface_ = "can0"; } - - if (info_.hardware_parameters.count("max_power")) { - max_power_ = std::stod(info_.hardware_parameters.at("max_power")); + + if (info_.hardware_parameters.count("can_id")) { + can_id_ = static_cast(std::stoi(info_.hardware_parameters.at("can_id"))); } else { - max_power_ = 1.0; + can_id_ = 0x130; // Default laser CAN ID } - gpio_fd_ = -1; - hw_connected_ = false; + can_connected_ = false; RCLCPP_INFO( rclcpp::get_logger("LaserHardwareInterface"), - "Initialized laser on GPIO pin %d", gpio_pin_); + "Initialized laser on CAN interface %s with ID 0x%X", + can_interface_.c_str(), can_id_); return hardware_interface::CallbackReturn::SUCCESS; } @@ -143,44 +68,64 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_configure( 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( + // Open CAN bus + if (!canBus_.open(can_interface_, + std::bind(&LaserHardwareInterface::onCanMessage, this, std::placeholders::_1))) + { + RCLCPP_WARN( rclcpp::get_logger("LaserHardwareInterface"), - "Failed to setup GPIO output on pin %d", gpio_pin_); - return hardware_interface::CallbackReturn::ERROR; + "Failed to open CAN interface %s - running in SIMULATION mode", + can_interface_.c_str()); + can_connected_ = false; + } else { + can_connected_ = true; + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Successfully opened CAN interface %s", can_interface_.c_str()); } - hw_connected_ = true; - is_ready_ = 1.0; + is_connected_ = can_connected_ ? 1.0 : 0.0; RCLCPP_INFO( rclcpp::get_logger("LaserHardwareInterface"), - "Successfully configured laser hardware"); + "Laser hardware configured (%s)", can_connected_ ? "CAN MODE" : "SIMULATION"); return hardware_interface::CallbackReturn::SUCCESS; } +void LaserHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) +{ + // Handle incoming CAN messages (e.g., temperature readings) + if (frame.id == can_id_) { + // Parse response based on command byte + if (frame.dlc > 0 && frame.data[0] == CMD_READ_TEMP) { + // Temperature response - parse if available + if (frame.dlc >= 2) { + temperature_ = static_cast(frame.data[1]); + } + } + } +} + std::vector LaserHardwareInterface::export_state_interfaces() { std::vector state_interfaces; - // Use the joint name from URDF - const std::string& name = info_.joints[0].name; + // Use the gpio name from URDF + const std::string& name = info_.gpios[0].name; // Laser state (ON/OFF) state_interfaces.emplace_back( hardware_interface::StateInterface(name, "laser_state", &laser_state_)); - // Power level + // Temperature state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "power_level", &power_level_)); + hardware_interface::StateInterface(name, "temperature", &temperature_)); - // Hardware ready status + // Connection status state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "is_ready", &is_ready_)); + hardware_interface::StateInterface(name, "is_connected", &is_connected_)); RCLCPP_INFO( rclcpp::get_logger("LaserHardwareInterface"), @@ -194,17 +139,13 @@ LaserHardwareInterface::export_command_interfaces() { std::vector command_interfaces; - // Use the joint name from URDF - const std::string& name = info_.joints[0].name; + // Use the gpio name from URDF + const std::string& name = info_.gpios[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()); @@ -230,16 +171,20 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_deactivate( "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; + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_id_; + can_tx_frame_.dlc = 1; + can_tx_frame_.data[0] = CMD_LASER_OFF; + canBus_.send(can_tx_frame_); RCLCPP_INFO( rclcpp::get_logger("LaserHardwareInterface"), "Laser turned OFF (safety)"); } + laser_state_ = 0.0; + return hardware_interface::CallbackReturn::SUCCESS; } @@ -251,18 +196,18 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_cleanup( "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; + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_id_; + can_tx_frame_.dlc = 1; + can_tx_frame_.data[0] = CMD_LASER_OFF; + canBus_.send(can_tx_frame_); + + canBus_.close(); } - hw_connected_ = false; - is_ready_ = 0.0; + can_connected_ = false; + is_connected_ = 0.0; RCLCPP_INFO( rclcpp::get_logger("LaserHardwareInterface"), @@ -278,7 +223,6 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_shutdown( rclcpp::get_logger("LaserHardwareInterface"), "Shutting down laser hardware..."); - // Delegate to cleanup to release resources return on_cleanup(previous_state); } @@ -286,7 +230,8 @@ 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 + // CAN messages are handled asynchronously via callback + // State is updated in onCanMessage() return hardware_interface::return_type::OK; } @@ -294,35 +239,25 @@ 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"); + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_id_; + can_tx_frame_.dlc = 1; + can_tx_frame_.data[0] = commanded_on ? CMD_LASER_ON : CMD_LASER_OFF; + canBus_.send(can_tx_frame_); } - } - // 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; + laser_state_ = commanded_on ? 1.0 : 0.0; - RCLCPP_DEBUG( + RCLCPP_INFO( rclcpp::get_logger("LaserHardwareInterface"), - "Power level set to %.2f", power_level_); + "Laser turned %s%s", commanded_on ? "ON" : "OFF", + can_connected_ ? "" : " (simulated)"); } return hardware_interface::return_type::OK; @@ -335,4 +270,3 @@ hardware_interface::return_type LaserHardwareInterface::write( 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 index 0e3b4e4..01af46c 100644 --- a/src/hardware_interfaces/led_ros2_control/CMakeLists.txt +++ b/src/hardware_interfaces/led_ros2_control/CMakeLists.txt @@ -14,6 +14,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS # find dependencies find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(ament_cmake_auto REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) 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 index 7919ed5..046e01f 100644 --- 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 @@ -25,6 +25,7 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" namespace led_ros2_control 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 index 7413fe6..2b2c0b4 100644 --- a/src/hardware_interfaces/led_ros2_control/src/led_hardware_interface.cpp +++ b/src/hardware_interfaces/led_ros2_control/src/led_hardware_interface.cpp @@ -139,21 +139,24 @@ hardware_interface::CallbackReturn LEDHardwareInterface::on_configure( // Setup GPIO output gpio_fd_ = gpio_utils::setup_gpio_output(gpio_pin_); if (gpio_fd_ < 0) { - RCLCPP_ERROR( + RCLCPP_WARN( rclcpp::get_logger("LEDHardwareInterface"), - "Failed to setup GPIO output on pin %d", gpio_pin_); - return hardware_interface::CallbackReturn::ERROR; + "Failed to setup GPIO output on pin %d - running in SIMULATION mode", gpio_pin_); + hw_connected_ = false; // Mark as simulation mode + } else { + hw_connected_ = true; + // Set initial state on real hardware + gpio_utils::write_gpio(gpio_fd_, default_state_); + RCLCPP_INFO( + rclcpp::get_logger("LEDHardwareInterface"), + "Successfully configured LED hardware (GPIO mode)"); } - hw_connected_ = true; - is_connected_ = 1.0; - - // Set initial state - gpio_utils::write_gpio(gpio_fd_, default_state_); + is_connected_ = hw_connected_ ? 1.0 : 0.0; RCLCPP_INFO( rclcpp::get_logger("LEDHardwareInterface"), - "Successfully configured LED hardware"); + "LED hardware configured (%s)", hw_connected_ ? "REAL GPIO" : "SIMULATION"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -163,8 +166,8 @@ LEDHardwareInterface::export_state_interfaces() { std::vector state_interfaces; - // Use the joint name from URDF - const std::string& name = info_.joints[0].name; + // Use the gpio name from URDF + const std::string& name = info_.gpios[0].name; // LED state state_interfaces.emplace_back( @@ -186,8 +189,8 @@ LEDHardwareInterface::export_command_interfaces() { std::vector command_interfaces; - // Use the joint name from URDF - const std::string& name = info_.joints[0].name; + // Use the gpio name from URDF + const std::string& name = info_.gpios[0].name; // LED command command_interfaces.emplace_back( @@ -281,22 +284,22 @@ 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"); + // Write to real GPIO if available, otherwise simulate + if (hw_connected_ && gpio_fd_ >= 0) { + 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%s", commanded_on ? "ON" : "OFF", + hw_connected_ ? "" : " (simulated)"); } return hardware_interface::return_type::OK; diff --git a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml index 93a84ff..4333282 100644 --- a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml +++ b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml @@ -40,6 +40,14 @@ controller_manager: # swerve_drive_controller: # type: drive_controllers/SwerveDriveController + + # LED GPIO Controller + led_gpio_controller: + type: gpio_controllers/GpioCommandController + + # Killswitch GPIO Controller + killswitch_gpio_controller: + type: gpio_controllers/GpioCommandController ackermann_steering_controller: @@ -91,3 +99,37 @@ drive_position_controller: - steer_fl_joint - steer_fr_joint interface_name: position + +# LED GPIO Controller Configuration +led_gpio_controller: + ros__parameters: + gpios: + - status_led + command_interfaces: + status_led: + interfaces: + - led_command + state_interfaces: + status_led: + interfaces: + - led_state + - is_connected + +# Killswitch GPIO Controller Configuration +killswitch_gpio_controller: + ros__parameters: + gpios: + - drive_killswitch + command_interfaces: + drive_killswitch: + interfaces: + - kill_all + - kill_main + - kill_jetson + state_interfaces: + drive_killswitch: + interfaces: + - kill_all_sent + - kill_main_sent + - kill_jetson_sent + - is_connected 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 15e4412..0aa5bae 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -36,26 +36,9 @@ 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 + # Laser GPIO Controller (CAN-based) + laser_gpio_controller: + type: gpio_controllers/GpioCommandController joint_group_position_controller: ros__parameters: @@ -115,41 +98,18 @@ science_controller: command_interfaces: [position, velocity] state_interfaces: [position, velocity] -# Laser Controller Configurations -laser_onoff_controller: +# Laser GPIO Controller Configuration +laser_gpio_controller: ros__parameters: - joints: + gpios: - 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 + command_interfaces: + spectrometry_laser: + interfaces: + - laser_command + state_interfaces: + spectrometry_laser: + interfaces: + - laser_state + - temperature + - is_connected \ No newline at end of file From 8a08e5d32b3648898c68f4a82524d5041fb20c80 Mon Sep 17 00:00:00 2001 From: Dwarakesh Baraneetharan Date: Sun, 1 Feb 2026 20:50:21 -0500 Subject: [PATCH 3/3] fix: Use hex CAN IDs and add GPIO controller spawners --- .../drive/drive.killswitch.ros2_control.xacro | 5 ++-- .../science/science.laser.ros2_control.xacro | 5 ++-- .../src/killswitch_hardware_interface.cpp | 3 +- .../src/laser_hardware_interface.cpp | 3 +- .../launch/athena_drive.launch.py | 29 +++++++++++++++++++ .../launch/athena_science.launch.py | 29 +++++++++++++++++++ 6 files changed, 66 insertions(+), 8 deletions(-) diff --git a/src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro b/src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro index 334b857..2ee5259 100644 --- a/src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro @@ -1,14 +1,13 @@ - - + killswitch_ros2_control/KillswitchHardwareInterface ${can_interface} - ${can_id} + 0x100 diff --git a/src/description/ros2_control/science/science.laser.ros2_control.xacro b/src/description/ros2_control/science/science.laser.ros2_control.xacro index 50719bb..ee7f97c 100644 --- a/src/description/ros2_control/science/science.laser.ros2_control.xacro +++ b/src/description/ros2_control/science/science.laser.ros2_control.xacro @@ -1,15 +1,14 @@ - - + laser_ros2_control/LaserHardwareInterface ${can_interface} - ${can_id} + 0x130 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 index 972889c..3d335e8 100644 --- a/src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp +++ b/src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp @@ -49,7 +49,8 @@ hardware_interface::CallbackReturn KillswitchHardwareInterface::on_init( } if (info_.hardware_parameters.count("can_id")) { - can_id_ = static_cast(std::stoi(info_.hardware_parameters.at("can_id"))); + // Parse hex string (e.g., "0x100") - base 0 auto-detects hex/decimal + can_id_ = static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); } else { can_id_ = 0x100; // Default killswitch CAN ID } 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 index e392b0d..1e74bde 100644 --- a/src/hardware_interfaces/laser_ros2_control/src/laser_hardware_interface.cpp +++ b/src/hardware_interfaces/laser_ros2_control/src/laser_hardware_interface.cpp @@ -46,7 +46,8 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_init( } if (info_.hardware_parameters.count("can_id")) { - can_id_ = static_cast(std::stoi(info_.hardware_parameters.at("can_id"))); + // Parse hex string (e.g., "0x130") - base 0 auto-detects hex/decimal + can_id_ = static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); } else { can_id_ = 0x130; // Default laser CAN ID } diff --git a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py index 2a52b8f..4594ea4 100644 --- a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py +++ b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py @@ -233,6 +233,18 @@ def generate_launch_description(): ) ] + # GPIO controller spawners for LED and Killswitch + gpio_controller_names = ["led_gpio_controller", "killswitch_gpio_controller"] + gpio_controller_spawners = [] + for controller in gpio_controller_names: + gpio_controller_spawners += [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "-c", "/controller_manager"], + ) + ] + inactive_robot_controller_names = ["ackermann_steering_controller", "drive_velocity_controller", "drive_position_controller"] inactive_robot_controller_spawners = [] for controller in inactive_robot_controller_names: @@ -244,6 +256,22 @@ def generate_launch_description(): ) ] + # Delay GPIO controller spawners after joint_state_broadcaster + delay_gpio_controller_spawners_after_joint_state_broadcaster_spawner = [] + for i, controller in enumerate(gpio_controller_spawners): + delay_gpio_controller_spawners_after_joint_state_broadcaster_spawner += [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=( + gpio_controller_spawners[i - 1] + if i > 0 + else joint_state_broadcaster_spawner + ), + on_exit=[controller], + ) + ) + ] + controller_switcher_node = RegisterEventHandler( event_handler=OnProcessExit( target_action=inactive_robot_controller_spawners[-1], @@ -364,4 +392,5 @@ def generate_launch_description(): ] + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner + + delay_gpio_controller_spawners_after_joint_state_broadcaster_spawner ) \ No newline at end of file diff --git a/src/subsystems/science/science_bringup/launch/athena_science.launch.py b/src/subsystems/science/science_bringup/launch/athena_science.launch.py index ff479e0..8778536 100644 --- a/src/subsystems/science/science_bringup/launch/athena_science.launch.py +++ b/src/subsystems/science/science_bringup/launch/athena_science.launch.py @@ -187,6 +187,18 @@ def generate_launch_description(): ) ] + # GPIO controller spawner for Laser + gpio_controller_names = ["laser_gpio_controller"] + gpio_controller_spawners = [] + for controller in gpio_controller_names: + gpio_controller_spawners += [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "-c", "/controller_manager"], + ) + ] + inactive_robot_controller_names = ["joint_group_velocity_controller", "joint_group_position_controller"] inactive_robot_controller_spawners = [] # Set the ones you want inactive in the beginning (e.g., velocity controller, etc.) for controller in inactive_robot_controller_names: @@ -198,6 +210,22 @@ def generate_launch_description(): ) ] + # Delay GPIO controller spawners after joint_state_broadcaster + delay_gpio_controller_spawners_after_joint_state_broadcaster_spawner = [] + for i, controller in enumerate(gpio_controller_spawners): + delay_gpio_controller_spawners_after_joint_state_broadcaster_spawner += [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=( + gpio_controller_spawners[i - 1] + if i > 0 + else joint_state_broadcaster_spawner + ), + on_exit=[controller], + ) + ) + ] + # Handle switching between controllers controller_switcher_node = RegisterEventHandler( event_handler=OnProcessExit( @@ -279,4 +307,5 @@ def generate_launch_description(): ] + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner + + delay_gpio_controller_spawners_after_joint_state_broadcaster_spawner )