umdloop · dwarakeshbaraneetharan · Jan 6, 2026
diff --git a/src/description/ros2_control/science/science.killswitch.ros2_control.xacro b/src/description/ros2_control/science/science.killswitch.ros2_control.xacro
@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="killswitch_ros2_control" params="name button_gpio_pin main_power_gpio_pin jetson_power_gpio_pin all_power_gpio_pin active_high:=true">
+
+        <ros2_control name="${name}" type="system">
+
+            <hardware>
+                <plugin>killswitch_ros2_control/KillswitchHardwareInterface</plugin>
+                <param name="button_gpio_pin">${button_gpio_pin}</param>
+                <param name="main_power_gpio_pin">${main_power_gpio_pin}</param>
+                <param name="jetson_power_gpio_pin">${jetson_power_gpio_pin}</param>
+                <param name="all_power_gpio_pin">${all_power_gpio_pin}</param>
+                <param name="active_high">${active_high}</param>
+            </hardware>
+
+            <joint name="${name}">
+                <state_interface name="button_state"/>
+                <state_interface name="main_power_killed"/>
+                <state_interface name="jetson_power_killed"/>
+                <state_interface name="all_power_killed"/>
+                <state_interface name="is_connected"/>
+                <command_interface name="kill_main_power"/>
+                <command_interface name="kill_jetson_power"/>
+                <command_interface name="kill_all_power"/>
+            </joint>
+
+        </ros2_control>
+
+    </xacro:macro>
+
+</robot>
+
diff --git a/src/description/ros2_control/science/science.laser.ros2_control.xacro b/src/description/ros2_control/science/science.laser.ros2_control.xacro
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="laser_ros2_control" params="name gpio_pin min_power:=0.0 max_power:=1.0">
+
+        <ros2_control name="${name}" type="system">
+
+            <hardware>
+                <plugin>laser_ros2_control/LaserHardwareInterface</plugin>
+                <param name="gpio_pin">${gpio_pin}</param>
+                <param name="min_power">${min_power}</param>
+                <param name="max_power">${max_power}</param>
+            </hardware>
+
+            <joint name="${name}">
+                <state_interface name="laser_state"/>
+                <state_interface name="power_level"/>
+                <state_interface name="is_ready"/>
+                <command_interface name="laser_command"/>
+                <command_interface name="power_command"/>
+            </joint>
+
+        </ros2_control>
+
+    </xacro:macro>
+
+</robot>
+
diff --git a/src/description/ros2_control/science/science.led.ros2_control.xacro b/src/description/ros2_control/science/science.led.ros2_control.xacro
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="led_ros2_control" params="name gpio_pin default_state:=off">
+
+        <ros2_control name="${name}" type="system">
+
+            <hardware>
+                <plugin>led_ros2_control/LEDHardwareInterface</plugin>
+                <param name="gpio_pin">${gpio_pin}</param>
+                <param name="default_state">${default_state}</param>
+            </hardware>
+
+            <joint name="${name}">
+                <state_interface name="led_state"/>
+                <state_interface name="is_connected"/>
+                <command_interface name="led_command"/>
+            </joint>
+
+        </ros2_control>
+
+    </xacro:macro>
+
+</robot>
+
diff --git a/src/description/urdf/athena_science.urdf.xacro b/src/description/urdf/athena_science.urdf.xacro
@@ -9,6 +9,9 @@
   <xacro:include filename="$(find description)/ros2_control/science/science.stepper.ros2_control.xacro" />
   <xacro:include filename="$(find description)/ros2_control/science/science.servo.ros2_control.xacro" />
   <xacro:include filename="$(find description)/ros2_control/science/science.talon.ros2_control.xacro"/>
+  <xacro:include filename="$(find description)/ros2_control/science/science.laser.ros2_control.xacro"/>
+  <xacro:include filename="$(find description)/ros2_control/science/science.killswitch.ros2_control.xacro"/>
+  <xacro:include filename="$(find description)/ros2_control/science/science.led.ros2_control.xacro"/>
 
   <!-- create link fixed to the "world" -->
   <link name="world"/>
@@ -30,7 +33,21 @@
     <xacro:servo_ros2_control name="servo"/>
 
     <!-- Import talon ros2_control description -->
-    <xacro:talon_ros2_control name="talon"/> 
+    <xacro:talon_ros2_control name="talon"/>
+
+    <!-- Import laser ros2_control description -->
+    <xacro:laser_ros2_control name="spectrometry_laser" gpio_pin="17"/>
+
+    <!-- Import killswitch ros2_control description -->
+    <xacro:killswitch_ros2_control 
+      name="drive_killswitch" 
+      button_gpio_pin="27" 
+      main_power_gpio_pin="22" 
+      jetson_power_gpio_pin="23" 
+      all_power_gpio_pin="24"/>
+
+    <!-- Import LED ros2_control description -->
+    <xacro:led_ros2_control name="status_led" gpio_pin="25"/>
   </xacro:unless>
 
 </robot>
diff --git 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
@@ -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
+$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+$<INSTALL_INTERFACE:include/killswitch_ros2_control>
+)
+
+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/...killswitch_ros2_control/include/killswitch_ros2_control/killswitch_hardware_interface.hpp b/...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 <memory>
+#include <string>
+#include <vector>
+
+#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<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  std::vector<hardware_interface::CommandInterface> 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
@@ -0,0 +1,10 @@
+<library path="killswitch_ros2_control">
+  <class name="killswitch_ros2_control/KillswitchHardwareInterface"
+         type="killswitch_ros2_control::KillswitchHardwareInterface"
+         base_class_type="hardware_interface::SystemInterface">
+    <description>
+      Drive killswitch hardware interface via ros2_control.
+      Monitors physical killswitch button and controls power kill relays.
+    </description>
+  </class>
+</library>
diff --git a/src/hardware_interfaces/killswitch_ros2_control/package.xml b/src/hardware_interfaces/killswitch_ros2_control/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>killswitch_ros2_control</name>
+  <version>0.1.0</version>
+  <description>Killswitch hardware interface for ros2_control - GPIO-based drive killswitch system</description>
+  <maintainer email="dwarakeshbaraneetharan@gmail.com">Dwarakesh Baraneetharan</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>ament_cmake</build_depend>
+
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <exec_depend>ament_cmake</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>