Spectro

.github/workflows/main.yml

@@ -37,5 +37,5 @@ jobs:
         run: |
           source /opt/ros/humble/setup.bash
           colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release \
-            --packages-skip zed_components zed_debug zed_ros2 zed_wrapper obstacle_detection athena_sensors nav_bringup
+            --packages-skip zed_components zed_debug zed_ros2 zed_wrapper obstacle_detection athena_sensors nav_bringup raman_msgs
         shell: bash

src/description/ros2_control/science/science.ccd.ros2_control.xacro

@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="ccd_ros2_control" params="name can_interface:=can0">
+
+        <ros2_control name="${name}" type="system">
+
+            <hardware>
+                <plugin>ccd_ros2_control/CCDHardwareInterface</plugin>
+                <param name="can_interface">${can_interface}</param>
+                <param name="can_id">0x100</param>
+                <param name="update_rate">10</param> <!-- Hz -->
+                <param name="logger_rate">5</param>  <!-- Hz -->
+                <param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
+            </hardware>
+
+            <gpio name="${name}">
+                <param name="node_id">0</param>
+                <command_interface name="capture_binary"/>
+                <command_interface name="capture_byte"/>
+                <command_interface name="status_request"/>
+                <command_interface name="maintenance_request"/>
+                <command_interface name="maintenance_frame_high"/>
+                <command_interface name="maintenance_frame_low"/>
+                <command_interface name="maintenance_data_count"/>
+
+
+                <state_interface name="is_connected"/>
+                <state_interface name="command_success"/>
+                <state_interface name="acquisition_in_progress"/>
+                <state_interface name="data_ready"/>
+                <state_interface name="frames_received"/>
+                <state_interface name="last_frame_id"/>
+                <state_interface name="status"/>
+
+            </gpio>
+
+        </ros2_control>
+
+    </xacro:macro>
+
+</robot>
+

src/description/ros2_control/science/science.dc.ros2_control.xacro

@@ -17,7 +17,6 @@
       <joint name="dc_auger">
         <param name="node_id">0</param>
         <param name="joint_type">revolute</param>
-
         <param name="gear_ratio">50.9</param>
         <param name="inverted">true</param>
         <command_interface name="position"/>

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

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-    <xacro:macro name="laser_ros2_control" params="name can_interface:=can0">
+    <xacro:macro name="laser_ros2_control" params="name can_interface:=can0 port_id:=0">
 
         <ros2_control name="${name}" type="system">
 

src/description/ros2_control/science/science.sensor_diode.ros2_control.xacro

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="sensor_diode_ros2_control" params="name can_interface:=can0">
+
+        <ros2_control name="${name}" type="system">
+
+            <hardware>
+                <plugin>sensor_diode_ros2_control/SensorDiodeHardwareInterface</plugin>
+                <param name="can_interface">${can_interface}</param>
+                <param name="can_id">0x110</param>
+                <param name="update_rate">10</param> <!-- Hz -->
+                <param name="logger_rate">5</param>  <!-- Hz -->
+                <param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
+            </hardware>
+
+            <gpio name="${name}">
+                <command_interface name="request_measurement"/>
+                <command_interface name="status_request"/>
+                <command_interface name="maintenance_request"/>
+                <command_interface name="maintenance_frame_high"/>
+                <command_interface name="maintenance_frame_low"/>
+                <command_interface name="maintenance_data_count"/>
+
+                <state_interface name="wavelength_intensity"/>
+                <state_interface name="command_success"/>
+                <state_interface name="is_connected"/>
+                <state_interface name="status"/>
+            </gpio>
+
+        </ros2_control>
+
+    </xacro:macro>
+
+</robot>
+

src/description/urdf/athena_science.urdf.xacro

@@ -13,6 +13,8 @@
   <xacro:include filename="$(find description)/ros2_control/science/science.dc.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.ccd.ros2_control.xacro"/>
+  <xacro:include filename="$(find description)/ros2_control/science/science.sensor_diode.ros2_control.xacro"/>
   <xacro:include filename="$(find description)/ros2_control/science/science.led.ros2_control.xacro"/>
   <xacro:include filename="$(find description)/ros2_control/science/science.limit_switch.ros2_control.xacro"/>
 
