Add laser, killswitch, and LED hardware interfaces

src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="killswitch_ros2_control" params="name can_interface:=can0">
+
+    <ros2_control name="${name}" type="system">
+      <hardware>
+        <plugin>killswitch_ros2_control/KillswitchHardwareInterface</plugin>
+        <param name="can_interface">${can_interface}</param>
+        <param name="can_id">0x100</param>
+      </hardware>
+
+      <gpio name="${name}">
+        <state_interface name="kill_all_sent"/>
+        <state_interface name="kill_main_sent"/>
+        <state_interface name="kill_jetson_sent"/>
+        <state_interface name="is_connected"/>
+        <command_interface name="kill_all"/>
+        <command_interface name="kill_main"/>
+        <command_interface name="kill_jetson"/>
+      </gpio>
+
+    </ros2_control>
+
+  </xacro:macro>
+
+</robot>

src/description/ros2_control/drive/drive.led.ros2_control.xacro

@@ -0,0 +1,23 @@
+<?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>
+
+      <gpio name="${name}">
+        <state_interface name="led_state"/>
+        <state_interface name="is_connected"/>
+        <command_interface name="led_command"/>
+      </gpio>
+
+    </ros2_control>
+
+  </xacro:macro>
+
+</robot>

src/description/ros2_control/science/science.laser.ros2_control.xacro

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="laser_ros2_control" params="name can_interface:=can0">
+
+        <ros2_control name="${name}" type="system">
+
+            <hardware>
+                <plugin>laser_ros2_control/LaserHardwareInterface</plugin>
+                <param name="can_interface">${can_interface}</param>
+                <param name="can_id">0x130</param>
+            </hardware>
+
+            <gpio name="${name}">
+                <state_interface name="laser_state"/>
+                <state_interface name="temperature"/>
+                <state_interface name="is_connected"/>
+                <command_interface name="laser_command"/>
+            </gpio>
+
+        </ros2_control>
+
+    </xacro:macro>
+
+</robot>
+

src/description/urdf/athena_drive.urdf.xacro

@@ -36,6 +36,12 @@
   <!-- Import odrive ros2_control description -->
   <xacro:include filename="$(find description)/ros2_control/drive/drive.odrive.ros2_control.xacro" />
 
+  <!-- Import LED ros2_control description -->
+  <xacro:include filename="$(find description)/ros2_control/drive/drive.led.ros2_control.xacro" />
+
+  <!-- Import killswitch ros2_control description -->
+  <xacro:include filename="$(find description)/ros2_control/drive/drive.killswitch.ros2_control.xacro" />
+
   <!-- Contains description of drive -->
   <xacro:description/>
 
@@ -66,6 +72,12 @@
   <xacro:unless value="$(arg use_mock_hardware)">
     <xacro:odrive_ros2_control name="odrive"/>
     <xacro:rmd_ros2_control name="rmd"/>
+
+    <!-- LED status indicator (GPIO-based) -->
+    <xacro:led_ros2_control name="status_led" gpio_pin="25"/>
+
+    <!-- Killswitch (CAN-based) -->
+    <xacro:killswitch_ros2_control name="drive_killswitch"/>
   </xacro:unless>
 
 

src/description/urdf/athena_science.urdf.xacro

@@ -9,6 +9,7 @@
   <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"/>
 
   <!-- create link fixed to the "world" -->
   <link name="world"/>
@@ -30,7 +31,10 @@
     <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 (CAN-based) -->
+    <xacro:laser_ros2_control name="spectrometry_laser"/>
   </xacro:unless>
 
 </robot>

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

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
+$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+$<INSTALL_INTERFACE:include/killswitch_ros2_control>
+)
+
+# 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()

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 <memory>
+#include <string>
+#include <vector>
+#include <cstdint>
+#include <array>
+
+#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<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:
+  // 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_

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>
+      Killswitch hardware interface via ros2_control.
+      CAN-based power kill commands for main, jetson, and all systems.
+    </description>
+  </class>
+</library>

src/hardware_interfaces/killswitch_ros2_control/package.xml

@@ -0,0 +1,30 @@
+<?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 - CAN-based power 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>
+
+  <!-- Real-time CAN communication library -->
+  <build_depend>umdloop_can_library</build_depend>
+  <exec_depend>umdloop_can_library</exec_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>