diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index bb9126b9..5f2544ff 100644 --- a/.github/workflows/main.yml +++ b/.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 diff --git a/src/description/ros2_control/science/science.ccd.ros2_control.xacro b/src/description/ros2_control/science/science.ccd.ros2_control.xacro new file mode 100644 index 00000000..0d51b324 --- /dev/null +++ b/src/description/ros2_control/science/science.ccd.ros2_control.xacro @@ -0,0 +1,43 @@ + + + + + + + + + ccd_ros2_control/CCDHardwareInterface + ${can_interface} + 0x100 + 10 + 5 + 0 + + + + 0 + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/description/ros2_control/science/science.dc.ros2_control.xacro b/src/description/ros2_control/science/science.dc.ros2_control.xacro index 24f7d2b6..008294f5 100644 --- a/src/description/ros2_control/science/science.dc.ros2_control.xacro +++ b/src/description/ros2_control/science/science.dc.ros2_control.xacro @@ -17,7 +17,6 @@ 0 revolute - 50.9 true diff --git a/src/description/ros2_control/science/science.laser.ros2_control.xacro b/src/description/ros2_control/science/science.laser.ros2_control.xacro index 6fea9233..d68d5b68 100644 --- a/src/description/ros2_control/science/science.laser.ros2_control.xacro +++ b/src/description/ros2_control/science/science.laser.ros2_control.xacro @@ -1,7 +1,7 @@ - + diff --git a/src/description/ros2_control/science/science.sensor_diode.ros2_control.xacro b/src/description/ros2_control/science/science.sensor_diode.ros2_control.xacro new file mode 100644 index 00000000..1fdeb90d --- /dev/null +++ b/src/description/ros2_control/science/science.sensor_diode.ros2_control.xacro @@ -0,0 +1,36 @@ + + + + + + + + + sensor_diode_ros2_control/SensorDiodeHardwareInterface + ${can_interface} + 0x110 + 10 + 5 + 0 + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/description/urdf/athena_science.urdf.xacro b/src/description/urdf/athena_science.urdf.xacro index d8e053bb..d7075d5e 100644 --- a/src/description/urdf/athena_science.urdf.xacro +++ b/src/description/urdf/athena_science.urdf.xacro @@ -13,6 +13,8 @@ + + @@ -44,6 +46,12 @@ + + + + + + diff --git a/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt b/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt new file mode 100644 index 00000000..c1c6aa68 --- /dev/null +++ b/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 +$ +$ +) + +# 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() diff --git a/src/hardware_interfaces/ccd_ros2_control/ccd_hardware_interface.xml b/src/hardware_interfaces/ccd_ros2_control/ccd_hardware_interface.xml new file mode 100644 index 00000000..1908c0dd --- /dev/null +++ b/src/hardware_interfaces/ccd_ros2_control/ccd_hardware_interface.xml @@ -0,0 +1,9 @@ + + + + Hardware interface for spectrometry ccd control via ros2_control. + + + diff --git a/src/hardware_interfaces/ccd_ros2_control/include/ccd_ros2_control/ccd_hardware_interface.hpp b/src/hardware_interfaces/ccd_ros2_control/include/ccd_ros2_control/ccd_hardware_interface.hpp new file mode 100644 index 00000000..b1301df6 --- /dev/null +++ b/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 +#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/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 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; + + void logger_function(); + + // Mirrors servo's DecodedCommand for maintenance frame unpacking + struct DecodedCommand + { + uint8_t command_id; + std::vector u8_data; + std::vector i16_data; + std::vector 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 decoded_maintenance_frame; + std::vector binary_pixels; + std::vector 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::SharedPtr pixel_publisher_; + + std::vector 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(counts_in); + const uint64_t payload = static_cast(payload_in); + + const uint8_t u8_count = static_cast((counts >> 16) & 0xFF); + const uint8_t i16_count = static_cast((counts >> 8) & 0xFF); + const uint8_t i32_count = static_cast( 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::max() + : ((1ULL << bits) - 1ULL); + return (payload >> bit_offset) & mask; + }; + + DecodedCommand result{}; + result.command_id = static_cast(pop_bits(8)); + for (uint8_t i = 0; i < u8_count; ++i) + result.u8_data.push_back(static_cast(pop_bits(8))); + for (uint8_t i = 0; i < i16_count; ++i) + result.i16_data.push_back(static_cast(static_cast(pop_bits(16)))); + for (uint8_t i = 0; i < i32_count; ++i) + result.i32_data.push_back(static_cast(static_cast(pop_bits(32)))); + return result; + } +}; + +} // namespace ccd_ros2_control + +#endif // CCD_ROS2_CONTROL__CCD_HARDWARE_INTERFACE_HPP_ \ No newline at end of file diff --git a/src/hardware_interfaces/ccd_ros2_control/package.xml b/src/hardware_interfaces/ccd_ros2_control/package.xml new file mode 100644 index 00000000..2ed0f15f --- /dev/null +++ b/src/hardware_interfaces/ccd_ros2_control/package.xml @@ -0,0 +1,35 @@ + + + + ccd_ros2_control + 0.0.0 + TODO: Package description + henry + TODO: License declaration + + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + ament_cmake + realtime_tools + msgs + msgs + ament_cmake + rosidl_default_runtime + rosidl_interface_packages + + + umdloop_can_library + umdloop_can_library + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/src/hardware_interfaces/ccd_ros2_control/src/ccd_hardware_interface.cpp b/src/hardware_interfaces/ccd_ros2_control/src/ccd_hardware_interface.cpp new file mode 100644 index 00000000..eb20e702 --- /dev/null +++ b/src/hardware_interfaces/ccd_ros2_control/src/ccd_hardware_interface.cpp @@ -0,0 +1,446 @@ +#include "ccd_ros2_control/ccd_hardware_interface.hpp" + +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include + +namespace ccd_ros2_control +{ + +void CCDHardwareInterface::logger_function() +{ + if (CCDJoints_.empty()) return; + const auto & joint = CCDJoints_.front(); + + std::ostringstream oss; + oss << "\033[2J\033[H \nCCD Logger" + << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface_ + << " | CAN ID: 0x" << std::hex << std::uppercase << joint.can_id << std::dec + << " | Update Rate: " << update_rate_ + << " | Logger Rate: " << logger_rate_ << "\n" + << "Elapsed Time: " << elapsed_time_ << "\n" + << "\n--- Joint Specific ---\n" + << "JOINT: " << joint.name << "\n" + << "-- Commands --\n" + << "Capture Binary: " << joint.capture_binary_cmd + << " | Capture Byte: " << joint.capture_byte_cmd << "\n" + << "Status Request: " << joint.status_request + << " | Maintenance Request: " << joint.maintenance_request << "\n" + << "-- State --\n" + << "Is Connected: " << joint.is_connected + << " | Command Success: " << joint.command_success + << " | Acquisition Active: " << joint.acquisition_in_progress + << " | Data Ready: " << joint.data_ready << "\n" + << "Frames Received: " << joint.frames_received + << " | Last Frame ID: " << joint.last_frame_id + << " | Status: " << joint.status << "\n"; + + RCLCPP_INFO(rclcpp::get_logger("CCDHardwareInterface"), "%s", oss.str().c_str()); +} + +bool CCDHardwareInterface::format_maintenance_command( + CANLib::CanFrame & frame, uint32_t can_id, const DecodedCommand & decoded_cmd) +{ + frame.data.fill(0); + frame.id = can_id; + frame.dlc = 2; + frame.data[0] = decoded_cmd.command_id; + if (decoded_cmd.u8_data.size() != 1 || + !decoded_cmd.i16_data.empty() || + !decoded_cmd.i32_data.empty()) + { + return false; + } + frame.data[1] = decoded_cmd.u8_data[0]; + return true; +} + +hardware_interface::CallbackReturn CCDHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + can_interface_ = info_.hardware_parameters.count("can_interface") ? + info_.hardware_parameters.at("can_interface") : "can0"; + update_rate_ = info_.hardware_parameters.count("update_rate") ? + std::stoi(info_.hardware_parameters.at("update_rate")) : 10; + logger_rate_ = info_.hardware_parameters.count("logger_rate") ? + std::stoi(info_.hardware_parameters.at("logger_rate")) : 5; + logger_state_ = info_.hardware_parameters.count("logger_state") ? + std::stoi(info_.hardware_parameters.at("logger_state")) : 0; + + uint32_t can_id = 0x100; + if (info_.hardware_parameters.count("can_id")) { + try { + can_id = static_cast( + std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); + } catch (const std::exception & e) { + RCLCPP_ERROR(rclcpp::get_logger("CCDHardwareInterface"), + "Failed to parse 'can_id': %s", e.what()); + return hardware_interface::CallbackReturn::ERROR; + } + } + + CCDJoints_.clear(); + CCDJoints_.push_back(CCDJoint{ + info_.gpios[0].name, + can_id, + // state + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + // command + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + // tracking + 0.0, 0.0, 0.0, 0.0, + // internal + false, false, 0.0, + {}, + std::vector(3648, 0), + std::vector(3648, 0) + }); + + can_connected_ = false; + elapsed_time_ = 0.0; + elapsed_logger_time_ = 0.0; + + RCLCPP_INFO(rclcpp::get_logger("CCDHardwareInterface"), + "CCD initialized on %s, CAN ID 0x%X", can_interface_.c_str(), can_id); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn CCDHardwareInterface::on_configure( + const rclcpp_lifecycle::State &) +{ + auto & joint = CCDJoints_.front(); + + if (can_connected_) { + canBus_.close(); + can_connected_ = false; + } + + if (!canBus_.open(can_interface_, + std::bind(&CCDHardwareInterface::on_can_message, this, std::placeholders::_1))) + { + RCLCPP_WARN(rclcpp::get_logger("CCDHardwareInterface"), + "Failed to open CAN interface %s", can_interface_.c_str()); + can_connected_ = false; + } else { + can_connected_ = true; + } + + joint.is_connected = can_connected_ ? 1.0 : 0.0; + joint.status = joint.is_connected; + joint.command_success = 0.0; + joint.acquisition_in_progress = 0.0; + joint.data_ready = 0.0; + joint.frames_received = 0.0; + joint.last_frame_id = 0.0; + + pixel_pub_node_ = rclcpp::Node::make_shared("ccd_hwi_pixel_publisher"); + pixel_publisher_ = pixel_pub_node_->create_publisher( + "/raman/raw_pixels", rclcpp::QoS(1).reliable()); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +void CCDHardwareInterface::on_can_message(const CANLib::CanFrame & frame) +{ + auto & joint = CCDJoints_.front(); + + // ACK frame on 0x101 — success/fail for the last request + if (frame.id == CAN_RESP_ACK_ID && frame.dlc >= 2) { + const uint8_t cmd_byte = frame.data[0]; + const bool success = frame.data[1] != 0; + joint.command_success = success ? 1.0 : 0.0; + joint.status = success ? 1.0 : 0.0; + + if (cmd_byte == CMD_REQUEST_BINARY) { + if (success) { + joint.waiting_for_binary_data = true; + joint.waiting_for_byte_data = false; + joint.acquisition_in_progress = 1.0; + joint.data_ready = 0.0; + joint.frames_received = 0.0; + joint.last_frame_id = 0.0; + std::fill(joint.binary_pixels.begin(), joint.binary_pixels.end(), 0); + } else { + joint.acquisition_in_progress = 0.0; + } + } else if (cmd_byte == CMD_REQUEST_BYTE) { + if (success) { + joint.waiting_for_byte_data = true; + joint.waiting_for_binary_data = false; + joint.acquisition_in_progress = 1.0; + joint.data_ready = 0.0; + joint.frames_received = 0.0; + joint.last_frame_id = 0.0; + std::fill(joint.byte_pixels.begin(), joint.byte_pixels.end(), 0); + } else { + joint.acquisition_in_progress = 0.0; + } + } + return; + } + + // Binary data frames on 0x102 + if (frame.id == CAN_RESP_BINARY_ID && joint.waiting_for_binary_data && frame.dlc == 8) { + const uint16_t frame_id = static_cast(frame.data[0]); + joint.last_frame_id = static_cast(frame_id); + joint.frames_received += 1.0; + + const size_t start_idx = static_cast(frame_id) * 56; + for (size_t byte_idx = 0; byte_idx < 7; ++byte_idx) { + const uint8_t packed = frame.data[1 + byte_idx]; + for (size_t bit = 0; bit < 8; ++bit) { + const size_t pixel_idx = start_idx + byte_idx * 8 + bit; + if (pixel_idx < joint.binary_pixels.size()) { + joint.binary_pixels[pixel_idx] = static_cast((packed >> bit) & 0x01); + } + } + } + + if (joint.frames_received >= 66.0) { + joint.waiting_for_binary_data = false; + joint.acquisition_in_progress = 0.0; + joint.data_ready = 1.0; + RCLCPP_INFO(rclcpp::get_logger("CCDHardwareInterface"), "Binary acquisition complete"); + } + return; + } + + // Byte data frames on 0x103 + if (frame.id == CAN_RESP_BYTE_ID && joint.waiting_for_byte_data && frame.dlc == 8) { + const uint16_t frame_id = + static_cast(frame.data[0]) | + (static_cast(frame.data[1]) << 8); + + joint.last_frame_id = static_cast(frame_id); + joint.frames_received += 1.0; + + const size_t start_idx = static_cast(frame_id) * 6; + for (size_t i = 0; i < 6; ++i) { + const size_t pixel_idx = start_idx + i; + if (pixel_idx < joint.byte_pixels.size()) { + joint.byte_pixels[pixel_idx] = frame.data[2 + i]; + } + } + + if (joint.frames_received >= 608.0) { + joint.waiting_for_byte_data = false; + joint.acquisition_in_progress = 0.0; + joint.data_ready = 1.0; + + // Publish completed pixel buffer + if (pixel_publisher_) { + msgs::msg::RamanSpectrum msg; + msg.header.stamp = rclcpp::Clock().now(); + msg.header.frame_id = "ccd_sensor"; + msg.spectrometer_id = "pda_spectrometer"; + msg.accumulations = 1; + msg.intensities.reserve(joint.byte_pixels.size()); + for (const auto & px : joint.byte_pixels) { + msg.intensities.push_back(static_cast(px)); + } + pixel_publisher_->publish(msg); + rclcpp::spin_some(pixel_pub_node_); + } + RCLCPP_INFO(rclcpp::get_logger("CCDHardwareInterface"), "8-bit acquisition complete"); + } + return; + } + +} + +std::vector +CCDHardwareInterface::export_state_interfaces() +{ + auto & joint = CCDJoints_.front(); + std::vector state_interfaces; + state_interfaces.emplace_back(joint.name, "is_connected", &joint.is_connected); + state_interfaces.emplace_back(joint.name, "command_success", &joint.command_success); + state_interfaces.emplace_back(joint.name, "acquisition_in_progress", &joint.acquisition_in_progress); + state_interfaces.emplace_back(joint.name, "data_ready", &joint.data_ready); + state_interfaces.emplace_back(joint.name, "frames_received", &joint.frames_received); + state_interfaces.emplace_back(joint.name, "last_frame_id", &joint.last_frame_id); + state_interfaces.emplace_back(joint.name, "status", &joint.status); + return state_interfaces; +} + +std::vector +CCDHardwareInterface::export_command_interfaces() +{ + auto & joint = CCDJoints_.front(); + std::vector command_interfaces; + command_interfaces.emplace_back(joint.name, "capture_binary", &joint.capture_binary_cmd); + command_interfaces.emplace_back(joint.name, "capture_byte", &joint.capture_byte_cmd); + command_interfaces.emplace_back(joint.name, "status_request", &joint.status_request); + command_interfaces.emplace_back(joint.name, "maintenance_request", &joint.maintenance_request); + command_interfaces.emplace_back(joint.name, "maintenance_frame_high", &joint.maintenance_frame_high); + command_interfaces.emplace_back(joint.name, "maintenance_frame_low", &joint.maintenance_frame_low); + command_interfaces.emplace_back(joint.name, "maintenance_data_count", &joint.maintenance_data_count); + return command_interfaces; +} + +hardware_interface::CallbackReturn CCDHardwareInterface::on_activate( + const rclcpp_lifecycle::State &) +{ + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn CCDHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State &) +{ + auto & joint = CCDJoints_.front(); + joint.acquisition_in_progress = 0.0; + joint.waiting_for_binary_data = false; + joint.waiting_for_byte_data = false; + + if (can_connected_) { + canBus_.close(); + can_connected_ = false; + joint.is_connected = 0.0; + joint.status = 0.0; + } + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn CCDHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State &) +{ + auto & joint = CCDJoints_.front(); + if (can_connected_) { + canBus_.close(); + } + can_connected_ = false; + joint.is_connected = 0.0; + joint.status = 0.0; + joint.acquisition_in_progress = 0.0; + joint.waiting_for_binary_data = false; + joint.waiting_for_byte_data = false; + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn CCDHardwareInterface::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + return on_cleanup(previous_state); +} + +hardware_interface::return_type CCDHardwareInterface::read( + const rclcpp::Time &, const rclcpp::Duration &) +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type CCDHardwareInterface::write( + const rclcpp::Time &, const rclcpp::Duration & period) +{ + auto & joint = CCDJoints_.front(); + + elapsed_time_ += period.seconds(); + elapsed_logger_time_ += period.seconds(); + if (logger_rate_ > 0 && + elapsed_logger_time_ > (1.0 / static_cast(logger_rate_))) + { + elapsed_logger_time_ = 0.0; + if (logger_state_ == 1) logger_function(); + } + + // --- Capture commands (one-shot on rising edge) --- + if (joint.capture_binary_cmd > 0.5) { + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.can_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = CMD_REQUEST_BINARY; + can_tx_frame_.data[1] = CONFIRM_SEND; + canBus_.send(can_tx_frame_); + } + joint.data_ready = 0.0; + joint.command_success = 0.0; + joint.capture_binary_cmd = 0.0; + } + + if (joint.capture_byte_cmd > 0.5) { + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.can_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = CMD_REQUEST_BYTE; + can_tx_frame_.data[1] = CONFIRM_SEND; + canBus_.send(can_tx_frame_); + } + joint.data_ready = 0.0; + joint.command_success = 0.0; + joint.capture_byte_cmd = 0.0; + } + + // --- Status request: one-shot (negative) or heartbeat (positive Hz) --- + const double curr_status_req = joint.status_request; + if (curr_status_req < 0.0 && joint.prev_status_request >= 0.0) { + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.can_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = CMD_REQUEST_BINARY; + can_tx_frame_.data[1] = CONFIRM_SEND; + canBus_.send(can_tx_frame_); + } + } else if (curr_status_req > 0.0) { + joint.elapsed_status_request_time += period.seconds(); + if (joint.elapsed_status_request_time > (1.0 / curr_status_req)) { + joint.elapsed_status_request_time = 0.0; + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.can_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = CMD_REQUEST_BINARY; + can_tx_frame_.data[1] = CONFIRM_SEND; + canBus_.send(can_tx_frame_); + } + } + } + joint.prev_status_request = curr_status_req; + + // --- Maintenance request: same one-shot/heartbeat pattern as servo --- + auto doubles_to_payload = [](double high, double low) -> int64_t { + return static_cast( + (static_cast(high) << 32) | static_cast(low)); + }; + + joint.maintenance_frame = static_cast(doubles_to_payload( + joint.maintenance_frame_high, joint.maintenance_frame_low)); + const auto decoded_maintenance_cmd = unpack_command_full( + static_cast(joint.maintenance_data_count), + static_cast(joint.maintenance_frame)); + + CANLib::CanFrame maint_frame; + if (format_maintenance_command(maint_frame, joint.can_id, decoded_maintenance_cmd)) { + const double curr_maint_req = joint.maintenance_request; + if (curr_maint_req < 0.0 && joint.prev_maintenance_request >= 0.0) { + if (can_connected_) canBus_.send(maint_frame); + } else if (curr_maint_req > 0.0) { + joint.elapsed_maintenance_request_time += period.seconds(); + if (joint.elapsed_maintenance_request_time > (1.0 / curr_maint_req)) { + joint.elapsed_maintenance_request_time = 0.0; + if (can_connected_) canBus_.send(maint_frame); + } + } + joint.prev_maintenance_request = joint.maintenance_request; + } + + return hardware_interface::return_type::OK; +} + +} // namespace ccd_ros2_control + +PLUGINLIB_EXPORT_CLASS( + ccd_ros2_control::CCDHardwareInterface, + hardware_interface::SystemInterface) \ No newline at end of file diff --git a/src/hardware_interfaces/laser_ros2_control/include/laser_ros2_control/laser_hardware_interface.hpp b/src/hardware_interfaces/laser_ros2_control/include/laser_ros2_control/laser_hardware_interface.hpp index a8e5d56b..e15a3ce9 100644 --- a/src/hardware_interfaces/laser_ros2_control/include/laser_ros2_control/laser_hardware_interface.hpp +++ b/src/hardware_interfaces/laser_ros2_control/include/laser_ros2_control/laser_hardware_interface.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -25,25 +26,33 @@ class LaserHardwareInterface : public hardware_interface::SystemInterface 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; + const rclcpp::Time & time, + const rclcpp::Duration & period) override; + hardware_interface::return_type write( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + const rclcpp::Time & time, + const rclcpp::Duration & period) override; void logger_function(); @@ -62,9 +71,11 @@ class LaserHardwareInterface : public hardware_interface::SystemInterface double elapsed_status_request_time; }; - void onCanMessage(const CANLib::CanFrame & frame); - + void on_can_message(const CANLib::CanFrame & frame); std::string can_interface_; + uint32_t can_id_; + uint8_t port_id_; + CANLib::SocketCanBus canBus_; CANLib::CanFrame can_tx_frame_; bool can_connected_; @@ -75,9 +86,9 @@ class LaserHardwareInterface : public hardware_interface::SystemInterface double elapsed_time_; double elapsed_logger_time_; - static constexpr uint8_t CMD_LASER_ON = 0x60; - static constexpr uint8_t CMD_LASER_OFF = 0x80; - static constexpr uint8_t CMD_READ_TEMP = 0x85; + // CAN command bytes + static constexpr uint8_t CMD_LASER_CONTROL = 0x20; + static constexpr uint8_t CMD_LASER_STATUS = 0x30; }; } // namespace laser_ros2_control diff --git a/src/hardware_interfaces/laser_ros2_control/src/laser_hardware_interface.cpp b/src/hardware_interfaces/laser_ros2_control/src/laser_hardware_interface.cpp index 7e3c528e..efcd4b77 100644 --- a/src/hardware_interfaces/laser_ros2_control/src/laser_hardware_interface.cpp +++ b/src/hardware_interfaces/laser_ros2_control/src/laser_hardware_interface.cpp @@ -1,5 +1,6 @@ #include "laser_ros2_control/laser_hardware_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_list_macros.hpp" #include "rclcpp/rclcpp.hpp" @@ -41,7 +42,7 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) { if (hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) + hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } @@ -54,52 +55,105 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_init( std::stoi(info_.hardware_parameters.at("logger_rate")) : 0; logger_state_ = info_.hardware_parameters.count("logger_state") ? std::stoi(info_.hardware_parameters.at("logger_state")) : 0; - const uint32_t can_id = info_.hardware_parameters.count("can_id") ? + + can_id_ = info_.hardware_parameters.count("can_id") ? static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)) : 0x130; + // port_id is the lower nibble of can_id by convention, or default to 0 + port_id_ = info_.hardware_parameters.count("port_id") ? + static_cast(std::stoul(info_.hardware_parameters.at("port_id"), nullptr, 0)) : 0x00; + LASERJoints_.clear(); LASERJoints_.push_back(LaserJoint{ info_.gpios[0].name, - can_id, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 + can_id_, + 0.0, // laser_state + 0.0, // temperature + 0.0, // is_connected + 0.0, // status + 0.0, // laser_command + 0.0, // status_request + 0.0, // prev_status_request + 0.0 // elapsed_status_request_time }); - can_connected_ = false; - elapsed_time_ = 0.0; + can_connected_ = false; + elapsed_time_ = 0.0; elapsed_logger_time_ = 0.0; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn LaserHardwareInterface::on_configure( const rclcpp_lifecycle::State &) { + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Configuring laser hardware..."); + + // Close existing connection if re-configuring + if (can_connected_) { + canBus_.close(); + can_connected_ = false; + } + if (!canBus_.open( - can_interface_, - std::bind(&LaserHardwareInterface::onCanMessage, this, std::placeholders::_1))) + can_interface_, + std::bind(&LaserHardwareInterface::on_can_message, 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()); } - auto & joint = LASERJoints_.front(); + + auto & joint = LASERJoints_.front(); joint.is_connected = can_connected_ ? 1.0 : 0.0; - joint.status = joint.is_connected; + joint.status = joint.is_connected; + + 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) +void LaserHardwareInterface::on_can_message(const CANLib::CanFrame & frame) { auto & joint = LASERJoints_.front(); - if (frame.id == joint.can_id && frame.dlc > 1 && frame.data[0] == CMD_READ_TEMP) { - joint.temperature = static_cast(frame.data[1]); - joint.status = 1.0; + + if (frame.id != can_id_ || frame.dlc < 2) { + return; + } + + // 0x20 + port_id: laser control confirmation (protocol 0x20 response) + if (frame.data[0] == static_cast(CMD_LASER_CONTROL + port_id_)) { + const bool success = frame.data[1] != 0; + joint.status = success ? 1.0 : 0.0; + if (success) { + // Confirm the state we commanded is now reflected in hardware + joint.laser_state = joint.laser_command > 0.5 ? 1.0 : 0.0; + } + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Laser control %s from CAN", success ? "confirmed" : "failed"); + return; + } + + // 0x30 + port_id: status response (protocol 0x30 response) + if (frame.data[0] == static_cast(CMD_LASER_STATUS + port_id_)) { + const bool success = frame.data[1] != 0; + joint.status = success ? 1.0 : 0.0; + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Laser status response: %s", success ? "OK" : "FAILED"); + return; } } @@ -107,10 +161,10 @@ std::vector LaserHardwareInterface::export_s { auto & joint = LASERJoints_.front(); std::vector state_interfaces; - state_interfaces.emplace_back(joint.name, "laser_state", &joint.laser_state); - state_interfaces.emplace_back(joint.name, "temperature", &joint.temperature); + state_interfaces.emplace_back(joint.name, "laser_state", &joint.laser_state); + state_interfaces.emplace_back(joint.name, "temperature", &joint.temperature); state_interfaces.emplace_back(joint.name, "is_connected", &joint.is_connected); - state_interfaces.emplace_back(joint.name, "status", &joint.status); + state_interfaces.emplace_back(joint.name, "status", &joint.status); return state_interfaces; } @@ -118,7 +172,7 @@ std::vector LaserHardwareInterface::export { auto & joint = LASERJoints_.front(); std::vector command_interfaces; - command_interfaces.emplace_back(joint.name, "laser_command", &joint.laser_command); + command_interfaces.emplace_back(joint.name, "laser_command", &joint.laser_command); command_interfaces.emplace_back(joint.name, "status_request", &joint.status_request); return command_interfaces; } @@ -133,14 +187,25 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_deactivate( const rclcpp_lifecycle::State &) { auto & joint = LASERJoints_.front(); + + // Safety: send laser OFF before deactivating if (can_connected_) { - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint.can_id; - can_tx_frame_.dlc = 1; - can_tx_frame_.data[0] = CMD_LASER_OFF; + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.can_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = static_cast(CMD_LASER_CONTROL + port_id_); + can_tx_frame_.data[1] = 0; canBus_.send(can_tx_frame_); + + canBus_.close(); + can_connected_ = false; + joint.is_connected = 0.0; } - joint.laser_state = 0.0; + + joint.laser_state = 0.0; + joint.laser_command = 0.0; + joint.status = 0.0; + return hardware_interface::CallbackReturn::SUCCESS; } @@ -148,17 +213,23 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_cleanup( const rclcpp_lifecycle::State &) { auto & joint = LASERJoints_.front(); + if (can_connected_) { - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint.can_id; - can_tx_frame_.dlc = 1; - can_tx_frame_.data[0] = CMD_LASER_OFF; + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.can_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = static_cast(CMD_LASER_CONTROL + port_id_); + can_tx_frame_.data[1] = 0; canBus_.send(can_tx_frame_); canBus_.close(); } - can_connected_ = false; + + can_connected_ = false; joint.is_connected = 0.0; - joint.status = 0.0; + joint.laser_state = 0.0; + joint.laser_command = 0.0; + joint.status = 0.0; + return hardware_interface::CallbackReturn::SUCCESS; } @@ -179,50 +250,66 @@ hardware_interface::return_type LaserHardwareInterface::write( { auto & joint = LASERJoints_.front(); - elapsed_time_ += period.seconds(); + elapsed_time_ += period.seconds(); elapsed_logger_time_ += period.seconds(); - if (logger_rate_ > 0 && elapsed_logger_time_ > (1.0 / static_cast(logger_rate_))) { + if (logger_rate_ > 0 && + elapsed_logger_time_ > (1.0 / static_cast(logger_rate_))) + { elapsed_logger_time_ = 0.0; if (logger_state_ == 1) { logger_function(); } } + // --- Laser on/off: send on state change --- const bool commanded_on = joint.laser_command > 0.5; - const bool currently_on = joint.laser_state > 0.5; + const bool currently_on = joint.laser_state > 0.5; if (commanded_on != currently_on) { if (can_connected_) { - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint.can_id; - can_tx_frame_.dlc = 1; - can_tx_frame_.data[0] = commanded_on ? CMD_LASER_ON : CMD_LASER_OFF; + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.can_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = static_cast(CMD_LASER_CONTROL + port_id_); + can_tx_frame_.data[1] = commanded_on ? 1 : 0; canBus_.send(can_tx_frame_); + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Laser command sent: %s", commanded_on ? "ON" : "OFF"); + } else { + // Simulation mode: update state immediately, no hardware to confirm + joint.laser_state = commanded_on ? 1.0 : 0.0; + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Laser turned %s (simulated)", commanded_on ? "ON" : "OFF"); } - joint.laser_state = commanded_on ? 1.0 : 0.0; } - if (joint.status_request < 0.0 && joint.prev_status_request >= 0.0) { + // --- Status request: one-shot (negative) or heartbeat (positive Hz) --- + const double curr_status_req = joint.status_request; + if (curr_status_req < 0.0 && joint.prev_status_request >= 0.0) { if (can_connected_) { - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint.can_id; - can_tx_frame_.dlc = 1; - can_tx_frame_.data[0] = CMD_READ_TEMP; + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.can_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = static_cast(CMD_LASER_STATUS + port_id_); + can_tx_frame_.data[1] = 1; canBus_.send(can_tx_frame_); } - } else if (joint.status_request > 0.0) { + } else if (curr_status_req > 0.0) { joint.elapsed_status_request_time += period.seconds(); - if (joint.elapsed_status_request_time > (1.0 / joint.status_request)) { + if (joint.elapsed_status_request_time > (1.0 / curr_status_req)) { joint.elapsed_status_request_time = 0.0; if (can_connected_) { - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint.can_id; - can_tx_frame_.dlc = 1; - can_tx_frame_.data[0] = CMD_READ_TEMP; + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.can_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = static_cast(CMD_LASER_STATUS + port_id_); + can_tx_frame_.data[1] = 1; canBus_.send(can_tx_frame_); } } } - joint.prev_status_request = joint.status_request; + joint.prev_status_request = curr_status_req; return hardware_interface::return_type::OK; } @@ -230,4 +317,5 @@ hardware_interface::return_type LaserHardwareInterface::write( } // namespace laser_ros2_control PLUGINLIB_EXPORT_CLASS( - laser_ros2_control::LaserHardwareInterface, hardware_interface::SystemInterface) + laser_ros2_control::LaserHardwareInterface, + hardware_interface::SystemInterface) \ No newline at end of file diff --git a/src/hardware_interfaces/sensor_diode_ros2_control/CMakeLists.txt b/src/hardware_interfaces/sensor_diode_ros2_control/CMakeLists.txt new file mode 100644 index 00000000..2701cb83 --- /dev/null +++ b/src/hardware_interfaces/sensor_diode_ros2_control/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.8) +project(sensor_diode_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( + sensor_diode_ros2_control + SHARED + src/sensor_diode_hardware_interface.cpp +) + +target_compile_features(sensor_diode_ros2_control PUBLIC cxx_std_20) +target_include_directories(sensor_diode_ros2_control PUBLIC +$ +$ +) + +# Link CAN library +target_link_libraries(sensor_diode_ros2_control PUBLIC umdloop_can_library::umdloop_can_library) + +ament_target_dependencies( + sensor_diode_ros2_control PUBLIC + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +# Export hardware plugins +pluginlib_export_plugin_description_file(hardware_interface sensor_diode_hardware_interface.xml) + +# INSTALL +install( + DIRECTORY include/ + DESTINATION include/sensor_diode_ros2_control +) + +install(TARGETS sensor_diode_ros2_control + EXPORT export_sensor_diode_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_sensor_diode_ros2_control) +ament_export_include_directories(include) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/hardware_interfaces/sensor_diode_ros2_control/include/sensor_diode_ros2_control/sensor_diode_hardware_interface.hpp b/src/hardware_interfaces/sensor_diode_ros2_control/include/sensor_diode_ros2_control/sensor_diode_hardware_interface.hpp new file mode 100644 index 00000000..bbc071d7 --- /dev/null +++ b/src/hardware_interfaces/sensor_diode_ros2_control/include/sensor_diode_ros2_control/sensor_diode_hardware_interface.hpp @@ -0,0 +1,152 @@ +#ifndef SENSOR_DIODE_ROS2_CONTROL__SENSOR_DIODE_HARDWARE_INTERFACE_HPP_ +#define SENSOR_DIODE_ROS2_CONTROL__SENSOR_DIODE_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/CanFrame.hpp" +#include "umdloop_can_library/SocketCanBus.hpp" + +namespace sensor_diode_ros2_control +{ + +class SensorDiodeHardwareInterface : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(SensorDiodeHardwareInterface) + + 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; + + void logger_function(); + +private: + struct SensorDiodeJoint + { + std::string name; + uint32_t can_id; + + // State interfaces + double wavelength_intensity; + double command_success; + double is_connected; + double status; + + // Command interfaces + double request_measurement_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; + double prev_request_measurement_cmd; + + // Internal + bool awaiting_response; + double maintenance_frame; + }; + + void onCanMessage(const CANLib::CanFrame & frame); + + std::string can_interface_; + uint32_t can_id_; + uint8_t port_id_; + + CANLib::SocketCanBus canBus_; + CANLib::CanFrame can_tx_frame_; + bool can_connected_; + + std::vector DIODEJoints_; + + int update_rate_; + int logger_rate_; + int logger_state_; + double elapsed_time_; + double elapsed_logger_time_; + + static constexpr uint8_t CMD_READ_DIODE_VALUE = 0x20; + static constexpr uint8_t CONFIRM_SEND = 0x01; + + // Maintenance frame unpacking — same pattern as servo/CCD + struct DecodedCommand + { + uint8_t command_id; + std::vector u8_data; + std::vector i16_data; + std::vector i32_data; + }; + + inline DecodedCommand unpack_command_full(int32_t counts_in, int64_t payload_in) + { + const uint32_t counts = static_cast(counts_in); + const uint64_t payload = static_cast(payload_in); + + const uint8_t u8_count = static_cast((counts >> 16) & 0xFF); + const uint8_t i16_count = static_cast((counts >> 8) & 0xFF); + const uint8_t i32_count = static_cast( 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::max() + : ((1ULL << bits) - 1ULL); + return (payload >> bit_offset) & mask; + }; + + DecodedCommand result{}; + result.command_id = static_cast(pop_bits(8)); + for (uint8_t i = 0; i < u8_count; ++i) + result.u8_data.push_back(static_cast(pop_bits(8))); + for (uint8_t i = 0; i < i16_count; ++i) + result.i16_data.push_back(static_cast(static_cast(pop_bits(16)))); + for (uint8_t i = 0; i < i32_count; ++i) + result.i32_data.push_back(static_cast(static_cast(pop_bits(32)))); + return result; + } +}; + +} // namespace sensor_diode_ros2_control + +#endif // SENSOR_DIODE_ROS2_CONTROL__SENSOR_DIODE_HARDWARE_INTERFACE_HPP_ \ No newline at end of file diff --git a/src/hardware_interfaces/sensor_diode_ros2_control/package.xml b/src/hardware_interfaces/sensor_diode_ros2_control/package.xml new file mode 100644 index 00000000..72b68e21 --- /dev/null +++ b/src/hardware_interfaces/sensor_diode_ros2_control/package.xml @@ -0,0 +1,27 @@ + + + + sensor_diode_ros2_control + 0.0.0 + TODO: Package description + henry + TODO: License declaration + + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + + umdloop_can_library + umdloop_can_library + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/hardware_interfaces/sensor_diode_ros2_control/sensor_diode_hardware_interface.xml b/src/hardware_interfaces/sensor_diode_ros2_control/sensor_diode_hardware_interface.xml new file mode 100644 index 00000000..dfab98f9 --- /dev/null +++ b/src/hardware_interfaces/sensor_diode_ros2_control/sensor_diode_hardware_interface.xml @@ -0,0 +1,9 @@ + + + + Hardware interface for spectrometry sensor_diode control via ros2_control. + + + diff --git a/src/hardware_interfaces/sensor_diode_ros2_control/src/sensor_diode_hardware_interface.cpp b/src/hardware_interfaces/sensor_diode_ros2_control/src/sensor_diode_hardware_interface.cpp new file mode 100644 index 00000000..0419e7cc --- /dev/null +++ b/src/hardware_interfaces/sensor_diode_ros2_control/src/sensor_diode_hardware_interface.cpp @@ -0,0 +1,332 @@ +#include "sensor_diode_ros2_control/sensor_diode_hardware_interface.hpp" + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/rclcpp.hpp" + +#include + +namespace sensor_diode_ros2_control +{ + +void SensorDiodeHardwareInterface::logger_function() +{ + if (DIODEJoints_.empty()) return; + const auto & joint = DIODEJoints_.front(); + + std::ostringstream oss; + oss << "\033[2J\033[H \nSENSOR DIODE Logger" + << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface_ + << " | CAN ID: 0x" << std::hex << std::uppercase << joint.can_id << std::dec + << " | Port ID: " << static_cast(port_id_) + << " | Update Rate: " << update_rate_ + << " | Logger Rate: " << logger_rate_ << "\n" + << "Elapsed Time: " << elapsed_time_ << "\n" + << "\n--- Joint Specific ---\n" + << "JOINT: " << joint.name << "\n" + << "-- Commands --\n" + << "Request Measurement: " << joint.request_measurement_cmd + << " | Status Request: " << joint.status_request + << " | Maintenance Request: " << joint.maintenance_request << "\n" + << "-- State --\n" + << "Wavelength Intensity: " << joint.wavelength_intensity + << " | Command Success: " << joint.command_success + << " | Is Connected: " << joint.is_connected + << " | Status: " << joint.status << "\n"; + + RCLCPP_INFO(rclcpp::get_logger("SensorDiodeHardwareInterface"), "%s", oss.str().c_str()); +} + +hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + can_interface_ = info_.hardware_parameters.count("can_interface") ? + info_.hardware_parameters.at("can_interface") : "can0"; + update_rate_ = info_.hardware_parameters.count("update_rate") ? + std::stoi(info_.hardware_parameters.at("update_rate")) : 10; + logger_rate_ = info_.hardware_parameters.count("logger_rate") ? + std::stoi(info_.hardware_parameters.at("logger_rate")) : 5; + logger_state_ = info_.hardware_parameters.count("logger_state") ? + std::stoi(info_.hardware_parameters.at("logger_state")) : 0; + + can_id_ = info_.hardware_parameters.count("can_id") ? + static_cast( + std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)) : 0x110; + port_id_ = info_.hardware_parameters.count("port_id") ? + static_cast( + std::stoul(info_.hardware_parameters.at("port_id"), nullptr, 0)) : 0x00; + + DIODEJoints_.clear(); + DIODEJoints_.push_back(SensorDiodeJoint{ + info_.gpios[0].name, + can_id_, + // state + 0.0, 0.0, 0.0, 0.0, + // command + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + // tracking + 0.0, 0.0, 0.0, 0.0, 0.0, + // internal + false, 0.0 + }); + + can_connected_ = false; + elapsed_time_ = 0.0; + elapsed_logger_time_ = 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Initialized sensor diode on %s, CAN ID 0x%X, port %u", + can_interface_.c_str(), can_id_, port_id_); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_configure( + const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Configuring sensor diode hardware..."); + + if (can_connected_) { + canBus_.close(); + can_connected_ = false; + } + + if (!canBus_.open( + can_interface_, + std::bind(&SensorDiodeHardwareInterface::onCanMessage, this, std::placeholders::_1))) + { + RCLCPP_WARN( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "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("SensorDiodeHardwareInterface"), + "Successfully opened CAN interface %s", can_interface_.c_str()); + } + + auto & joint = DIODEJoints_.front(); + joint.is_connected = can_connected_ ? 1.0 : 0.0; + joint.status = joint.is_connected; + joint.command_success = 0.0; + joint.awaiting_response = false; + + RCLCPP_INFO( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Sensor diode configured (%s)", can_connected_ ? "CAN MODE" : "SIMULATION"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +void SensorDiodeHardwareInterface::onCanMessage(const CANLib::CanFrame & frame) +{ + if (frame.id != can_id_ || frame.dlc < 2) return; + + auto & joint = DIODEJoints_.front(); + + // 0x20 + port_id: diode value response + if (frame.data[0] == static_cast(CMD_READ_DIODE_VALUE + port_id_)) { + joint.wavelength_intensity = static_cast(frame.data[1]); + joint.command_success = 1.0; + joint.status = 1.0; + joint.awaiting_response = false; + RCLCPP_DEBUG( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Diode value received: %.0f", joint.wavelength_intensity); + } +} + +std::vector +SensorDiodeHardwareInterface::export_state_interfaces() +{ + auto & joint = DIODEJoints_.front(); + std::vector state_interfaces; + state_interfaces.emplace_back(joint.name, "wavelength_intensity", &joint.wavelength_intensity); + state_interfaces.emplace_back(joint.name, "command_success", &joint.command_success); + state_interfaces.emplace_back(joint.name, "is_connected", &joint.is_connected); + state_interfaces.emplace_back(joint.name, "status", &joint.status); + return state_interfaces; +} + +std::vector +SensorDiodeHardwareInterface::export_command_interfaces() +{ + auto & joint = DIODEJoints_.front(); + std::vector command_interfaces; + command_interfaces.emplace_back(joint.name, "request_measurement", &joint.request_measurement_cmd); + command_interfaces.emplace_back(joint.name, "status_request", &joint.status_request); + command_interfaces.emplace_back(joint.name, "maintenance_request", &joint.maintenance_request); + command_interfaces.emplace_back(joint.name, "maintenance_frame_high", &joint.maintenance_frame_high); + command_interfaces.emplace_back(joint.name, "maintenance_frame_low", &joint.maintenance_frame_low); + command_interfaces.emplace_back(joint.name, "maintenance_data_count", &joint.maintenance_data_count); + return command_interfaces; +} + +hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_activate( + const rclcpp_lifecycle::State &) +{ + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State &) +{ + auto & joint = DIODEJoints_.front(); + joint.awaiting_response = false; + joint.request_measurement_cmd = 0.0; + + if (can_connected_) { + canBus_.close(); + can_connected_ = false; + joint.is_connected = 0.0; + joint.status = 0.0; + } + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State &) +{ + auto & joint = DIODEJoints_.front(); + + if (can_connected_) { + canBus_.close(); + } + + can_connected_ = false; + joint.is_connected = 0.0; + joint.status = 0.0; + joint.wavelength_intensity = 0.0; + joint.command_success = 0.0; + joint.awaiting_response = false; + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + return on_cleanup(previous_state); +} + +hardware_interface::return_type SensorDiodeHardwareInterface::read( + const rclcpp::Time &, const rclcpp::Duration &) +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type SensorDiodeHardwareInterface::write( + const rclcpp::Time &, const rclcpp::Duration & period) +{ + auto & joint = DIODEJoints_.front(); + + elapsed_time_ += period.seconds(); + elapsed_logger_time_ += period.seconds(); + if (logger_rate_ > 0 && + elapsed_logger_time_ > (1.0 / static_cast(logger_rate_))) + { + elapsed_logger_time_ = 0.0; + if (logger_state_ == 1) logger_function(); + } + + // --- Measurement request: one-shot on rising edge > 0.5 --- + if (joint.request_measurement_cmd > 0.5 && joint.prev_request_measurement_cmd <= 0.5) { + joint.command_success = 0.0; + joint.awaiting_response = true; + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.can_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = static_cast(CMD_READ_DIODE_VALUE + port_id_); + can_tx_frame_.data[1] = CONFIRM_SEND; + canBus_.send(can_tx_frame_); + RCLCPP_INFO( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Requested diode measurement on port %u", port_id_); + } + } + joint.prev_request_measurement_cmd = joint.request_measurement_cmd; + + // --- Status request: one-shot (negative) or heartbeat (positive Hz) --- + const double curr_status_req = joint.status_request; + if (curr_status_req < 0.0 && joint.prev_status_request >= 0.0) { + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.can_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = static_cast(CMD_READ_DIODE_VALUE + port_id_); + can_tx_frame_.data[1] = CONFIRM_SEND; + canBus_.send(can_tx_frame_); + } + } else if (curr_status_req > 0.0) { + joint.elapsed_status_request_time += period.seconds(); + if (joint.elapsed_status_request_time > (1.0 / curr_status_req)) { + joint.elapsed_status_request_time = 0.0; + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.can_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = static_cast(CMD_READ_DIODE_VALUE + port_id_); + can_tx_frame_.data[1] = CONFIRM_SEND; + canBus_.send(can_tx_frame_); + } + } + } + joint.prev_status_request = curr_status_req; + + // --- Maintenance request: one-shot (negative) or heartbeat (positive Hz) --- + auto doubles_to_payload = [](double high, double low) -> int64_t { + return static_cast( + (static_cast(high) << 32) | static_cast(low)); + }; + + joint.maintenance_frame = static_cast(doubles_to_payload( + joint.maintenance_frame_high, joint.maintenance_frame_low)); + const auto decoded = unpack_command_full( + static_cast(joint.maintenance_data_count), + static_cast(joint.maintenance_frame)); + + // Only send if the decoded command has valid u8 payload + if (decoded.u8_data.size() == 1 && + decoded.i16_data.empty() && + decoded.i32_data.empty()) + { + CANLib::CanFrame maint_frame{}; + maint_frame.id = joint.can_id; + maint_frame.dlc = 2; + maint_frame.data[0] = decoded.command_id; + maint_frame.data[1] = decoded.u8_data[0]; + + const double curr_maint_req = joint.maintenance_request; + if (curr_maint_req < 0.0 && joint.prev_maintenance_request >= 0.0) { + if (can_connected_) canBus_.send(maint_frame); + } else if (curr_maint_req > 0.0) { + joint.elapsed_maintenance_request_time += period.seconds(); + if (joint.elapsed_maintenance_request_time > (1.0 / curr_maint_req)) { + joint.elapsed_maintenance_request_time = 0.0; + if (can_connected_) canBus_.send(maint_frame); + } + } + joint.prev_maintenance_request = joint.maintenance_request; + } + + return hardware_interface::return_type::OK; +} + +} // namespace sensor_diode_ros2_control + +PLUGINLIB_EXPORT_CLASS( + sensor_diode_ros2_control::SensorDiodeHardwareInterface, + hardware_interface::SystemInterface) \ No newline at end of file diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index d2de6a2b..41606fb5 100644 --- a/src/msgs/CMakeLists.txt +++ b/src/msgs/CMakeLists.txt @@ -20,6 +20,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/JointStatus.msg" "msg/SystemInfo.msg" "msg/Heading.msg" + "msg/RamanPeaks.msg" + "msg/RamanSpectrum.msg" "msg/LedStatus.msg" "srv/SetController.srv" "srv/StatusReq.srv" diff --git a/src/msgs/msg/RamanPeaks.msg b/src/msgs/msg/RamanPeaks.msg new file mode 100644 index 00000000..420b2da0 --- /dev/null +++ b/src/msgs/msg/RamanPeaks.msg @@ -0,0 +1,6 @@ +std_msgs/Header header +string spectrometer_id + +float64[] peak_wavenumbers +float64[] peak_intensities +float64[] peak_widths \ No newline at end of file diff --git a/src/msgs/msg/RamanSpectrum.msg b/src/msgs/msg/RamanSpectrum.msg new file mode 100644 index 00000000..70995382 --- /dev/null +++ b/src/msgs/msg/RamanSpectrum.msg @@ -0,0 +1,9 @@ +std_msgs/Header header + +float64[] wavenumber_axis +float64[] intensities + +string spectrometer_id +float64 integration_time_ms +float64 laser_wavelength_nm +uint32 accumulations \ No newline at end of file diff --git a/src/subsystems/science/raman_spectrometer/raman_driver/package.xml b/src/subsystems/science/raman_spectrometer/raman_driver/package.xml new file mode 100644 index 00000000..82015bc9 --- /dev/null +++ b/src/subsystems/science/raman_spectrometer/raman_driver/package.xml @@ -0,0 +1,19 @@ + + + + raman_driver + 0.1.0 + ROS2 driver node for Raman spectrometer PDA acquisition + UMD Loop + Apache-2.0 + + ament_python + + rclpy + raman_msgs + std_msgs + + + ament_python + + \ No newline at end of file diff --git a/src/subsystems/science/raman_spectrometer/raman_driver/raman_driver/__init__.py b/src/subsystems/science/raman_spectrometer/raman_driver/raman_driver/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/subsystems/science/raman_spectrometer/raman_driver/raman_driver/raman_driver_node.py b/src/subsystems/science/raman_spectrometer/raman_driver/raman_driver/raman_driver_node.py new file mode 100644 index 00000000..579b957b --- /dev/null +++ b/src/subsystems/science/raman_spectrometer/raman_driver/raman_driver/raman_driver_node.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +""" +ROS2 node that acquires raw spectra from the PDA and publishes RamanSpectrum messages. +Adapt _acquire_spectrum() to your actual hardware/radio interface. +""" +import rclpy +from rclpy.node import Node +from raman_msgs.msg import RamanSpectrum +import numpy as np + + +class RamanDriverNode(Node): + def __init__(self): + super().__init__('raman_driver_node') + + # Parameters + self.declare_parameter('acquisition_rate_hz', 1.0) + self.declare_parameter('integration_time_ms', 100.0) + self.declare_parameter('laser_wavelength_nm', 785.0) + self.declare_parameter('spectrometer_id', 'pda_spectrometer') + self.declare_parameter('num_photodiodes', 3648) + self.declare_parameter('wavenumber_min', 200.0) + self.declare_parameter('wavenumber_max', 3500.0) + + rate = self.get_parameter('acquisition_rate_hz').value + self.integration_time = self.get_parameter('integration_time_ms').value + self.laser_wl = self.get_parameter('laser_wavelength_nm').value + self.spec_id = self.get_parameter('spectrometer_id').value + self.num_pd = self.get_parameter('num_photodiodes').value + wn_min = self.get_parameter('wavenumber_min').value + wn_max = self.get_parameter('wavenumber_max').value + + # Pixel-to-wavenumber calibration + # TODO: Replace with actual calibration (polynomial or lookup table) + self.wavenumber_axis = np.linspace(wn_min, wn_max, self.num_pd) + + # Publisher + self.pub = self.create_publisher(RamanSpectrum, '/raman/raw_spectrum', 10) + + # Timer-driven acquisition + period = 1.0 / rate + self.timer = self.create_timer(period, self.acquire_and_publish) + + self.get_logger().info( + f'RamanDriverNode started: {self.num_pd} photodiodes, ' + f'{rate} Hz, integration={self.integration_time} ms' + ) + + def _acquire_spectrum(self) -> np.ndarray: + """ + Acquire a single spectrum from hardware. Returns intensity array. + + TODO: Replace with actual acquisition: + - Send trigger command to PDA over CAN/radio + - Read back pixel intensity values + - Reassemble CAN frames into full array + + Current implementation: simulated spectrum with 0-255 uint8 range for testing. + """ + intensities = np.random.randint(0, 256, self.num_pd).astype(np.float64) + + # Simulated Lorentzian peaks (scaled to 0-255 range) + for center_wn, amp, width in [ + (1001, 200, 8), (1031, 120, 10), (1155, 80, 12), + (1450, 60, 15), (1602, 150, 10), (2852, 100, 20), + (2904, 130, 18), (3054, 70, 22), + ]: + intensities += amp * (width ** 2) / ( + (self.wavenumber_axis - center_wn) ** 2 + width ** 2 + ) + + # Clamp to 0-255 range + intensities = np.clip(intensities, 0, 255) + return intensities + + def acquire_and_publish(self): + intensities = self._acquire_spectrum() + + msg = RamanSpectrum() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'raman_probe' + msg.wavenumber_axis = self.wavenumber_axis.tolist() + msg.intensities = intensities.tolist() + msg.spectrometer_id = self.spec_id + msg.integration_time_ms = self.integration_time + msg.laser_wavelength_nm = self.laser_wl + msg.accumulations = 1 + + self.pub.publish(msg) + self.get_logger().debug('Published raw spectrum') + + +def main(args=None): + rclpy.init(args=args) + node = RamanDriverNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/subsystems/science/raman_spectrometer/raman_driver/resource/raman_driver b/src/subsystems/science/raman_spectrometer/raman_driver/resource/raman_driver new file mode 100644 index 00000000..e69de29b diff --git a/src/subsystems/science/raman_spectrometer/raman_driver/setup.cfg b/src/subsystems/science/raman_spectrometer/raman_driver/setup.cfg new file mode 100644 index 00000000..de8ccb04 --- /dev/null +++ b/src/subsystems/science/raman_spectrometer/raman_driver/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/raman_driver +[install] +install_scripts=$base/lib/raman_driver diff --git a/src/subsystems/science/raman_spectrometer/raman_driver/setup.py b/src/subsystems/science/raman_spectrometer/raman_driver/setup.py new file mode 100644 index 00000000..09b10c6c --- /dev/null +++ b/src/subsystems/science/raman_spectrometer/raman_driver/setup.py @@ -0,0 +1,30 @@ +from setuptools import find_packages, setup + +package_name = 'raman_driver' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='henry', + maintainer_email='henrygdnr@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'raman_driver_node = raman_driver.raman_driver_node:main', + ], + }, +) diff --git a/src/subsystems/science/raman_spectrometer/raman_driver/test/test_copyright.py b/src/subsystems/science/raman_spectrometer/raman_driver/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/src/subsystems/science/raman_spectrometer/raman_driver/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/subsystems/science/raman_spectrometer/raman_driver/test/test_flake8.py b/src/subsystems/science/raman_spectrometer/raman_driver/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/subsystems/science/raman_spectrometer/raman_driver/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/subsystems/science/raman_spectrometer/raman_driver/test/test_pep257.py b/src/subsystems/science/raman_spectrometer/raman_driver/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/subsystems/science/raman_spectrometer/raman_driver/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/subsystems/science/science_bringup/CMakeLists.txt b/src/subsystems/science/science_bringup/CMakeLists.txt index 280abdff..09ac8e6b 100644 --- a/src/subsystems/science/science_bringup/CMakeLists.txt +++ b/src/subsystems/science/science_bringup/CMakeLists.txt @@ -20,6 +20,12 @@ install( DESTINATION share/${PROJECT_NAME} ) +install(PROGRAMS + scripts/ccd_can_simulator.py + scripts/sensor_diode_can_simulator.py + DESTINATION lib/${PROJECT_NAME} +) + if(BUILD_TESTING) endif() 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 aec7761f..ecad386a 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -15,9 +15,20 @@ science_controller: type: science_controllers/ScienceManual + # CCD Controller (CAN-based) + ccd_controller: + type: science_controllers/CCDSnapshotController + + # Laser GPIO Controller (CAN-based) laser_gpio_controller: type: gpio_controllers/GpioCommandController + sensor_diode_gpio_controller: + type: gpio_controllers/GpioCommandController + + motor_status_broadcaster: + type: general_controllers/MotorStatusBroadcaster + fluoro_led_gpio_controller: type: gpio_controllers/GpioCommandController @@ -79,6 +90,26 @@ command_interfaces: [position, velocity] state_interfaces: [position, velocity] +ccd_controller: + ros__parameters: + ccd_name: "spectrometry_ccd" + command_interfaces: ["capture_byte"] + state_interfaces: + - is_connected + - command_success + - acquisition_in_progress + - data_ready + - frames_received + - last_frame_id + + snapshot_service_name: "~/request_snapshot" + status_publish_topic: "~/status" + snapshot_publish_topic: "/raman/raw_spectrum" + + publish_rate: 10.0 + acquisition_timeout_sec: 5.0 + expected_total_frames: 609 + # Laser GPIO Controller Configuration /**/laser_gpio_controller: ros__parameters: @@ -92,10 +123,26 @@ spectrometry_laser: interfaces: - laser_state - - temperature - is_connected -/**/fluoro_led_gpio_controller: +# Sensor Diode Controller Configuration +sensor_diode_gpio_controller: + ros__parameters: + gpios: + - sensor_diode + command_interfaces: + sensor_diode: + interfaces: + - request_measurement + state_interfaces: + sensor_diode: + interfaces: + - wavelength_intensity + - command_success + - is_connected + - status + +fluoro_led_gpio_controller: ros__parameters: gpios: - fluoro_led @@ -131,12 +178,16 @@ - servo_auger_lift gpios: - spectrometry_laser + - sensor_diode - fluoro_led - science_limit_switch command_interfaces: spectrometry_laser: interfaces: - status_request + sensor_diode: + interfaces: + - status_request fluoro_led: interfaces: - status_request @@ -191,10 +242,16 @@ servo_auger_lift: interfaces: - status + sensor_diode: + interfaces: + - status fluoro_led: interfaces: - status science_limit_switch: interfaces: - status + spectrometry_laser: + interfaces: + - status publish_rate: 10.0 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 fdf3be13..67d5a8a8 100644 --- a/src/subsystems/science/science_bringup/launch/athena_science.launch.py +++ b/src/subsystems/science/science_bringup/launch/athena_science.launch.py @@ -46,6 +46,7 @@ def generate_launch_description(): description="YAML file with the controllers configuration.", ) ) + declared_arguments.append( DeclareLaunchArgument( "description_package", @@ -222,7 +223,7 @@ def generate_launch_description(): )''' # Active Spawners - robot_controller_names = ["science_controller"] # robot_controller + robot_controller_names = ["science_controller", "ccd_controller"] # robot_controller robot_controller_spawners = [] for controller in robot_controller_names: robot_controller_spawners += [ @@ -234,14 +235,20 @@ def generate_launch_description(): ] # GPIO controller spawner for Laser - gpio_controller_names = ["laser_gpio_controller", "fluoro_led_gpio_controller"] + + gpio_controller_names = ["laser_gpio_controller", "fluoro_led_gpio_controller", "sensor_diode_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"], + arguments=[ + controller, + "-c", "/controller_manager", + "--param-file", robot_controllers, # ← add this + ], ) ] diff --git a/src/subsystems/science/science_bringup/package.xml b/src/subsystems/science/science_bringup/package.xml index 6e5c4815..9e51486f 100644 --- a/src/subsystems/science/science_bringup/package.xml +++ b/src/subsystems/science/science_bringup/package.xml @@ -26,6 +26,7 @@ joint_state_broadcaster general_controllers + gpio_controllers joint_trajectory_controller diff --git a/src/subsystems/science/science_bringup/scripts/ccd_can_simulator.py b/src/subsystems/science/science_bringup/scripts/ccd_can_simulator.py new file mode 100644 index 00000000..8b2d19f8 --- /dev/null +++ b/src/subsystems/science/science_bringup/scripts/ccd_can_simulator.py @@ -0,0 +1,282 @@ +#!/usr/bin/env python3 +""" +CCD CAN Bus Simulator +===================== +Simulates the PCB-side CAN responses for the CCD hardware interface. + +Protocol: + Listen on CAN ID 0x100 for: + - 0x20 (CMD_REQUEST_BINARY): triggers 1 ACK + 66 binary data frames + - 0x30 (CMD_REQUEST_BYTE): triggers 1 ACK + 608 byte data frames + + ACK frame: CAN ID 0x101, data[0]=cmd_byte, data[1]=1 (success) + Binary frames: CAN ID 0x102, data[0]=frame_id, data[1..7]=56 bits of pixel data + Byte frames: CAN ID 0x103, data[0]=frame_id_low, data[1]=frame_id_high, + data[2..7]=6 pixel intensity bytes + +Usage: + # Real CAN interface + python3 ccd_can_simulator.py --interface can0 + + # Virtual CAN (for testing without hardware) + sudo modprobe vcan + sudo ip link add dev can0 type vcan + sudo ip link set up can0 + python3 ccd_can_simulator.py --interface can0 + + # Specify mode explicitly + python3 ccd_can_simulator.py --interface can0 --mode byte + python3 ccd_can_simulator.py --interface can0 --mode binary +""" + +import argparse +import struct +import time +import socket +import struct as st +import sys + +# ── CAN constants ──────────────────────────────────────────────────────────── +CAN_CMD_ID = 0x100 # HWI sends requests here +CAN_ACK_ID = 0x101 # We send ACK here +CAN_BINARY_ID = 0x102 # We send binary data frames here +CAN_BYTE_ID = 0x103 # We send byte data frames here + +CMD_REQUEST_BINARY = 0x20 +CMD_REQUEST_BYTE = 0x30 + +# CCD sensor parameters +NUM_PIXELS = 3648 +BINARY_FRAMES = 66 # ceil(3648 / 56) +BYTE_FRAMES = 608 # ceil(3648 / 6) +PIXELS_PER_BINARY_FRAME = 56 +PIXELS_PER_BYTE_FRAME = 6 + +# Timing +INTER_FRAME_DELAY_S = 0.001 # 1ms between frames — adjust to match real PCB + + +# ── CAN socket helpers ─────────────────────────────────────────────────────── + +# Raw CAN frame format: = id(4) + dlc(2) + pad(2) + data(8) +CAN_FRAME_FMT = "=IH2x8s" +CAN_FRAME_SIZE = st.calcsize(CAN_FRAME_FMT) + +def open_can_socket(interface: str) -> socket.socket: + sock = socket.socket(socket.AF_CAN, socket.SOCK_RAW, socket.CAN_RAW) + sock.bind((interface,)) + sock.settimeout(30.0) + print(f"[SIM] Opened CAN socket on {interface}") + return sock + +def send_frame(sock: socket.socket, can_id: int, data: bytes) -> None: + dlc = len(data) + padded = data.ljust(8, b'\x00') + frame = st.pack(CAN_FRAME_FMT, can_id, dlc, padded) + sock.send(frame) + +def recv_frame(sock: socket.socket): + """Returns (can_id, dlc, data) or None on timeout.""" + try: + raw = sock.recv(CAN_FRAME_SIZE) + can_id, dlc, data = st.unpack(CAN_FRAME_FMT, raw) + can_id &= socket.CAN_EFF_MASK # strip flags + return can_id, dlc, data[:dlc] + except TimeoutError: + return None + + +# ── Pixel data generators ──────────────────────────────────────────────────── + +def generate_binary_pixels(num_pixels: int = NUM_PIXELS): + """ + Generates a simple test pattern: alternating 0/1 bits. + Replace with any pattern you want to test (e.g. all bright, all dark, + Gaussian peak, ramp, etc.) + """ + return [(i % 2) for i in range(num_pixels)] + +def generate_byte_pixels(num_pixels: int = NUM_PIXELS): + """ + Generates a synthetic Raman-like spectrum: + - Gaussian peak around pixel 1800 (roughly mid-range) + - Background noise floor + Replace with a flat, ramp, or real reference spectrum as needed. + """ + import math + pixels = [] + peak_center = num_pixels // 2 + peak_width = num_pixels // 10 + for i in range(num_pixels): + gaussian = 200 * math.exp(-0.5 * ((i - peak_center) / peak_width) ** 2) + noise = (i * 7 + 13) % 20 # deterministic noise + value = int(gaussian + noise + 10) + pixels.append(min(255, max(0, value))) + return pixels + + +# ── Frame builders ─────────────────────────────────────────────────────────── + +def build_ack_frame(cmd_byte: int, success: bool = True): + return bytes([cmd_byte, 1 if success else 0]) + +def build_binary_data_frame(frame_id: int, pixels: list, start_pixel: int): + """ + Packs 56 binary pixel values into 7 bytes (8 pixels per byte), little-endian. + frame_id fits in 1 byte (0-65, since 66 frames total). + """ + data = bytearray(8) + data[0] = frame_id & 0xFF + for byte_idx in range(7): + packed = 0 + for bit in range(8): + pixel_idx = start_pixel + byte_idx * 8 + bit + if pixel_idx < len(pixels) and pixels[pixel_idx]: + packed |= (1 << bit) + data[1 + byte_idx] = packed + return bytes(data) + +def build_byte_data_frame(frame_id: int, pixels: list, start_pixel: int): + """ + Packs 6 byte-intensity pixel values. + frame_id is uint16 split across data[0] (low) and data[1] (high). + """ + data = bytearray(8) + data[0] = frame_id & 0xFF + data[1] = (frame_id >> 8) & 0xFF + for i in range(6): + pixel_idx = start_pixel + i + data[2 + i] = pixels[pixel_idx] if pixel_idx < len(pixels) else 0 + return bytes(data) + + +# ── Acquisition simulators ─────────────────────────────────────────────────── + +def simulate_binary_acquisition(sock: socket.socket, port_id: int): + print(f"[SIM] Binary acquisition requested (port {port_id})") + + # 1. Send ACK + ack = build_ack_frame(CMD_REQUEST_BINARY, success=True) + send_frame(sock, CAN_ACK_ID, ack) + print(f"[SIM] Sent ACK on 0x{CAN_ACK_ID:03X}") + time.sleep(INTER_FRAME_DELAY_S) + + # 2. Generate pixel data + pixels = generate_binary_pixels() + + # 3. Send 66 data frames on 0x102 + for frame_id in range(BINARY_FRAMES): + start_pixel = frame_id * PIXELS_PER_BINARY_FRAME + frame_data = build_binary_data_frame(frame_id, pixels, start_pixel) + send_frame(sock, CAN_BINARY_ID, frame_data) + + if frame_id % 10 == 0 or frame_id == BINARY_FRAMES - 1: + print(f"[SIM] Binary frame {frame_id + 1}/{BINARY_FRAMES} " + f"(pixels {start_pixel}–{start_pixel + PIXELS_PER_BINARY_FRAME - 1})") + + time.sleep(INTER_FRAME_DELAY_S) + + print(f"[SIM] Binary acquisition complete — {BINARY_FRAMES} frames sent") + +def simulate_byte_acquisition(sock: socket.socket, port_id: int): + print(f"[SIM] 8-bit acquisition requested (port {port_id})") + + # 1. Send ACK + ack = build_ack_frame(CMD_REQUEST_BYTE, success=True) + send_frame(sock, CAN_ACK_ID, ack) + print(f"[SIM] Sent ACK on 0x{CAN_ACK_ID:03X}") + time.sleep(INTER_FRAME_DELAY_S) + + # 2. Generate pixel data + pixels = generate_byte_pixels() + + # 3. Send 608 data frames on 0x103 + for frame_id in range(BYTE_FRAMES): + start_pixel = frame_id * PIXELS_PER_BYTE_FRAME + frame_data = build_byte_data_frame(frame_id, pixels, start_pixel) + send_frame(sock, CAN_BYTE_ID, frame_data) + + if frame_id % 50 == 0 or frame_id == BYTE_FRAMES - 1: + print(f"[SIM] Byte frame {frame_id + 1}/{BYTE_FRAMES} " + f"(pixels {start_pixel}–{start_pixel + PIXELS_PER_BYTE_FRAME - 1})") + + time.sleep(INTER_FRAME_DELAY_S) + + print(f"[SIM] 8-bit acquisition complete — {BYTE_FRAMES} frames sent") + + + +def run_simulator(interface: str, force_mode: str = None, delay: float = INTER_FRAME_DELAY_S): + global INTER_FRAME_DELAY_S + INTER_FRAME_DELAY_S = delay + sock = open_can_socket(interface) + print(f"[SIM] Listening for CCD requests on CAN ID 0x{CAN_CMD_ID:03X}...") + print(f"[SIM] Inter-frame delay: {INTER_FRAME_DELAY_S * 1000:.1f} ms") + print(f"[SIM] Press Ctrl+C to stop\n") + + try: + while True: + result = recv_frame(sock) + if result is None: + print("[SIM] Timeout waiting for request — still listening...") + continue + + can_id, dlc, data = result + + if can_id != CAN_CMD_ID: + continue # ignore frames not addressed to CCD + + if dlc < 2: + print(f"[SIM] Received short frame on 0x{can_id:03X}, ignoring") + continue + + cmd_byte = data[0] + validate = data[1] + + if validate == 0: + print(f"[SIM] Received unvalidated request (cmd=0x{cmd_byte:02X}), ignoring") + continue + + # Determine port_id from lower nibble + port_id = cmd_byte & 0x0F + cmd_base = cmd_byte & 0xF0 + + if force_mode == "binary" or cmd_base == CMD_REQUEST_BINARY: + simulate_binary_acquisition(sock, port_id) + elif force_mode == "byte" or cmd_base == CMD_REQUEST_BYTE: + simulate_byte_acquisition(sock, port_id) + else: + print(f"[SIM] Unknown command byte 0x{cmd_byte:02X}, ignoring") + + except KeyboardInterrupt: + print("\n[SIM] Stopped by user") + finally: + sock.close() + print("[SIM] CAN socket closed") + + +# ── Entry point ────────────────────────────────────────────────────────────── + +def main(): + parser = argparse.ArgumentParser(description="CCD CAN Bus Simulator") + parser.add_argument( + "--interface", "-i", + default="can0", + help="CAN interface name (default: can0)") + parser.add_argument( + "--mode", "-m", + choices=["binary", "byte"], + default=None, + help="Force acquisition mode regardless of command byte (default: auto-detect)") + parser.add_argument( + "--delay", "-d", + type=float, + default=INTER_FRAME_DELAY_S, + help=f"Inter-frame delay in seconds (default: {INTER_FRAME_DELAY_S})") + + args = parser.parse_args() + + run_simulator(args.interface, force_mode=args.mode) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/subsystems/science/science_bringup/scripts/sensor_diode_can_simulator.py b/src/subsystems/science/science_bringup/scripts/sensor_diode_can_simulator.py new file mode 100755 index 00000000..720fabbb --- /dev/null +++ b/src/subsystems/science/science_bringup/scripts/sensor_diode_can_simulator.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 +import argparse +import math +import time +import socket +import struct as st + +CAN_CMD_ID = 0x110 +CAN_RESP_ID = 0x110 +CMD_READ_DIODE = 0x20 +RESPONSE_DELAY_S = 0.001 + +CAN_FRAME_FMT = "=IH2x8s" +CAN_FRAME_SIZE = st.calcsize(CAN_FRAME_FMT) + +def open_can_socket(interface): + sock = socket.socket(socket.AF_CAN, socket.SOCK_RAW, socket.CAN_RAW) + sock.bind((interface,)) + sock.settimeout(30.0) + print(f"[SIM] Opened CAN socket on {interface}") + return sock + +def send_frame(sock, can_id, data): + dlc = len(data) + padded = data.ljust(8, b'\x00') + frame = st.pack(CAN_FRAME_FMT, can_id, dlc, padded) + sock.send(frame) + +def recv_frame(sock): + try: + raw = sock.recv(CAN_FRAME_SIZE) + can_id, dlc, data = st.unpack(CAN_FRAME_FMT, raw) + can_id &= socket.CAN_EFF_MASK + return can_id, dlc, data[:dlc] + except TimeoutError: + return None + +class IntensityGenerator: + def __init__(self, pattern, fixed_value=128): + self.pattern = pattern + self.fixed_value = fixed_value + self.step = 0 + + def next(self): + val = 0 + if self.pattern == "fixed": + val = self.fixed_value + elif self.pattern == "ramp": + val = self.step % 256 + elif self.pattern == "sine": + val = int(127.5 + 117.5 * math.sin(2 * math.pi * self.step / 100)) + elif self.pattern == "noise": + val = (self.step * 37 + 13) % 256 + elif self.pattern == "gaussian": + center = 100 + width = 20 + pos = self.step % 200 + val = int(240 * math.exp(-0.5 * ((pos - center) / width) ** 2) + 10) + self.step += 1 + return min(255, max(0, val)) + +def build_response_frame(port_id, intensity): + return bytes([(CMD_READ_DIODE + port_id) & 0xFF, intensity & 0xFF]) + +def run_simulator(interface, port_id, pattern, fixed_value, delay): + global RESPONSE_DELAY_S + RESPONSE_DELAY_S = delay + + sock = open_can_socket(interface) + generator = IntensityGenerator(pattern, fixed_value) + + print(f"[SIM] Sensor Diode Simulator") + print(f"[SIM] Listening on CAN ID 0x{CAN_CMD_ID:03X}") + print(f"[SIM] Responding on CAN ID 0x{CAN_RESP_ID:03X}") + print(f"[SIM] Port ID: {port_id}") + print(f"[SIM] Pattern: {pattern}" + (f" (value={fixed_value})" if pattern == "fixed" else "")) + print(f"[SIM] Response delay: {RESPONSE_DELAY_S * 1000:.1f} ms") + print(f"[SIM] Press Ctrl+C to stop\n") + + request_count = 0 + + try: + while True: + result = recv_frame(sock) + if result is None: + print("[SIM] Timeout — still listening...") + continue + + can_id, dlc, data = result + + if can_id != CAN_CMD_ID: + continue + if dlc < 2: + print(f"[SIM] Short frame on 0x{can_id:03X}, ignoring") + continue + + cmd_byte = data[0] + validate = data[1] + + if validate == 0: + print(f"[SIM] Unvalidated request (cmd=0x{cmd_byte:02X}), ignoring") + continue + + received_port = cmd_byte & 0x0F + cmd_base = cmd_byte & 0xF0 + + if cmd_base != CMD_READ_DIODE: + print(f"[SIM] Unknown command 0x{cmd_byte:02X}, ignoring") + continue + + if received_port != port_id: + print(f"[SIM] Request for port {received_port}, we are port {port_id}, ignoring") + continue + + intensity = generator.next() + time.sleep(RESPONSE_DELAY_S) + + send_frame(sock, CAN_RESP_ID, build_response_frame(port_id, intensity)) + + request_count += 1 + print(f"[SIM] Request #{request_count} → port={port_id}, intensity={intensity} (0x{intensity:02X})") + + except KeyboardInterrupt: + print(f"\n[SIM] Stopped — served {request_count} requests") + finally: + sock.close() + print("[SIM] CAN socket closed") + +def main(): + parser = argparse.ArgumentParser(description="Sensor Diode CAN Bus Simulator") + parser.add_argument("--interface", "-i", default="can0") + parser.add_argument("--port", "-p", type=int, default=0) + parser.add_argument("--pattern", choices=["fixed", "ramp", "sine", "noise", "gaussian"], default="sine") + parser.add_argument("--value", "-v", type=int, default=128) + parser.add_argument("--delay", "-d", type=float, default=RESPONSE_DELAY_S) + + args = parser.parse_args() + + run_simulator( + interface = args.interface, + port_id = args.port, + pattern = args.pattern, + fixed_value = args.value, + delay = args.delay + ) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/subsystems/science/science_controllers/CMakeLists.txt b/src/subsystems/science/science_controllers/CMakeLists.txt index 362ec6cf..152ea4ef 100644 --- a/src/subsystems/science/science_controllers/CMakeLists.txt +++ b/src/subsystems/science/science_controllers/CMakeLists.txt @@ -15,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_lifecycle realtime_tools std_srvs + msgs ) find_package(ament_cmake REQUIRED) @@ -30,7 +31,7 @@ endforeach() # Add science_manual library related compile commands generate_parameter_library( science_manual_parameters - src/science_controller.yaml + config/science_controller.yaml include/science_controllers/validate_science_controller_parameters.hpp ) @@ -39,6 +40,14 @@ add_library( SHARED src/science_controller.cpp ) + + +add_library( + ccd_snapshot_controller + SHARED + src/ccd_controller.cpp +) + target_include_directories(science_manual PUBLIC "$" "$" @@ -47,12 +56,20 @@ target_link_libraries(science_manual science_manual_parameters) ament_target_dependencies(science_manual ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(science_manual PRIVATE "SCIENCE_MANUAL_BUILDING_DLL") +target_include_directories(ccd_snapshot_controller PUBLIC + "$" + "$" + "$") +ament_target_dependencies(ccd_snapshot_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(ccd_snapshot_controller PRIVATE "CCD_SNAPSHOT_BUILDING_DLL") + pluginlib_export_plugin_description_file( controller_interface science_controllers.xml) install( TARGETS science_manual + ccd_snapshot_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -69,35 +86,35 @@ install( ) -# if(BUILD_TESTING) - -# ament_add_gmock(test_load_athena_science_manual test/test_load_athena_science_manual.cpp) -# target_include_directories(test_load_athena_science_manual PRIVATE include) -# ament_target_dependencies( -# test_load_athena_science_manual -# controller_manager -# hardware_interface -# ros2_control_test_assets -# ) - -# add_rostest_with_parameters_gmock(test_athena_science_manual test/test_athena_science_manual.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/athena_science_manual_params.yaml) -# target_include_directories(test_athena_science_manual PRIVATE include) -# target_link_libraries(test_athena_science_manual athena_science_manual) -# ament_target_dependencies( -# test_athena_science_manual -# controller_interface -# hardware_interface -# ) - -# add_rostest_with_parameters_gmock(test_athena_science_manual_preceeding test/test_athena_science_manual_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/athena_science_manual_preceeding_params.yaml) -# target_include_directories(test_athena_science_manual_preceeding PRIVATE include) -# target_link_libraries(test_athena_science_manual_preceeding athena_science_manual) -# ament_target_dependencies( -# test_athena_science_manual_preceeding -# controller_interface -# hardware_interface -# ) -# endif() +if(BUILD_TESTING) + + ament_add_gmock(test_load_athena_science_manual test/test_load_athena_science_manual.cpp) + target_include_directories(test_load_athena_science_manual PRIVATE include) + ament_target_dependencies( + test_load_athena_science_manual + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_athena_science_manual test/test_athena_science_manual.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/athena_science_manual_params.yaml) + target_include_directories(test_athena_science_manual PRIVATE include) + target_link_libraries(test_athena_science_manual athena_science_manual) + ament_target_dependencies( + test_athena_science_manual + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock(test_athena_science_manual_preceeding test/test_athena_science_manual_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/athena_science_manual_preceeding_params.yaml) + target_include_directories(test_athena_science_manual_preceeding PRIVATE include) + target_link_libraries(test_athena_science_manual_preceeding science_manual) + ament_target_dependencies( + test_athena_science_manual_preceeding + controller_interface + hardware_interface + ) + endif() ament_export_include_directories( include @@ -107,6 +124,7 @@ ament_export_dependencies( ) ament_export_libraries( science_manual + ccd_snapshot_controller ) ament_package() \ No newline at end of file diff --git a/src/subsystems/science/science_controllers/config/ccd_controller.yaml b/src/subsystems/science/science_controllers/config/ccd_controller.yaml new file mode 100644 index 00000000..d3ef0477 --- /dev/null +++ b/src/subsystems/science/science_controllers/config/ccd_controller.yaml @@ -0,0 +1,34 @@ +controller_manager: + ros__parameters: + ccd_controller: + type: science_controllers/CCDSnapshotController + +ccd_controller: + ros__parameters: + ccd_name: "spectrometry_ccd" + + command_interfaces: + - capture_byte + + state_interfaces: + - is_connected + - command_success + - acquisition_in_progress + - data_ready + - frames_received + - last_frame_id + + snapshot_service_name: "~/request_snapshot" + status_publish_topic: "~/status" + snapshot_publish_topic: "~/snapshot" + + publish_rate: 10.0 + acquisition_timeout_sec: 5.0 + expected_total_frames: 609 + + spectrometer_id: "pda_spectrometer" + integration_time_ms: 100.0 + laser_wavelength_nm: 785.0 + num_photodiodes: 3648 + wavenumber_min: 200.0 + wavenumber_max: 3500.0 \ No newline at end of file diff --git a/src/subsystems/science/science_controllers/src/science_controller.yaml b/src/subsystems/science/science_controllers/config/science_controller.yaml similarity index 100% rename from src/subsystems/science/science_controllers/src/science_controller.yaml rename to src/subsystems/science/science_controllers/config/science_controller.yaml diff --git a/src/subsystems/science/science_controllers/include/science_controllers/ccd_controller.hpp b/src/subsystems/science/science_controllers/include/science_controllers/ccd_controller.hpp new file mode 100644 index 00000000..49292371 --- /dev/null +++ b/src/subsystems/science/science_controllers/include/science_controllers/ccd_controller.hpp @@ -0,0 +1,172 @@ +// Copyright (c) 2025, UMDLoop +// All rights reserved. +// +// Proprietary License +// +// Unauthorized copying of this file, via any medium is strictly prohibited. +// The file is considered confidential. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#ifndef SCIENCE_CONTROLLERS__CCD_SNAPSHOT_CONTROLLER_HPP_ +#define SCIENCE_CONTROLLERS__CCD_SNAPSHOT_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "std_srvs/srv/set_bool.hpp" + +#include "rclcpp_lifecycle/state.hpp" +#include "science_controllers/visibility_control.h" +#include "std_srvs/srv/trigger.hpp" +#include "std_msgs/msg/string.hpp" +#include "msgs/msg/raman_spectrum.hpp" + + +namespace science_controllers +{ + +// name constants for state interfaces +static constexpr size_t STATE_MY_ITFS = 0; + +// name constants for command interfaces +static constexpr size_t CMD_MY_ITFS = 0; + + + +class CCDSnapshotController : public controller_interface::ControllerInterface +{ +public: + SCIENCE_CONTROLLERS__VISIBILITY_PUBLIC + CCDSnapshotController(); + + SCIENCE_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + SCIENCE_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + SCIENCE_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + SCIENCE_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + SCIENCE_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + SCIENCE_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + SCIENCE_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerModeSrvType = std_srvs::srv::SetBool; + + + +protected: + + void handle_snapshot_request( + const std::shared_ptr request, + std::shared_ptr response); + + void publish_status(const rclcpp::Time & time); + void publish_spectrum(const rclcpp::Time & time); + bool acquisition_timed_out(const rclcpp::Time & time) const; + + bool get_state_bool(size_t index) const; + double get_state_double(size_t index) const; + + std::vector make_wavenumber_axis() const; + std::vector get_latest_intensities(); + + std::string ccd_name_; + std::vector command_interface_names_; + std::vector state_interface_names_; + std::vector wavenumber_axis_; + + std::string snapshot_service_name_; + std::string status_publish_topic_; + std::string snapshot_publish_topic_; + + std::string spectrometer_id_; + double integration_time_ms_; + double laser_wavelength_nm_; + int num_photodiodes_; + double wavenumber_min_; + double wavenumber_max_; + double publish_rate_; + double acquisition_timeout_sec_; + int expected_total_frames_; + bool pixel_data_ready_ = false; + + struct SnapshotRequest + { + bool requested = false; + }; + + // Service + realtime_tools::RealtimeBuffer> snapshot_request_buffer_; + rclcpp::Service::SharedPtr snapshot_service_; + + // Publishers + using StatusMsg = std_msgs::msg::String; + using StatusPublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr status_publisher_; + std::unique_ptr realtime_status_publisher_; + + rclcpp::Publisher::SharedPtr spectrum_publisher_; + + // Subscribers + rclcpp::Subscription::SharedPtr pixel_subscriber_; + realtime_tools::RealtimeBuffer> pixel_buffer_; + + bool snapshot_requested_; + bool snapshot_in_progress_; + bool snapshot_complete_published_; + rclcpp::Time acquisition_start_time_; + rclcpp::Time last_status_publish_time_; + +private: + // callback for topic interface + + enum CommandInterfaces + { + CMD_CAPTURE_BYTE = 0, + + // Total number of interfaces + CMD_ITFS_COUNT + }; + + enum StateInterfaces + { + STATE_IS_CONNECTED = 0, + STATE_COMMAND_SUCCESS, + STATE_ACQUISITION_IN_PROGRESS, + STATE_DATA_READY, + STATE_FRAMES_RECEIVED, + STATE_LAST_FRAME_ID, + STATE_ITFS_COUNT + }; + + +}; + +} // namespace science_controllers + + +#endif // SCIENCE_CONTROLLERS__CCDSNAPSHOT_CONTROLLER_HPP_ diff --git a/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp b/src/subsystems/science/science_controllers/include/science_controllers/science_manual.hpp similarity index 100% rename from src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp rename to src/subsystems/science/science_controllers/include/science_controllers/science_manual.hpp diff --git a/src/subsystems/science/science_controllers/package.xml b/src/subsystems/science/science_controllers/package.xml index 0917cbe1..6bafc952 100644 --- a/src/subsystems/science/science_controllers/package.xml +++ b/src/subsystems/science/science_controllers/package.xml @@ -27,6 +27,8 @@ std_srvs + msgs + ament_cmake diff --git a/src/subsystems/science/science_controllers/science_controllers.xml b/src/subsystems/science/science_controllers/science_controllers.xml index aa7dee81..b55b221c 100644 --- a/src/subsystems/science/science_controllers/science_controllers.xml +++ b/src/subsystems/science/science_controllers/science_controllers.xml @@ -18,11 +18,21 @@ Source of this file are templates in [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. --> - + ScienceManual ros2_control controller. + + + + + + CCD snapshot controller for spectrometry acquisition. + + \ No newline at end of file diff --git a/src/subsystems/science/science_controllers/src/ccd_controller.cpp b/src/subsystems/science/science_controllers/src/ccd_controller.cpp new file mode 100644 index 00000000..6b1ad753 --- /dev/null +++ b/src/subsystems/science/science_controllers/src/ccd_controller.cpp @@ -0,0 +1,407 @@ +#include "science_controllers/ccd_controller.hpp" + +#include +#include +#include +#include +#include +#include + +#include "pluginlib/class_list_macros.hpp" + +namespace science_controllers +{ + +CCDSnapshotController::CCDSnapshotController() +: controller_interface::ControllerInterface() +{ +} + +controller_interface::CallbackReturn CCDSnapshotController::on_init() +{ + try { + auto_declare("ccd_name", "spectrometry_ccd"); + auto_declare>("command_interfaces", {"capture_byte"}); + auto_declare>( + "state_interfaces", + { + "is_connected", + "command_success", + "acquisition_in_progress", + "data_ready", + "frames_received", + "last_frame_id" + }); + + auto_declare("snapshot_service_name", "~/request_snapshot"); + auto_declare("status_publish_topic", "~/status"); + auto_declare("snapshot_publish_topic", "/raman/raw_spectrum"); + + auto_declare("spectrometer_id", "pda_spectrometer"); + auto_declare("integration_time_ms", 100.0); + auto_declare("laser_wavelength_nm", 785.0); + auto_declare("num_photodiodes", 3648); + auto_declare("wavenumber_min", 200.0); + auto_declare("wavenumber_max", 3500.0); + + auto_declare("publish_rate", 10.0); + auto_declare("acquisition_timeout_sec", 5.0); + auto_declare("expected_total_frames", 609); + } catch (const std::exception & e) { + fprintf(stderr, "Exception during CCDSnapshotController on_init: %s\n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + snapshot_requested_ = false; + snapshot_in_progress_ = false; + snapshot_complete_published_ = false; + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +CCDSnapshotController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration cfg; + cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (const auto & interface_name : command_interface_names_) { + cfg.names.push_back(ccd_name_ + "/" + interface_name); + } + + return cfg; +} + +controller_interface::InterfaceConfiguration +CCDSnapshotController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration cfg; + cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (const auto & interface_name : state_interface_names_) { + cfg.names.push_back(ccd_name_ + "/" + interface_name); + } + + return cfg; +} + +controller_interface::CallbackReturn CCDSnapshotController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + ccd_name_ = get_node()->get_parameter("ccd_name").as_string(); + command_interface_names_ = get_node()->get_parameter("command_interfaces").as_string_array(); + state_interface_names_ = get_node()->get_parameter("state_interfaces").as_string_array(); + + snapshot_service_name_ = get_node()->get_parameter("snapshot_service_name").as_string(); + status_publish_topic_ = get_node()->get_parameter("status_publish_topic").as_string(); + snapshot_publish_topic_ = get_node()->get_parameter("snapshot_publish_topic").as_string(); + + spectrometer_id_ = get_node()->get_parameter("spectrometer_id").as_string(); + integration_time_ms_ = get_node()->get_parameter("integration_time_ms").as_double(); + laser_wavelength_nm_ = get_node()->get_parameter("laser_wavelength_nm").as_double(); + wavenumber_min_ = get_node()->get_parameter("wavenumber_min").as_double(); + wavenumber_max_ = get_node()->get_parameter("wavenumber_max").as_double(); + + publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); + acquisition_timeout_sec_ = get_node()->get_parameter("acquisition_timeout_sec").as_double(); + + num_photodiodes_ = static_cast(get_node()->get_parameter("num_photodiodes").as_int()); + expected_total_frames_ = static_cast(get_node()->get_parameter("expected_total_frames").as_int()); + + if (command_interface_names_.size() != CMD_ITFS_COUNT) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Expected %zu command interface, got %zu", + static_cast(CMD_ITFS_COUNT), + command_interface_names_.size()); + return controller_interface::CallbackReturn::ERROR; + } + + if (state_interface_names_.size() != STATE_ITFS_COUNT) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Expected %zu state interfaces, got %zu", + static_cast(STATE_ITFS_COUNT), + state_interface_names_.size()); + return controller_interface::CallbackReturn::ERROR; + } + + wavenumber_axis_ = make_wavenumber_axis(); + + snapshot_service_ = get_node()->create_service( + snapshot_service_name_, + std::bind( + &CCDSnapshotController::handle_snapshot_request, + this, + std::placeholders::_1, + std::placeholders::_2)); + + pixel_subscriber_ = get_node()->create_subscription( + "/raman/raw_pixels", + rclcpp::QoS(1).reliable(), + [this](const msgs::msg::RamanSpectrum::SharedPtr msg) { + pixel_buffer_.writeFromNonRT(msg->intensities); + }); + + status_publisher_ = get_node()->create_publisher( + status_publish_topic_, rclcpp::QoS(10)); + + realtime_status_publisher_ = std::make_unique(status_publisher_); + + spectrum_publisher_ = get_node()->create_publisher( + snapshot_publish_topic_, rclcpp::QoS(10)); + + auto initial_request = std::make_shared(); + initial_request->requested = false; + snapshot_request_buffer_.writeFromNonRT(initial_request); + + RCLCPP_INFO( + get_node()->get_logger(), + "Configured CCDSnapshotController for HWI '%s', publishing RamanSpectrum on '%s'", + ccd_name_.c_str(), + snapshot_publish_topic_.c_str()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn CCDSnapshotController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (command_interfaces_.size() != CMD_ITFS_COUNT) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Expected %zu command interfaces, got %zu", + static_cast(CMD_ITFS_COUNT), + command_interfaces_.size()); + return controller_interface::CallbackReturn::ERROR; + } + + if (state_interfaces_.size() != STATE_ITFS_COUNT) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Expected %zu state interfaces, got %zu", + static_cast(STATE_ITFS_COUNT), + state_interfaces_.size()); + return controller_interface::CallbackReturn::ERROR; + } + + command_interfaces_[CMD_CAPTURE_BYTE].set_value(0.0); + + snapshot_requested_ = false; + snapshot_in_progress_ = false; + snapshot_complete_published_ = false; + acquisition_start_time_ = get_node()->now(); + last_status_publish_time_ = get_node()->now(); + + RCLCPP_INFO(get_node()->get_logger(), "CCD snapshot controller activated"); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn CCDSnapshotController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + for (auto & command_interface : command_interfaces_) { + command_interface.set_value(std::numeric_limits::quiet_NaN()); + } + + snapshot_requested_ = false; + snapshot_in_progress_ = false; + snapshot_complete_published_ = false; + + RCLCPP_INFO(get_node()->get_logger(), "CCD snapshot controller deactivated"); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type CCDSnapshotController::update( + const rclcpp::Time & time, + const rclcpp::Duration & /*period*/) +{ + auto request_ptr = snapshot_request_buffer_.readFromRT(); + + if (request_ptr && *request_ptr && (*request_ptr)->requested && !snapshot_requested_ && + !snapshot_in_progress_) + { + snapshot_requested_ = true; + snapshot_in_progress_ = true; + snapshot_complete_published_ = false; + acquisition_start_time_ = time; + + command_interfaces_[CMD_CAPTURE_BYTE].set_value(1.0); + + auto consumed_request = std::make_shared(); + consumed_request->requested = false; + snapshot_request_buffer_.writeFromNonRT(consumed_request); + + RCLCPP_INFO(get_node()->get_logger(), "CCD snapshot request sent"); + } else { + command_interfaces_[CMD_CAPTURE_BYTE].set_value(0.0); + } + + if (snapshot_in_progress_) { + if (get_state_bool(STATE_DATA_READY) || + get_state_double(STATE_FRAMES_RECEIVED) >= static_cast(expected_total_frames_ - 1)) { + if (!snapshot_complete_published_) { + if (!pixel_data_ready_) { + pixel_data_ready_ = true; + return controller_interface::return_type::OK; + } + + publish_spectrum(time); + publish_status(time); + snapshot_complete_published_ = true; + pixel_data_ready_ = false; + } + + snapshot_requested_ = false; + snapshot_in_progress_ = false; + } else if (acquisition_timed_out(time)) { + RCLCPP_ERROR( + get_node()->get_logger(), + "CCD snapshot timed out after %.2f seconds. Frames received: %.0f", + acquisition_timeout_sec_, + get_state_double(STATE_FRAMES_RECEIVED)); + + snapshot_requested_ = false; + snapshot_in_progress_ = false; + snapshot_complete_published_ = false; + publish_status(time); + } + } + + const double publish_period = publish_rate_ > 0.0 ? 1.0 / publish_rate_ : 1.0; + if ((time - last_status_publish_time_).seconds() >= publish_period) { + publish_status(time); + last_status_publish_time_ = time; + } + + return controller_interface::return_type::OK; +} + +void CCDSnapshotController::handle_snapshot_request( + const std::shared_ptr /*request*/, + std::shared_ptr response) +{ + if (snapshot_in_progress_) { + response->success = false; + response->message = "CCD snapshot already in progress"; + return; + } + + if (!get_state_bool(STATE_IS_CONNECTED)) { + response->success = false; + response->message = "CCD HWI is not connected"; + return; + } + + auto request_msg = std::make_shared(); + request_msg->requested = true; + snapshot_request_buffer_.writeFromNonRT(request_msg); + + response->success = true; + response->message = "CCD snapshot request accepted"; +} + +void CCDSnapshotController::publish_status(const rclcpp::Time & /*time*/) +{ + if (!realtime_status_publisher_ || !realtime_status_publisher_->trylock()) { + return; + } + + std::ostringstream ss; + ss << "ccd_name=" << ccd_name_ + << ", connected=" << get_state_double(STATE_IS_CONNECTED) + << ", command_success=" << get_state_double(STATE_COMMAND_SUCCESS) + << ", acquisition_in_progress=" << get_state_double(STATE_ACQUISITION_IN_PROGRESS) + << ", data_ready=" << get_state_double(STATE_DATA_READY) + << ", frames_received=" << get_state_double(STATE_FRAMES_RECEIVED) + << ", last_frame_id=" << get_state_double(STATE_LAST_FRAME_ID) + << ", expected_total_frames=" << expected_total_frames_; + + realtime_status_publisher_->msg_.data = ss.str(); + realtime_status_publisher_->unlockAndPublish(); +} + +void CCDSnapshotController::publish_spectrum(const rclcpp::Time & time) +{ + auto msg = msgs::msg::RamanSpectrum(); + + msg.header.stamp = time; + msg.header.frame_id = "raman_probe"; + msg.spectrometer_id = spectrometer_id_; + msg.integration_time_ms = integration_time_ms_; + msg.laser_wavelength_nm = laser_wavelength_nm_; + msg.accumulations = 1; + + msg.wavenumber_axis = wavenumber_axis_; + msg.intensities = get_latest_intensities(); + + spectrum_publisher_->publish(msg); + + RCLCPP_INFO( + get_node()->get_logger(), + "Published RamanSpectrum snapshot: %zu intensities, %.0f frames received", + msg.intensities.size(), + get_state_double(STATE_FRAMES_RECEIVED)); +} + +bool CCDSnapshotController::acquisition_timed_out(const rclcpp::Time & time) const +{ + return snapshot_in_progress_ && + ((time - acquisition_start_time_).seconds() > acquisition_timeout_sec_); +} + +bool CCDSnapshotController::get_state_bool(size_t index) const +{ + return get_state_double(index) > 0.5; +} + +double CCDSnapshotController::get_state_double(size_t index) const +{ + if (index >= state_interfaces_.size()) { + return 0.0; + } + return state_interfaces_[index].get_value(); +} + +std::vector CCDSnapshotController::make_wavenumber_axis() const +{ + std::vector axis; + + if (num_photodiodes_ <= 0) { + return axis; + } + + axis.resize(static_cast(num_photodiodes_)); + + if (num_photodiodes_ == 1) { + axis[0] = wavenumber_min_; + return axis; + } + + const double step = + (wavenumber_max_ - wavenumber_min_) / static_cast(num_photodiodes_ - 1); + + for (int i = 0; i < num_photodiodes_; ++i) { + axis[static_cast(i)] = wavenumber_min_ + step * static_cast(i); + } + + return axis; +} + +std::vector CCDSnapshotController::get_latest_intensities() +{ + auto buffer_ptr = pixel_buffer_.readFromRT(); + if (buffer_ptr && !(*buffer_ptr).empty()) { + return *buffer_ptr; + } + // Fallback to zeros if buffer not yet populated + return std::vector(static_cast(num_photodiodes_), 0.0); +} + +} // namespace science_controllers + +PLUGINLIB_EXPORT_CLASS( + science_controllers::CCDSnapshotController, + controller_interface::ControllerInterface) \ No newline at end of file diff --git a/src/subsystems/science/science_controllers/src/science_controller.cpp b/src/subsystems/science/science_controllers/src/science_controller.cpp index c6997dd7..e99fcd77 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.cpp +++ b/src/subsystems/science/science_controllers/src/science_controller.cpp @@ -6,7 +6,7 @@ // Unauthorized copying of this file, via any medium is strictly prohibited. // The file is considered confidential. -#include "science_controllers/science_controller.hpp" +#include "science_controllers/science_manual.hpp" #include "science_controllers/science_manual_parameters.hpp" #include