@@ -44,6 +46,12 @@
     <!-- Import laser ros2_control description (CAN-based) -->
     <xacro:laser_ros2_control name="spectrometry_laser" can_interface="$(arg can_interface)"/>
 
+    <!-- Import CCD ros2_control description (CAN-based) -->
+    <xacro:ccd_ros2_control name="spectrometry_ccd" can_interface="$(arg can_interface)"/>
+
+    <!-- Import sensor diode ros2_control description (CAN-based) -->
+    <xacro:sensor_diode_ros2_control name="sensor_diode" can_interface="$(arg can_interface)"/>
+
     <!-- Import LED and limit switch ros2_control descriptions -->
     <xacro:science_led_ros2_control name="fluoro_led_hwi" can_interface="$(arg can_interface)"/>
     <xacro:science_limit_switch_ros2_control name="science_limit_switch_hwi" can_interface="$(arg can_interface)"/>

src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt

@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 3.8)
+project(ccd_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
+  realtime_tools
+  msgs
+)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+find_package(msgs REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_auto_find_build_dependencies()
+
+include_directories(include)
+
+add_library(
+  ccd_ros2_control
+  SHARED
+  src/ccd_hardware_interface.cpp
+)
+
+target_compile_features(ccd_ros2_control PUBLIC cxx_std_20)
+target_include_directories(ccd_ros2_control PUBLIC
+$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+$<INSTALL_INTERFACE:include/ccd_ros2_control>
+)
+
+# Link CAN library
+target_link_libraries(ccd_ros2_control PUBLIC umdloop_can_library::umdloop_can_library)
+
+ament_target_dependencies(
+  ccd_ros2_control PUBLIC
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  msgs
+)
+
+# Export hardware plugins
+pluginlib_export_plugin_description_file(hardware_interface ccd_hardware_interface.xml)
+
+# INSTALL
+install(
+  DIRECTORY include/
+  DESTINATION include/ccd_ros2_control
+)
+
+install(TARGETS ccd_ros2_control
+  EXPORT export_ccd_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_ccd_ros2_control)
+ament_export_include_directories(include)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

src/hardware_interfaces/ccd_ros2_control/ccd_hardware_interface.xml

@@ -0,0 +1,9 @@
+<library path="ccd_ros2_control">
+  <class name="ccd_ros2_control/CCDHardwareInterface"
+         type="ccd_ros2_control::CCDHardwareInterface"
+         base_class_type="hardware_interface::SystemInterface">
+    <description>
+      Hardware interface for spectrometry ccd control via ros2_control.
+    </description>
+  </class>
+</library>

src/hardware_interfaces/ccd_ros2_control/include/ccd_ros2_control/ccd_hardware_interface.hpp

@@ -0,0 +1,169 @@
+#ifndef CCD_ROS2_CONTROL__CCD_HARDWARE_INTERFACE_HPP_
+#define CCD_ROS2_CONTROL__CCD_HARDWARE_INTERFACE_HPP_
+
+#include <limits>
+#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/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "umdloop_can_library/CanFrame.hpp"
+#include "umdloop_can_library/SocketCanBus.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "msgs/msg/raman_spectrum.hpp"
+
+namespace CANLib
+{
+struct CanFrame;
+}
+
+namespace ccd_ros2_control
+{
+
+class CCDHardwareInterface : public hardware_interface::SystemInterface
+{
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(CCDHardwareInterface)
+
+  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;
+
+  void logger_function();
+
+  // Mirrors servo's DecodedCommand for maintenance frame unpacking
+  struct DecodedCommand
+  {
+    uint8_t command_id;
+    std::vector<uint8_t> u8_data;
+    std::vector<int16_t> i16_data;
+    std::vector<int32_t> i32_data;
+  };
+
+  struct CCDJoint
+  {
+    std::string name;
+    uint32_t can_id;
+
+    // State interfaces
+    double is_connected;
+    double command_success;
+    double acquisition_in_progress;
+    double data_ready;
+    double frames_received;
+    double last_frame_id;
+    double status;
+
+    // Command interfaces
+    double capture_binary_cmd;
+    double capture_byte_cmd;
+    double status_request;
+    double maintenance_request;
+    double maintenance_frame_high;
+    double maintenance_frame_low;
+    double maintenance_data_count;
+
+    // Tracking
+    double prev_status_request;
+    double prev_maintenance_request;
+    double elapsed_status_request_time;
+    double elapsed_maintenance_request_time;
+
+    // Internal acquisition state
+    bool waiting_for_binary_data;
+    bool waiting_for_byte_data;
+    double maintenance_frame;
+    std::vector<uint8_t> decoded_maintenance_frame;
+    std::vector<uint8_t> binary_pixels;
+    std::vector<uint8_t> byte_pixels;
+  };
+
+private:
+  void on_can_message(const CANLib::CanFrame & frame);
+  bool format_maintenance_command(
+    CANLib::CanFrame & frame, uint32_t can_id, const DecodedCommand & decoded_cmd);
+
+  std::string can_interface_;
+  CANLib::SocketCanBus canBus_;
+  CANLib::CanFrame can_tx_frame_;
+  bool can_connected_;
+
+  int update_rate_;
+  int logger_rate_;
+  int logger_state_;
+  double elapsed_time_;
+  double elapsed_logger_time_;
+
+  rclcpp::Node::SharedPtr pixel_pub_node_;
+  rclcpp::Publisher<msgs::msg::RamanSpectrum>::SharedPtr pixel_publisher_;
+
+  std::vector<CCDJoint> CCDJoints_;
+
+  static constexpr uint8_t CMD_REQUEST_BINARY = 0x20;
+  static constexpr uint8_t CMD_REQUEST_BYTE   = 0x30;
+  static constexpr uint8_t CONFIRM_SEND       = 0x01;
+
+  // CAN response arbitration IDs per protocol
+  static constexpr uint32_t CAN_RESP_ACK_ID    = 0x101;
+  static constexpr uint32_t CAN_RESP_BINARY_ID = 0x102;
+  static constexpr uint32_t CAN_RESP_BYTE_ID   = 0x103;
+  static constexpr uint32_t CAN_PIXEL_PUB_ID = 0x104;
+
+  inline DecodedCommand unpack_command_full(int32_t counts_in, int64_t payload_in)
+  {
+    const uint32_t counts  = static_cast<uint32_t>(counts_in);
+    const uint64_t payload = static_cast<uint64_t>(payload_in);
+
+    const uint8_t u8_count  = static_cast<uint8_t>((counts >> 16) & 0xFF);
+    const uint8_t i16_count = static_cast<uint8_t>((counts >> 8)  & 0xFF);
+    const uint8_t i32_count = static_cast<uint8_t>( counts        & 0xFF);
+
+    int bit_offset = 64;
+    auto pop_bits = [&](int bits) -> uint64_t {
+      bit_offset -= bits;
+      const uint64_t mask = (bits == 64)
+        ? std::numeric_limits<uint64_t>::max()
+        : ((1ULL << bits) - 1ULL);
+      return (payload >> bit_offset) & mask;
+    };
+
+    DecodedCommand result{};
+    result.command_id = static_cast<uint8_t>(pop_bits(8));
+    for (uint8_t i = 0; i < u8_count;  ++i)
+      result.u8_data.push_back(static_cast<uint8_t>(pop_bits(8)));
+    for (uint8_t i = 0; i < i16_count; ++i)
+      result.i16_data.push_back(static_cast<int16_t>(static_cast<uint16_t>(pop_bits(16))));
+    for (uint8_t i = 0; i < i32_count; ++i)
+      result.i32_data.push_back(static_cast<int32_t>(static_cast<uint32_t>(pop_bits(32))));
+    return result;
+  }
+};
+
+}  // namespace ccd_ros2_control
+
+#endif  // CCD_ROS2_CONTROL__CCD_HARDWARE_INTERFACE_HPP_