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..2ee5259 --- /dev/null +++ b/src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro @@ -0,0 +1,27 @@ + + + + + + + + killswitch_ros2_control/KillswitchHardwareInterface + ${can_interface} + 0x100 + + + + + + + + + + + + + + + + + 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.laser.ros2_control.xacro b/src/description/ros2_control/science/science.laser.ros2_control.xacro new file mode 100644 index 0000000..ee7f97c --- /dev/null +++ b/src/description/ros2_control/science/science.laser.ros2_control.xacro @@ -0,0 +1,26 @@ + + + + + + + + + laser_ros2_control/LaserHardwareInterface + ${can_interface} + 0x130 + + + + + + + + + + + + + + + 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 ca032f7..80fc531 100644 --- a/src/description/urdf/athena_science.urdf.xacro +++ b/src/description/urdf/athena_science.urdf.xacro @@ -9,6 +9,7 @@ + @@ -30,7 +31,10 @@ - + + + + \ 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..4a42db5 --- /dev/null +++ b/src/hardware_interfaces/killswitch_ros2_control/CMakeLists.txt @@ -0,0 +1,77 @@ +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 + 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) +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 +$ +$ +) + +# Link CAN library +target_link_libraries(killswitch_ros2_control PUBLIC umdloop_can_library::umdloop_can_library) + +ament_target_dependencies( + killswitch_ros2_control PUBLIC + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +# 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..4636862 --- /dev/null +++ b/src/hardware_interfaces/killswitch_ros2_control/include/killswitch_ros2_control/killswitch_hardware_interface.hpp @@ -0,0 +1,131 @@ +// 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 +#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" + +#include "umdloop_can_library/SocketCanBus.hpp" +#include "umdloop_can_library/CanFrame.hpp" + +namespace killswitch_ros2_control +{ + +/** + * @brief Hardware interface for CAN-controlled killswitch via ros2_control + * + * 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): + * - 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_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): + * - can_interface: CAN interface name (default: "can0") + * - can_id: CAN ID for killswitch commands (default: 0x100) + */ +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: + // 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 new file mode 100644 index 0000000..9162004 --- /dev/null +++ b/src/hardware_interfaces/killswitch_ros2_control/killswitch_hardware_interface.xml @@ -0,0 +1,10 @@ + + + + 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 new file mode 100644 index 0000000..afa1bc2 --- /dev/null +++ b/src/hardware_interfaces/killswitch_ros2_control/package.xml @@ -0,0 +1,30 @@ + + + + killswitch_ros2_control + 0.1.0 + Killswitch hardware interface for ros2_control - CAN-based power killswitch system + Dwarakesh Baraneetharan + Apache-2.0 + + ament_cmake + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + + umdloop_can_library + umdloop_can_library + + 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..3d335e8 --- /dev/null +++ b/src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp @@ -0,0 +1,286 @@ +// 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 "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace killswitch_ros2_control +{ + +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 + kill_all_sent_ = 0.0; + kill_main_sent_ = 0.0; + kill_jetson_sent_ = 0.0; + is_connected_ = 0.0; + + // Initialize command variables + cmd_kill_all_ = 0.0; + cmd_kill_main_ = 0.0; + cmd_kill_jetson_ = 0.0; + + // Parse hardware parameters + if (info_.hardware_parameters.count("can_interface")) { + can_interface_ = info_.hardware_parameters.at("can_interface"); + } else { + can_interface_ = "can0"; + } + + if (info_.hardware_parameters.count("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 + } + + can_connected_ = false; + + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "Initialized killswitch on CAN interface %s with ID 0x%X", + can_interface_.c_str(), can_id_); + + 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..."); + + // Open CAN bus + if (!canBus_.open(can_interface_, + std::bind(&KillswitchHardwareInterface::onCanMessage, this, std::placeholders::_1))) + { + RCLCPP_WARN( + rclcpp::get_logger("KillswitchHardwareInterface"), + "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"), + "Successfully opened CAN interface %s", can_interface_.c_str()); + } + + is_connected_ = can_connected_ ? 1.0 : 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("KillswitchHardwareInterface"), + "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 gpio name from URDF + const std::string& name = info_.gpios[0].name; + + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "kill_all_sent", &kill_all_sent_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "kill_main_sent", &kill_main_sent_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "kill_jetson_sent", &kill_jetson_sent_)); + 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 gpio name from URDF + const std::string& name = info_.gpios[0].name; + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(name, "kill_all", &cmd_kill_all_)); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(name, "kill_main", &cmd_kill_main_)); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(name, "kill_jetson", &cmd_kill_jetson_)); + + 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 + cmd_kill_all_ = 0.0; + cmd_kill_main_ = 0.0; + cmd_kill_jetson_ = 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 send kill commands on deactivate + // Killswitch should only respond to explicit commands + + 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..."); + + if (can_connected_) { + canBus_.close(); + } + + can_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..."); + + return on_cleanup(previous_state); +} + +hardware_interface::return_type KillswitchHardwareInterface::read( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +{ + // CAN messages are handled asynchronously via callback + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type KillswitchHardwareInterface::write( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +{ + // 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 + } + + // 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 + } + + // 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; +} + +} // 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..3c301d5 --- /dev/null +++ b/src/hardware_interfaces/laser_ros2_control/CMakeLists.txt @@ -0,0 +1,77 @@ +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 + 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) +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 +$ +$ +) + +# Link CAN library +target_link_libraries(laser_ros2_control PUBLIC umdloop_can_library::umdloop_can_library) + +ament_target_dependencies( + laser_ros2_control PUBLIC + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +# 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..65206ab --- /dev/null +++ b/src/hardware_interfaces/laser_ros2_control/include/laser_ros2_control/laser_hardware_interface.hpp @@ -0,0 +1,127 @@ +// 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 +#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" + +#include "umdloop_can_library/SocketCanBus.hpp" +#include "umdloop_can_library/CanFrame.hpp" + +namespace laser_ros2_control +{ + +/** + * @brief Hardware interface for CAN-controlled spectrometry laser via ros2_control + * + * This interface controls a spectrometry laser through CAN bus. + * + * 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 + * - 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 + * + * Hardware Parameters (from URDF): + * - can_interface: CAN interface name (default: "can0") + * - can_id: CAN ID for laser commands (default: 0x130) + */ +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: + // 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 (hardware → ros2_control) + double laser_state_; // Current state: 0.0 = OFF, 1.0 = ON + double temperature_; // Laser temperature + double is_connected_; // CAN connected status + + // Command variables (ros2_control → hardware) + double laser_command_; // Commanded state + + // 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 new file mode 100644 index 0000000..6e16a20 --- /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. + 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 new file mode 100644 index 0000000..a72e298 --- /dev/null +++ b/src/hardware_interfaces/laser_ros2_control/package.xml @@ -0,0 +1,30 @@ + + + + laser_ros2_control + 0.1.0 + Laser hardware interface for ros2_control - CAN-based spectrometry laser control + Dwarakesh Baraneetharan + Apache-2.0 + + ament_cmake + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + + umdloop_can_library + umdloop_can_library + + 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..1e74bde --- /dev/null +++ b/src/hardware_interfaces/laser_ros2_control/src/laser_hardware_interface.cpp @@ -0,0 +1,273 @@ +// 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 "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace laser_ros2_control +{ + +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 + temperature_ = 0.0; + is_connected_ = 0.0; + + // Initialize command variables + laser_command_ = 0.0; // OFF + + // Parse hardware parameters + if (info_.hardware_parameters.count("can_interface")) { + can_interface_ = info_.hardware_parameters.at("can_interface"); + } else { + can_interface_ = "can0"; + } + + if (info_.hardware_parameters.count("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 + } + + can_connected_ = false; + + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Initialized laser on CAN interface %s with ID 0x%X", + can_interface_.c_str(), can_id_); + + 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..."); + + // Open CAN bus + if (!canBus_.open(can_interface_, + std::bind(&LaserHardwareInterface::onCanMessage, this, std::placeholders::_1))) + { + RCLCPP_WARN( + rclcpp::get_logger("LaserHardwareInterface"), + "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()); + } + + is_connected_ = can_connected_ ? 1.0 : 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "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 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_)); + + // Temperature + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "temperature", &temperature_)); + + // Connection status + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "is_connected", &is_connected_)); + + 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 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_)); + + 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 (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; +} + +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 (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(); + } + + can_connected_ = false; + is_connected_ = 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..."); + + return on_cleanup(previous_state); +} + +hardware_interface::return_type LaserHardwareInterface::read( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +{ + // CAN messages are handled asynchronously via callback + // State is updated in onCanMessage() + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type LaserHardwareInterface::write( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +{ + // 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 (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_); + } + + laser_state_ = commanded_on ? 1.0 : 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Laser turned %s%s", commanded_on ? "ON" : "OFF", + can_connected_ ? "" : " (simulated)"); + } + + 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..01af46c --- /dev/null +++ b/src/hardware_interfaces/led_ros2_control/CMakeLists.txt @@ -0,0 +1,70 @@ +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(backward_ros 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..046e01f --- /dev/null +++ b/src/hardware_interfaces/led_ros2_control/include/led_ros2_control/led_hardware_interface.hpp @@ -0,0 +1,113 @@ +// 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/node_interfaces/lifecycle_node_interface.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..2b2c0b4 --- /dev/null +++ b/src/hardware_interfaces/led_ros2_control/src/led_hardware_interface.cpp @@ -0,0 +1,315 @@ +// 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_WARN( + rclcpp::get_logger("LEDHardwareInterface"), + "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)"); + } + + is_connected_ = hw_connected_ ? 1.0 : 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("LEDHardwareInterface"), + "LED hardware configured (%s)", hw_connected_ ? "REAL GPIO" : "SIMULATION"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector +LEDHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + + // Use the gpio name from URDF + const std::string& name = info_.gpios[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 gpio name from URDF + const std::string& name = info_.gpios[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*/) +{ + // Check if LED command changed + bool commanded_on = (led_command_ > 0.5); + bool currently_on = (led_state_ > 0.5); + + if (commanded_on != currently_on) { + // 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; +} + +} // 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/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/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/config/athena_science_controllers.yaml b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml index 2965b2c..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,6 +36,10 @@ controller_manager: science_controller: type: science_controllers/ScienceManual + # Laser GPIO Controller (CAN-based) + laser_gpio_controller: + type: gpio_controllers/GpioCommandController + joint_group_position_controller: ros__parameters: joints: @@ -92,4 +96,20 @@ 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 GPIO Controller Configuration +laser_gpio_controller: + ros__parameters: + gpios: + - spectrometry_laser + command_interfaces: + spectrometry_laser: + interfaces: + - laser_command + state_interfaces: + spectrometry_laser: + interfaces: + - laser_state + - temperature + - is_connected \ 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 )