From 8040f92a89847a349f3ba88db39a035373dd4047 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Tue, 31 Mar 2026 14:42:01 -0400 Subject: [PATCH 01/16] Added DC Motor HWI --- .../science/science.dc.ros2_control.xacro | 49 ++ .../urdf/athena_science.urdf.xacro | 8 +- .../dc_ros2_control/CMakeLists.txt | 70 ++ .../dc_ros2_control/dc_hardware_interface.xml | 9 + .../dc_ros2_control/dc_hardware_interface.hpp | 169 +++++ .../dc_ros2_control/package.xml | 35 + .../src/dc_hardware_interface.cpp | 617 ++++++++++++++++++ .../config/athena_science_controllers.yaml | 16 +- .../science_controller.hpp | 1 + .../src/science_controller.cpp | 8 +- .../src/science_controller.yaml | 6 +- 11 files changed, 971 insertions(+), 17 deletions(-) create mode 100644 src/description/ros2_control/science/science.dc.ros2_control.xacro create mode 100644 src/hardware_interfaces/dc_ros2_control/CMakeLists.txt create mode 100644 src/hardware_interfaces/dc_ros2_control/dc_hardware_interface.xml create mode 100644 src/hardware_interfaces/dc_ros2_control/include/dc_ros2_control/dc_hardware_interface.hpp create mode 100644 src/hardware_interfaces/dc_ros2_control/package.xml create mode 100644 src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp diff --git a/src/description/ros2_control/science/science.dc.ros2_control.xacro b/src/description/ros2_control/science/science.dc.ros2_control.xacro new file mode 100644 index 00000000..27eb71b2 --- /dev/null +++ b/src/description/ros2_control/science/science.dc.ros2_control.xacro @@ -0,0 +1,49 @@ + + + + + + + + + dc_ros2_control/DCHardwareInterface + 20 + 5 + 1 + can0 + 0x70 + + + + 0 + revolute + + 50.9 + true + + + + + + + + + + 2 + revolute + + 50.9 + true + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/description/urdf/athena_science.urdf.xacro b/src/description/urdf/athena_science.urdf.xacro index 80fc5314..95e14105 100644 --- a/src/description/urdf/athena_science.urdf.xacro +++ b/src/description/urdf/athena_science.urdf.xacro @@ -6,8 +6,9 @@ - - + + + @@ -30,6 +31,9 @@ + + + diff --git a/src/hardware_interfaces/dc_ros2_control/CMakeLists.txt b/src/hardware_interfaces/dc_ros2_control/CMakeLists.txt new file mode 100644 index 00000000..4c11c388 --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.8) +project(dc_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_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +include_directories(include) + +add_library(dc_ros2_control SHARED + src/dc_hardware_interface.cpp +) + +target_compile_features(dc_ros2_control PUBLIC cxx_std_20) + +target_include_directories(dc_ros2_control PUBLIC + $ + $ +) + +# Link umdloop_can_library directly via CMake target (same as servo HWI) +target_link_libraries(dc_ros2_control PUBLIC umdloop_can_library::umdloop_can_library) + +ament_target_dependencies(dc_ros2_control PUBLIC + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +# Export hardware plugin +pluginlib_export_plugin_description_file(hardware_interface dc_hardware_interface.xml) + +# Install +install( + DIRECTORY include/ + DESTINATION include/dc_ros2_control +) + +install(TARGETS dc_ros2_control + EXPORT export_dc_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_dc_ros2_control) +ament_export_include_directories(include) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() \ No newline at end of file diff --git a/src/hardware_interfaces/dc_ros2_control/dc_hardware_interface.xml b/src/hardware_interfaces/dc_ros2_control/dc_hardware_interface.xml new file mode 100644 index 00000000..76bd8b7e --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/dc_hardware_interface.xml @@ -0,0 +1,9 @@ + + + + UMDLoop's ros2_control plugin for the DC motors + + + \ No newline at end of file diff --git a/src/hardware_interfaces/dc_ros2_control/include/dc_ros2_control/dc_hardware_interface.hpp b/src/hardware_interfaces/dc_ros2_control/include/dc_ros2_control/dc_hardware_interface.hpp new file mode 100644 index 00000000..f4b54f24 --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/include/dc_ros2_control/dc_hardware_interface.hpp @@ -0,0 +1,169 @@ +#ifndef DC_HARDWARE_INTERFACE_HPP_ +#define DC_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 +#include +#include + +#include "umdloop_can_library/SocketCanBus.hpp" +#include "umdloop_can_library/CanFrame.hpp" + +namespace dc_ros2_control +{ +class DCHardwareInterface : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(DCHardwareInterface) + + // Lifecycle + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) 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_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::return_type perform_command_mode_switch( + const std::vector& start_interfaces, + const std::vector& stop_interfaces) 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; + + // -- Helper Functions -- + void on_can_message(const CANLib::CanFrame& frame); + void logger_function(); + + // Unit conversions + double calculate_joint_position_from_motor_position(double motor_pos_deg, int gear_ratio); + double calculate_joint_displacement_from_motor_position(double motor_pos_deg, int gear_ratio, double dist_per_rev); + double calculate_joint_angular_velocity_from_motor_velocity(double motor_vel_dps, int gear_ratio); + double calculate_joint_linear_velocity_from_motor_velocity(double motor_vel_dps, int gear_ratio, double dist_per_rev); + + int16_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); + int16_t calculate_motor_position_from_desired_joint_displacement(double joint_position, int gear_ratio, double dist_per_rev); + int16_t calculate_motor_velocity_from_desired_joint_angular_velocity(double joint_velocity, int gear_ratio); + int16_t calculate_motor_velocity_from_desired_joint_linear_velocity(double joint_velocity, int gear_ratio, double dist_per_rev); + +private: + // Hardware Interface Parameters + int update_rate; + double elapsed_update_time; + double elapsed_time; + double elapsed_logger_time; + int logger_rate; + int logger_state; + int write_count; + + int num_joints; + + // Stores Arbitration IDs + int can_command_id; + uint32_t can_response_id; + + // Joint state (read from hardware) + std::vector joint_state_position_; + std::vector joint_state_velocity_; + + // Previous joint state (change detection) + std::vector prev_joint_state_position_; + std::vector prev_joint_state_velocity_; + + // Joint commands (written to hardware) + std::vector joint_command_position_; + std::vector joint_command_velocity_; + + // Previous joint commands (change detection) + std::vector prev_joint_command_position_; + std::vector prev_joint_command_velocity_; + + // Raw motor data from CAN bus (populated in on_can_message, consumed in read) + std::vector motor_position; + std::vector motor_velocity; + std::vector device_status; + + // Telemetry data (TODO: implement CAN telemetry commands) + std::vector motor_temperature_; + std::vector motor_torque_current_; + + // Maximum displacement for prismatic joints + std::vector max_disp; + + // Prismatic-specific: distance per revolution (future use) + std::vector distance_per_rev; + + // CAN Library + CANLib::SocketCanBus canBus; + CANLib::CanFrame can_tx_frame_; + CANLib::CanFrame can_rx_frame_; + std::string can_interface; + + // Per-joint parameters (read from XACRO) + std::vector joint_node_ids; + std::vector joint_gear_ratios; + std::vector joint_inverted; + + // Control mode + enum class integration_level_t : std::uint8_t + { + UNDEFINED = 0, + POSITION = 1, + VELOCITY = 2, + }; + + std::vector control_level_; + + // Joint type + enum class joint_type_t : std::uint8_t + { + REVOLUTE = 0, + PRISMATIC = 1, + }; + + std::vector joint_type_; + + // CAN Command Bytes (full byte values, used as: CMD + port_id) + static constexpr uint8_t PCB_HEARTBEAT_CMD = 0x10; + static constexpr uint8_t LED_STATUS_CMD = 0x11; + static constexpr uint8_t ABSOLUTE_POS_CONTROL_CMD = 0x20; + static constexpr uint8_t VELOCITY_CONTROL_CMD = 0x30; + static constexpr uint8_t MOTOR_STATE_CMD = 0x40; + static constexpr uint8_t MOTOR_STATUS_CMD = 0x50; + static constexpr uint8_t MAINTENANCE_CMD = 0x60; + static constexpr uint8_t DC_SPECS_CMD = 0x70; +}; + +} // namespace dc_ros2_control + +#endif // DC_HARDWARE_INTERFACE_HPP_ \ No newline at end of file diff --git a/src/hardware_interfaces/dc_ros2_control/package.xml b/src/hardware_interfaces/dc_ros2_control/package.xml new file mode 100644 index 00000000..6f174cc8 --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/package.xml @@ -0,0 +1,35 @@ + + + + dc_ros2_control + 0.0.0 + TODO: Package description + henry + TODO: License declaration + + ament_cmake + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + msgs + + + umdloop_can_library + umdloop_can_library + + ament_lint_auto + ament_lint_common + + ament_cmake + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file diff --git a/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp b/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp new file mode 100644 index 00000000..5cc4e956 --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp @@ -0,0 +1,617 @@ +#include "dc_ros2_control/dc_hardware_interface.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/lexical_casts.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace dc_ros2_control +{ + +// ============================================================================= +// Unit Conversions (matches servo HWI structure) +// ============================================================================= + +double DCHardwareInterface::calculate_joint_position_from_motor_position(double motor_pos_deg, int gear_ratio) { + // deg -> radians with gear ratio + return (motor_pos_deg * (M_PI / 180.0)) / gear_ratio; +} + +double DCHardwareInterface::calculate_joint_displacement_from_motor_position(double motor_pos_deg, int gear_ratio, double dist_per_rev) { + // deg -> meters: (deg / 360) * distance_per_rev / gear_ratio + return (motor_pos_deg / 360.0) * dist_per_rev / gear_ratio; +} + +double DCHardwareInterface::calculate_joint_angular_velocity_from_motor_velocity(double motor_vel_dps, int gear_ratio) { + // deg/s -> rad/s with gear ratio + return (motor_vel_dps * (M_PI / 180.0)) / gear_ratio; +} + +double DCHardwareInterface::calculate_joint_linear_velocity_from_motor_velocity(double motor_vel_dps, int gear_ratio, double dist_per_rev) { + // deg/s -> m/s + return (motor_vel_dps / 360.0) * dist_per_rev / gear_ratio; +} + +int16_t DCHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio) { + // radians -> deg with gear ratio + return static_cast(std::round((joint_position * (180.0 / M_PI)) * gear_ratio)); +} + +int16_t DCHardwareInterface::calculate_motor_position_from_desired_joint_displacement(double joint_position, int gear_ratio, double dist_per_rev) { + // meters -> deg: (m / distance_per_rev) * 360 * gear_ratio + return static_cast(std::round((joint_position / dist_per_rev) * 360.0 * gear_ratio)); +} + +int16_t DCHardwareInterface::calculate_motor_velocity_from_desired_joint_angular_velocity(double joint_velocity, int gear_ratio) { + // rad/s -> deg/s with gear ratio + return static_cast(std::round((joint_velocity * (180.0 / M_PI)) * gear_ratio)); +} + +int16_t DCHardwareInterface::calculate_motor_velocity_from_desired_joint_linear_velocity(double joint_velocity, int gear_ratio, double dist_per_rev) { + // m/s -> deg/s + return static_cast(std::round((joint_velocity / dist_per_rev) * 360.0 * gear_ratio)); +} + +// ============================================================================= +// Logger +// ============================================================================= + +void DCHardwareInterface::logger_function() +{ + if (num_joints == 0) return; + + std::string log_msg = "\033[2J\033[H \nDC Motor Logger"; + std::ostringstream oss; + std::string control_mode; + std::string joint_type_str; + std::string status; + + oss << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface + << " | Command CAN ID: 0x" << std::hex << std::uppercase << can_command_id + << " | Response CAN ID: 0x" << std::hex << std::uppercase << can_response_id + << " | HWI Update Rate: " << std::dec << update_rate + << " | Logger Update Rate: " << logger_rate << "\n" + << "Elapsed Time since first update: " << elapsed_time << "\n" + << "\n--- Joint Specific ---"; + + for (int i = 0; i < num_joints; i++) { + switch (control_level_[i]) { + case integration_level_t::POSITION: control_mode = "POSITION"; break; + case integration_level_t::VELOCITY: control_mode = "VELOCITY"; break; + default: control_mode = "UNDEFINED"; break; + } + + switch (joint_type_[i]) { + case joint_type_t::REVOLUTE: joint_type_str = "REVOLUTE"; break; + case joint_type_t::PRISMATIC: joint_type_str = "PRISMATIC"; break; + } + + switch (device_status[i]) { + case 0: status = "Undefined"; break; + case 1: status = "Idle"; break; + case 2: status = "Startup Sequence"; break; + case 3: status = "Error (Invalid Request)"; break; + case 4: status = "Error (Motor Disarmed)"; break; + case 5: status = "Error (Motor Failed)"; break; + case 6: status = "Error (Controller Failed)"; break; + case 7: status = "Error (ESTOP Requested)"; break; + case 8: status = "Error (Unknown Position)"; break; + case 9: status = "Position Control"; break; + case 10: status = "Velocity Control"; break; + case 11: status = "Motor Stopped"; break; + default: status = "UNKNOWN (" + std::to_string(device_status[i]) + ")"; break; + } + + oss << "\nJOINT: " << info_.joints[i].name << "\n" + << "Parameters: Node ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] + << " | Gear Ratio: " << std::dec << joint_gear_ratios[i] + << " | Inverted: " << (joint_inverted[i] ? "true" : "false") + << " | Device Status: " << status + << " | Control Mode: " << control_mode + << " | Joint Type: " << joint_type_str << "\n" + << "-- Commands --\n" + << "Joint Command Position: " << joint_command_position_[i] + << " | Joint Command Velocity: " << joint_command_velocity_[i] << "\n" + << "-- State --\n" + << "Joint Position: " << joint_state_position_[i] + << " | Joint Velocity: " << joint_state_velocity_[i] << "\n" + << "-- Telemetry --\n" + << "Temperature: " << motor_temperature_[i] << " C" + << " | Torque Current: " << motor_torque_current_[i] << " A\n"; + } + + log_msg += oss.str(); + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "%s", log_msg.c_str()); +} + +// ============================================================================= +// Initialization +// ============================================================================= + +hardware_interface::CallbackReturn DCHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // -- Per-joint parameters -- + for (auto& joint : info_.joints) { + joint_node_ids.push_back( + std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x0, 0xF)); + + // Gear ratio (default: 1) + if (joint.parameters.count("gear_ratio")) { + joint_gear_ratios.push_back(std::abs(std::stoi(joint.parameters.at("gear_ratio")))); + } else { + joint_gear_ratios.push_back(1); + } + + // Inverted (default: false) + if (joint.parameters.count("inverted")) { + joint_inverted.push_back(joint.parameters.at("inverted") == "true"); + } else { + joint_inverted.push_back(false); + } + + // Joint type (default: revolute) + std::string jtype = "revolute"; + if (joint.parameters.count("joint_type")) { + jtype = joint.parameters.at("joint_type"); + } + + if (jtype == "revolute") { + joint_type_.push_back(joint_type_t::REVOLUTE); + max_disp.push_back(std::nan("")); + distance_per_rev.push_back(0.0); + } else if (jtype == "prismatic" && joint.parameters.count("max_disp") && joint.parameters.count("distance_per_rev")) { + joint_type_.push_back(joint_type_t::PRISMATIC); + max_disp.push_back(std::abs(std::stod(joint.parameters.at("max_disp")))); + distance_per_rev.push_back(std::stod(joint.parameters.at("distance_per_rev"))); + } else { + RCLCPP_ERROR(rclcpp::get_logger("DCHardwareInterface"), + "Invalid joint_type for joint %s. Must be 'revolute' or 'prismatic' (with 'max_disp' and 'distance_per_rev').", + joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } + + // -- Hardware-level parameters -- + num_joints = static_cast(info_.joints.size()); + update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); + logger_rate = std::stoi(info_.hardware_parameters.at("logger_rate")); + logger_state = std::stoi(info_.hardware_parameters.at("logger_state")); + can_interface = info_.hardware_parameters.at("can_interface"); + can_command_id = std::stoi(info_.hardware_parameters.at("can_id"), nullptr, 0); + can_response_id = can_command_id + 0x01; + + elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; + write_count = 0; + + // -- Command and State Interface initialization -- + joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + prev_joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + joint_state_velocity_.assign(num_joints, 0.0); + prev_joint_state_velocity_.assign(num_joints, 0.0); + + joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + prev_joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + joint_command_velocity_.assign(num_joints, 0.0); + prev_joint_command_velocity_.assign(num_joints, 0.0); + + motor_position.assign(num_joints, 0.0); + motor_velocity.assign(num_joints, 0.0); + device_status.assign(num_joints, 0); + + // Telemetry placeholders (TODO: populate via CAN telemetry command) + motor_temperature_.assign(num_joints, 0.0); + motor_torque_current_.assign(num_joints, 0.0); + + // Default control mode: position + control_level_.resize(num_joints, integration_level_t::POSITION); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +// ============================================================================= +// Lifecycle +// ============================================================================= + +hardware_interface::CallbackReturn DCHardwareInterface::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + return this->on_cleanup(previous_state); +} + +std::vector +DCHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + + for (int i = 0; i < num_joints; i++) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_state_position_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_state_velocity_[i])); + + // Telemetry interfaces (TODO: populate via CAN telemetry command) + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, "motor_temperature", &motor_temperature_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, "torque_current", &motor_torque_current_[i])); + } + + return state_interfaces; +} + +std::vector +DCHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + + for (int i = 0; i < num_joints; i++) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_command_position_[i])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_command_velocity_[i])); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn DCHardwareInterface::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Configuring DC Motor HWI..."); + + if (!canBus.open(can_interface, + std::bind(&DCHardwareInterface::on_can_message, this, std::placeholders::_1))) + { + RCLCPP_ERROR(rclcpp::get_logger("DCHardwareInterface"), + "Failed to open CAN interface: %s", can_interface.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "CAN interface opened successfully."); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DCHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Cleaning up... please wait..."); + + // Send shutdown command (maintenance sub-command 3) to all joints + for (int i = 0; i < num_joints; i++) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = MAINTENANCE_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = 3; // DC Motor: Shutdown + canBus.send(can_tx_frame_); + } + + canBus.close(); + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Cleanup complete."); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DCHardwareInterface::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Activating... please wait..."); + + // Initialize command positions to current state + joint_command_position_ = joint_state_position_; + + for (size_t i = 0; i < joint_command_position_.size(); ++i) { + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "Joint %zu command position initialized to: %f", i, joint_command_position_[i]); + } + + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Successfully activated!"); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DCHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Deactivating... please wait..."); + + // Send stop command (maintenance sub-command 2) to all joints + for (int i = 0; i < num_joints; i++) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = MAINTENANCE_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = 2; // DC Motor: Stop + canBus.send(can_tx_frame_); + } + + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Successfully deactivated!"); + return hardware_interface::CallbackReturn::SUCCESS; +} + +// ============================================================================= +// CAN Receive Callback +// ============================================================================= + +void DCHardwareInterface::on_can_message(const CANLib::CanFrame& frame) +{ + can_rx_frame_ = frame; + + int data[7] = {0x00}; + uint8_t cmd_byte = can_rx_frame_.data[0]; + uint8_t command_nibble = cmd_byte & 0xF0; + uint8_t device_id_nibble = cmd_byte & 0x0F; + double raw_motor_position = 0.0; + double raw_motor_velocity = 0.0; + + for (int i = 0; i < num_joints; i++) { + if (can_rx_frame_.id != can_response_id || device_id_nibble != joint_node_ids[i]) + continue; + + if (command_nibble == MOTOR_STATE_CMD || + command_nibble == ABSOLUTE_POS_CONTROL_CMD || + command_nibble == VELOCITY_CONTROL_CMD) + { + // DECODING CAN MESSAGE + data[1] = can_rx_frame_.data[1]; // Position low byte + data[2] = can_rx_frame_.data[2]; // Position high byte + data[3] = can_rx_frame_.data[3]; // Velocity low byte + data[4] = can_rx_frame_.data[4]; // Velocity high byte + + // POSITION: int16 sign extension (NOT int32) + raw_motor_position = static_cast(static_cast((data[2] << 8) | data[1])); + + // VELOCITY: int16 + raw_motor_velocity = static_cast(static_cast((data[4] << 8) | data[3])); + + // Apply inversion + double dir = joint_inverted[i] ? -1.0 : 1.0; + + // CALCULATING JOINT STATE + if (joint_type_[i] == joint_type_t::REVOLUTE) { + motor_position[i] = dir * calculate_joint_position_from_motor_position(raw_motor_position, joint_gear_ratios[i]); + motor_velocity[i] = dir * calculate_joint_angular_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i]); + } + else if (joint_type_[i] == joint_type_t::PRISMATIC) { + motor_position[i] = dir * calculate_joint_displacement_from_motor_position(raw_motor_position, joint_gear_ratios[i], distance_per_rev[i]); + motor_velocity[i] = dir * calculate_joint_linear_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i], distance_per_rev[i]); + } + else { + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); + } + } + else if (command_nibble == MOTOR_STATUS_CMD) + { + device_status[i] = can_rx_frame_.data[1]; + } + // TODO: Handle telemetry response frames here when implemented + } +} + +// ============================================================================= +// Read (update state from CAN data) +// ============================================================================= + +hardware_interface::return_type DCHardwareInterface::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + for (int i = 0; i < num_joints; i++) { + if (prev_joint_state_velocity_[i] != motor_velocity[i]) { + joint_state_velocity_[i] = motor_velocity[i]; + prev_joint_state_velocity_[i] = joint_state_velocity_[i]; + } + + if (prev_joint_state_position_[i] != motor_position[i]) { + joint_state_position_[i] = motor_position[i]; + prev_joint_state_position_[i] = joint_state_position_[i]; + } + + // Check device status — allow Undefined(0), Idle(1), Position Control(9), Velocity Control(10) + int s = device_status[i]; + if (s != 0 && s != 1 && s != 9 && s != 10) { + RCLCPP_ERROR(rclcpp::get_logger("DCHardwareInterface"), + "Joint %s in error state: %d", info_.joints[i].name.c_str(), s); + return hardware_interface::return_type::ERROR; + } + + // TODO: Telemetry — populate motor_temperature_[i] and motor_torque_current_[i] via CAN + } + + return hardware_interface::return_type::OK; +} + +// ============================================================================= +// Write (send CAN commands) +// ============================================================================= + +hardware_interface::return_type DCHardwareInterface::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + elapsed_update_time += period.seconds(); + elapsed_time += period.seconds(); + double update_period = 1.0 / update_rate; + + // Logger update + elapsed_logger_time += period.seconds(); + double logging_period = 1.0 / logger_rate; + if (elapsed_logger_time > logging_period) { + elapsed_logger_time = 0.0; + if (logger_state == 1) { + logger_function(); + } + } + + // Rate-limited CAN writes — all joints per tick + if (elapsed_update_time > update_period) { + elapsed_update_time = 0.0; + + for (int i = 0; i < num_joints; i++) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + + // Apply inversion for outgoing commands + double dir = joint_inverted[i] ? -1.0 : 1.0; + + if (control_level_[i] == integration_level_t::POSITION && + std::isfinite(joint_command_position_[i]) && + joint_command_position_[i] != prev_joint_command_position_[i]) + { + // Clamp prismatic joints to [0, max_disp] + if (joint_type_[i] == joint_type_t::PRISMATIC) { + joint_command_position_[i] = std::clamp( + joint_command_position_[i], 0.0, max_disp[i]); + } + + int16_t motor_pos; + if (joint_type_[i] == joint_type_t::REVOLUTE) { + motor_pos = calculate_motor_position_from_desired_joint_position( + dir * joint_command_position_[i], joint_gear_ratios[i]); + } else { + motor_pos = calculate_motor_position_from_desired_joint_displacement( + dir * joint_command_position_[i], joint_gear_ratios[i], distance_per_rev[i]); + } + + can_tx_frame_.dlc = 3; + can_tx_frame_.data[0] = ABSOLUTE_POS_CONTROL_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = static_cast(motor_pos & 0xFF); + can_tx_frame_.data[2] = static_cast((motor_pos >> 8) & 0xFF); + + prev_joint_command_position_[i] = joint_command_position_[i]; + } + else if (control_level_[i] == integration_level_t::VELOCITY && + std::isfinite(joint_command_velocity_[i]) && + joint_command_velocity_[i] != prev_joint_command_velocity_[i]) + { + int16_t motor_vel; + if (joint_type_[i] == joint_type_t::REVOLUTE) { + motor_vel = calculate_motor_velocity_from_desired_joint_angular_velocity( + dir * joint_command_velocity_[i], joint_gear_ratios[i]); + } else { + motor_vel = calculate_motor_velocity_from_desired_joint_linear_velocity( + dir * joint_command_velocity_[i], joint_gear_ratios[i], distance_per_rev[i]); + } + + can_tx_frame_.dlc = 3; + can_tx_frame_.data[0] = VELOCITY_CONTROL_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = static_cast(motor_vel & 0xFF); + can_tx_frame_.data[2] = static_cast((motor_vel >> 8) & 0xFF); + + prev_joint_command_velocity_[i] = joint_command_velocity_[i]; + } + else + { + // No new command — poll motor state + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = MOTOR_STATE_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = 1; // Validate request + } + + canBus.send(can_tx_frame_); + } + } + + return hardware_interface::return_type::OK; +} + +// ============================================================================= +// Command Mode Switch +// ============================================================================= + +hardware_interface::return_type DCHardwareInterface::perform_command_mode_switch( + const std::vector& start_interfaces, + const std::vector& stop_interfaces) +{ + std::ostringstream ss; + ss << "perform_command_mode_switch called. start: ["; + for (auto &s : start_interfaces) ss << s << ","; + ss << "] stop: ["; + for (auto &s : stop_interfaces) ss << s << ","; + ss << "]"; + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "%s", ss.str().c_str()); + + std::vector requested_modes(num_joints, integration_level_t::UNDEFINED); + + // Process stop interfaces + for (const auto &ifname : stop_interfaces) { + for (int i = 0; i < num_joints; ++i) { + if (ifname.find(info_.joints[i].name) != std::string::npos) { + requested_modes[i] = integration_level_t::UNDEFINED; + } + } + } + + // Process start interfaces + for (const auto &ifname : start_interfaces) { + for (int i = 0; i < num_joints; ++i) { + const std::string pos_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_POSITION); + const std::string vel_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_VELOCITY); + if (ifname == pos_if) { + requested_modes[i] = integration_level_t::POSITION; + } else if (ifname == vel_if) { + requested_modes[i] = integration_level_t::VELOCITY; + } + } + } + + // Apply modes + for (int i = 0; i < num_joints; ++i) { + if (requested_modes[i] == integration_level_t::UNDEFINED) { + bool was_stopped = false; + for (const auto &ifname : stop_interfaces) { + if (ifname.find(info_.joints[i].name) != std::string::npos) { + was_stopped = true; + break; + } + } + if (was_stopped) { + control_level_[i] = integration_level_t::UNDEFINED; + joint_command_velocity_[i] = 0; + joint_command_position_[i] = joint_state_position_[i]; + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "Joint %s: stopped -> UNDEFINED", info_.joints[i].name.c_str()); + } + } else { + control_level_[i] = requested_modes[i]; + if (requested_modes[i] == integration_level_t::VELOCITY) { + joint_command_velocity_[i] = 0; + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "Joint %s: switched to VELOCITY", info_.joints[i].name.c_str()); + } else if (requested_modes[i] == integration_level_t::POSITION) { + joint_command_position_[i] = joint_state_position_[i]; + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "Joint %s: switched to POSITION (initialized to %f)", + info_.joints[i].name.c_str(), joint_command_position_[i]); + } + } + } + + return hardware_interface::return_type::OK; +} + +} // namespace dc_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + dc_ros2_control::DCHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file diff --git a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml index 299f7b03..d47bf6dd 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -31,10 +31,10 @@ joint_group_position_controller: - servo_rack_and_pinion_r - servo_scoop_a - servo_scoop_b - - talon_scoop + - dc_scoop - servo_sampler_lift_l - servo_sampler_lift_l - - talon_auger + - dc_auger - servo_auger_lift interface_name: position @@ -47,10 +47,10 @@ joint_group_velocity_controller: - servo_rack_and_pinion_r - servo_scoop_a - servo_scoop_b - - talon_scoop + - dc_scoop - servo_sampler_lift_l - servo_sampler_lift_r - - talon_auger + - dc_auger - servo_auger_lift interface_name: velocity @@ -66,12 +66,12 @@ science_controller: # Scoops scoop_servos: ["servo_scoop_a", "servo_scoop_b"] - scoop_spinner: "talon_scoop" + scoop_spinner: "dc_scoop" # Sampler sampler_lift_l: "servo_sampler_lift_l" sampler_lift_r: "servo_sampler_lift_r" - auger_spinner: "talon_auger" + auger_spinner: "dc_auger" cap: "servo_auger_lift" interface_name: position command_interfaces: [position, velocity] @@ -98,8 +98,8 @@ motor_status_broadcaster: joints: - stepper_motor_a - stepper_motor_b - - talon_lift - - talon_scoop + - dc_lift + - dc_scoop interfaces: - motor_temperature - torque_current diff --git a/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp b/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp index 75872f50..6af6d49c 100644 --- a/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp +++ b/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp @@ -140,6 +140,7 @@ class ScienceManual : public controller_interface::ControllerInterface std::vector talon_joints_; std::vector servo_joints_; std::vector rack_pinion_joints_; + std::vector dc_joints_; std::vector joints_; //std::string talon_auger_; diff --git a/src/subsystems/science/science_controllers/src/science_controller.cpp b/src/subsystems/science/science_controllers/src/science_controller.cpp index ebbaea9e..c6997dd7 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.cpp +++ b/src/subsystems/science/science_controllers/src/science_controller.cpp @@ -85,8 +85,8 @@ controller_interface::CallbackReturn ScienceManual::on_configure( params_.pump_b }; - talon_joints_.clear(); - talon_joints_ = { + dc_joints_.clear(); + dc_joints_ = { params_.scoop_spinner, params_.auger_spinner }; @@ -246,7 +246,7 @@ controller_interface::CallbackReturn ScienceManual::on_deactivate( command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); } stepper_pump_joints_.clear(); - talon_joints_.clear(); + dc_joints_.clear(); servo_joints_.clear(); rack_pinion_joints_.clear(); state_joints_.clear(); @@ -375,7 +375,7 @@ controller_interface::return_type ScienceManual::update( // Stepper motors (position) command_interfaces_[IDX_PUMP_A_VELOCITY].set_value(stepper_cmd * (M_PI / 180.0)); command_interfaces_[IDX_PUMP_B_VELOCITY].set_value(stepper_cmd * (M_PI / 180.0)); - // Talons (velocity) + // DC motors (velocity) command_interfaces_[IDX_SAMPLER_LIFT_LEFT_VELOCITY].set_value(sampler_lift_pos_l * (M_PI / 180.0)); command_interfaces_[IDX_SAMPLER_LIFT_RIGHT_VELOCITY].set_value(sampler_lift_pos_r * (M_PI / 180.0)); command_interfaces_[IDX_SCOOP_SPINNER_VELOCITY].set_value(scoop_spinner_cmd); diff --git a/src/subsystems/science/science_controllers/src/science_controller.yaml b/src/subsystems/science/science_controllers/src/science_controller.yaml index 7371edf2..eb1938cf 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.yaml +++ b/src/subsystems/science/science_controllers/src/science_controller.yaml @@ -74,8 +74,8 @@ science_manual: scoop_spinner: type: string - default_value: "talon_scoop" - description: "Scoop Talon motor controller" + default_value: "dc_scoop" + description: "Scoop DC motor" read_only: true validation: not_empty<>: null @@ -94,7 +94,7 @@ science_manual: auger_spinner: type: string - default_value: "talon_auger" + default_value: "dc_auger" description: "(Optional)" read_only: false From 33b1e4484bba810c1801aca90aab0d04a4bf8851 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Fri, 1 May 2026 16:04:56 -0400 Subject: [PATCH 02/16] First iteration of HWIs: DC, CCD, Diode, Fixing Laser, CCD Custom Controller, Laser & Diode GPIO Controllers --- .../science/science.ccd.ros2_control.xacro | 31 ++ .../science/science.laser.ros2_control.xacro | 4 +- .../science.mock_talon.ros2_control.xacro | 17 + .../science.sensor_diode.ros2_control.xacro | 27 ++ .../urdf/athena_science.urdf.xacro | 11 +- .../ccd_ros2_control/CMakeLists.txt | 82 ++++ .../ccd_hardware_interface.xml | 9 + .../ccd_hardware_interface.hpp | 134 ++++++ .../ccd_ros2_control/package.xml | 34 ++ .../src/ccd_hardware_interface.cpp | 376 +++++++++++++++++ .../laser_hardware_interface.hpp | 17 +- .../src/laser_hardware_interface.cpp | 127 +++--- .../sensor_diode_ros2_control/CMakeLists.txt | 77 ++++ .../sensor_diode_hardware_interface.hpp | 111 +++++ .../sensor_diode_ros2_control/package.xml | 27 ++ .../sensor_diode_hardware_interface.xml | 9 + .../src/sensor_diode_hardware_interface.cpp | 285 +++++++++++++ .../msg/raman_preprocessing/CMakeLists.txt | 26 ++ src/msgs/msg/raman_preprocessing/package.xml | 18 + src/msgs/{package.xml => package.xml.bak} | 0 src/msgs/raman_msgs/CMakeLists.txt | 20 + src/msgs/raman_msgs/msg/RamanPeaks.msg | 6 + src/msgs/raman_msgs/msg/RamanSpectrum.msg | 9 + src/msgs/raman_msgs/package.xml | 22 + src/msgs/raman_msgs/raman_driver | 1 + .../raman_driver/package.xml | 19 + .../raman_driver/raman_driver/__init__.py | 0 .../raman_driver/raman_driver_node.py | 102 +++++ .../raman_driver/resource/raman_driver | 0 .../raman_spectrometer/raman_driver/setup.cfg | 4 + .../raman_spectrometer/raman_driver/setup.py | 30 ++ .../raman_driver/test/test_copyright.py | 25 ++ .../raman_driver/test/test_flake8.py | 25 ++ .../raman_driver/test/test_pep257.py | 23 + .../config/athena_science_controllers.yaml | 44 +- .../launch/athena_science.launch.py | 11 +- .../science/science_bringup/package.xml | 1 + .../science_controllers/CMakeLists.txt | 78 ++-- .../config/ccd_controller.yaml | 34 ++ .../{src => config}/science_controller.yaml | 0 .../science_controllers/ccd_controller.hpp | 170 ++++++++ .../science/science_controllers/package.xml | 2 + .../science_controllers.xml | 10 + .../src/ccd_controller.cpp | 399 ++++++++++++++++++ 44 files changed, 2350 insertions(+), 107 deletions(-) create mode 100644 src/description/ros2_control/science/science.ccd.ros2_control.xacro create mode 100644 src/description/ros2_control/science/science.mock_talon.ros2_control.xacro create mode 100644 src/description/ros2_control/science/science.sensor_diode.ros2_control.xacro create mode 100644 src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt create mode 100644 src/hardware_interfaces/ccd_ros2_control/ccd_hardware_interface.xml create mode 100644 src/hardware_interfaces/ccd_ros2_control/include/ccd_ros2_control/ccd_hardware_interface.hpp create mode 100644 src/hardware_interfaces/ccd_ros2_control/package.xml create mode 100644 src/hardware_interfaces/ccd_ros2_control/src/ccd_hardware_interface.cpp create mode 100644 src/hardware_interfaces/sensor_diode_ros2_control/CMakeLists.txt create mode 100644 src/hardware_interfaces/sensor_diode_ros2_control/include/sensor_diode_ros2_control/sensor_diode_hardware_interface.hpp create mode 100644 src/hardware_interfaces/sensor_diode_ros2_control/package.xml create mode 100644 src/hardware_interfaces/sensor_diode_ros2_control/sensor_diode_hardware_interface.xml create mode 100644 src/hardware_interfaces/sensor_diode_ros2_control/src/sensor_diode_hardware_interface.cpp create mode 100644 src/msgs/msg/raman_preprocessing/CMakeLists.txt create mode 100644 src/msgs/msg/raman_preprocessing/package.xml rename src/msgs/{package.xml => package.xml.bak} (100%) create mode 100644 src/msgs/raman_msgs/CMakeLists.txt create mode 100644 src/msgs/raman_msgs/msg/RamanPeaks.msg create mode 100644 src/msgs/raman_msgs/msg/RamanSpectrum.msg create mode 100644 src/msgs/raman_msgs/package.xml create mode 120000 src/msgs/raman_msgs/raman_driver create mode 100644 src/subsystems/science/raman_spectrometer/raman_driver/package.xml create mode 100644 src/subsystems/science/raman_spectrometer/raman_driver/raman_driver/__init__.py create mode 100644 src/subsystems/science/raman_spectrometer/raman_driver/raman_driver/raman_driver_node.py create mode 100644 src/subsystems/science/raman_spectrometer/raman_driver/resource/raman_driver create mode 100644 src/subsystems/science/raman_spectrometer/raman_driver/setup.cfg create mode 100644 src/subsystems/science/raman_spectrometer/raman_driver/setup.py create mode 100644 src/subsystems/science/raman_spectrometer/raman_driver/test/test_copyright.py create mode 100644 src/subsystems/science/raman_spectrometer/raman_driver/test/test_flake8.py create mode 100644 src/subsystems/science/raman_spectrometer/raman_driver/test/test_pep257.py create mode 100644 src/subsystems/science/science_controllers/config/ccd_controller.yaml rename src/subsystems/science/science_controllers/{src => config}/science_controller.yaml (100%) create mode 100644 src/subsystems/science/science_controllers/include/science_controllers/ccd_controller.hpp create mode 100644 src/subsystems/science/science_controllers/src/ccd_controller.cpp 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..5b2fe10e --- /dev/null +++ b/src/description/ros2_control/science/science.ccd.ros2_control.xacro @@ -0,0 +1,31 @@ + + + + + + + + + ccd_ros2_control/CCDHardwareInterface + ${can_interface} + 0x100 + + + + + + + + + + + + + + + + + + + + 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 ee7f97c7..d1fe479b 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 @@ - + @@ -9,11 +9,11 @@ laser_ros2_control/LaserHardwareInterface ${can_interface} 0x130 + ${port_id} - diff --git a/src/description/ros2_control/science/science.mock_talon.ros2_control.xacro b/src/description/ros2_control/science/science.mock_talon.ros2_control.xacro new file mode 100644 index 00000000..5df33c15 --- /dev/null +++ b/src/description/ros2_control/science/science.mock_talon.ros2_control.xacro @@ -0,0 +1,17 @@ + + + + + + mock_components/GenericSystem + + + + + + + + + + + \ No newline at end of file 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..57e70dc5 --- /dev/null +++ b/src/description/ros2_control/science/science.sensor_diode.ros2_control.xacro @@ -0,0 +1,27 @@ + + + + + + + + + sensor_diode_ros2_control/SensorDiodeHardwareInterface + ${can_interface} + 0x110 + + + + + + + + + + + + + + + + diff --git a/src/description/urdf/athena_science.urdf.xacro b/src/description/urdf/athena_science.urdf.xacro index 95e14105..890ceca1 100644 --- a/src/description/urdf/athena_science.urdf.xacro +++ b/src/description/urdf/athena_science.urdf.xacro @@ -11,6 +11,8 @@ + + @@ -35,10 +37,17 @@ - + + + + + + + + \ No newline at end of file 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..d7a0ce8d --- /dev/null +++ b/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt @@ -0,0 +1,82 @@ +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) +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..123d61f2 --- /dev/null +++ b/src/hardware_interfaces/ccd_ros2_control/include/ccd_ros2_control/ccd_hardware_interface.hpp @@ -0,0 +1,134 @@ +// Copyright (c) 2024 UMD Loop +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#ifndef CCD_ROS2_CONTROL__CCD_HARDWARE_INTERFACE_HPP_ +#define CCD_ROS2_CONTROL__CCD_HARDWARE_INTERFACE_HPP_ + +#include +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "umdloop_can_library/SocketCanBus.hpp" +#include "umdloop_can_library/CanFrame.hpp" + +namespace ccd_ros2_control +{ + +/** + * @brief Hardware interface for CAN-controlled spectrometry ccd via ros2_control + * + * This interface controls a spectrometry ccd through CAN bus. + * + * CAN Protocol (ID: 0x100): + * - ON: DATA[0] = 0x60 + * - OFF: DATA[0] = 0x80 + * - Read Temperature: DATA[0] = 0x85 + * - Read Wavelength: DATA[0] = 0x90 + * + * State Interfaces (read by controllers): + * - ccd_state: 0.0 = OFF, 1.0 = ON + * - temperature: Ccd temperature (if available) + * - is_connected: Is CAN connected (0.0 or 1.0) + * + * Command Interfaces (written by controllers): + * - ccd_command: 0.0 = turn OFF, 1.0 = turn ON + * + * Hardware Parameters (from URDF): + * - can_interface: CAN interface name (default: "can0") + * - can_id: CAN ID for ccd commands (default: 0x100) + */ +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; + +private: + // CAN message handler + void onCanMessage(const CANLib::CanFrame& frame); + + // Configuration parameters + std::string can_interface_; + uint32_t can_id_; + + // CAN bus + CANLib::SocketCanBus canBus_; + CANLib::CanFrame can_tx_frame_; + bool can_connected_; + + // State variables (hardware -> ros2_control) + double is_connected_; + double command_success_; + double acquisition_in_progress_; + double data_ready_; + double frames_received_; + double last_frame_id_; + + // Command variables (ros2_control -> hardware) + double capture_binary_cmd_; + double capture_byte_cmd_; + + bool waiting_for_binary_data_; + bool waiting_for_byte_data_; + std::vector binary_pixels_; + std::vector byte_pixels_; + + // CAN command bytes + static constexpr uint8_t CMD_REQUEST_BINARY = 0x20; // Request Binary (1-bit) Measurements + static constexpr uint8_t CMD_REQUEST_BYTE = 0x30; // Request 8-bit Measurements +}; + +} // 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..dc766c5a --- /dev/null +++ b/src/hardware_interfaces/ccd_ros2_control/package.xml @@ -0,0 +1,34 @@ + + + + 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 + ament_cmake + rosidl_default_runtime + rosidl_interface_packages + + + umdloop_can_library + umdloop_can_library + + ament_lint_auto + ament_lint_common + + + ament_cmake + + 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..06dd64a5 --- /dev/null +++ b/src/hardware_interfaces/ccd_ros2_control/src/ccd_hardware_interface.cpp @@ -0,0 +1,376 @@ +// Copyright (c) 2026 UMD Loop +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// + +#include "ccd_ros2_control/ccd_hardware_interface.hpp" + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include + +namespace ccd_ros2_control +{ + +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; + } + + // Initialize state variables + is_connected_ = 0.0; + command_success_ = 0.0; + acquisition_in_progress_ = 0.0; + data_ready_ = 0.0; + frames_received_ = 0.0; + last_frame_id_ = 0.0; + + // Initialize command variables + capture_binary_cmd_ = 0.0; + capture_byte_cmd_ = 0.0; + + // Initialize internal acquisition state + waiting_for_binary_data_ = false; + waiting_for_byte_data_ = false; + binary_pixels_.assign(3648, 0); + byte_pixels_.assign(3648, 0); + + // Parse hardware parameters + if (info_.hardware_parameters.count("can_interface")) { + can_interface_ = info_.hardware_parameters.at("can_interface"); + } else { + can_interface_ = "can0"; + } + + if (info_.hardware_parameters.count("can_id")) { + can_id_ = static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); + } else { + can_id_ = 0x100; + } + + can_connected_ = false; + + RCLCPP_INFO( + rclcpp::get_logger("CCDHardwareInterface"), + "Initialized CCD on CAN interface %s with ID 0x%X", + can_interface_.c_str(), can_id_); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn CCDHardwareInterface::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("CCDHardwareInterface"), + "Configuring CCD hardware..."); + + if (can_connected_) { + canBus_.close(); + can_connected_ = false; + } + + if (!canBus_.open(can_interface_, + std::bind(&CCDHardwareInterface::onCanMessage, this, std::placeholders::_1))) + { + RCLCPP_WARN( + rclcpp::get_logger("CCDHardwareInterface"), + "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("CCDHardwareInterface"), + "Successfully opened CAN interface %s", can_interface_.c_str()); + } + + is_connected_ = can_connected_ ? 1.0 : 0.0; + command_success_ = 0.0; + acquisition_in_progress_ = 0.0; + data_ready_ = 0.0; + frames_received_ = 0.0; + last_frame_id_ = 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("CCDHardwareInterface"), + "CCD hardware configured (%s)", can_connected_ ? "CAN MODE" : "SIMULATION"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +void CCDHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) +{ + if (frame.data[0] == CMD_REQUEST_BINARY && frame.dlc >= 2) { + command_success_ = static_cast(frame.data[1] ? 1.0 : 0.0); + + if (frame.data[1]) { + waiting_for_binary_data_ = true; + waiting_for_byte_data_ = false; + acquisition_in_progress_ = 1.0; + data_ready_ = 0.0; + frames_received_ = 0.0; + last_frame_id_ = 0.0; + std::fill(binary_pixels_.begin(), binary_pixels_.end(), 0); + } else { + acquisition_in_progress_ = 0.0; + } + return; + } + + if (frame.data[0] == CMD_REQUEST_BYTE && frame.dlc >= 2) { + command_success_ = static_cast(frame.data[1] ? 1.0 : 0.0); + + if (frame.data[1]) { + waiting_for_byte_data_ = true; + waiting_for_binary_data_ = false; + acquisition_in_progress_ = 1.0; + data_ready_ = 0.0; + frames_received_ = 0.0; + last_frame_id_ = 0.0; + std::fill(byte_pixels_.begin(), byte_pixels_.end(), 0); + } else { + acquisition_in_progress_ = 0.0; + } + return; + } + + if (waiting_for_binary_data_ && frame.dlc == 8) { + const uint16_t frame_id = static_cast(frame.data[0]); + last_frame_id_ = static_cast(frame_id); + 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 < binary_pixels_.size()) { + binary_pixels_[pixel_idx] = static_cast((packed >> bit) & 0x01); + } + } + } + + // 3648 / 56 = 65 remainder 8, so 66 data frames total + if (frames_received_ >= 66.0) { + waiting_for_binary_data_ = false; + acquisition_in_progress_ = 0.0; + data_ready_ = 1.0; + RCLCPP_INFO( + rclcpp::get_logger("CCDHardwareInterface"), + "Binary CCD acquisition complete"); + return; + } + } + + if (waiting_for_byte_data_ && frame.dlc == 8) { + const uint16_t frame_id = + static_cast(frame.data[0]) | + (static_cast(frame.data[1]) << 8); + + last_frame_id_ = static_cast(frame_id); + 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 < byte_pixels_.size()) { + byte_pixels_[pixel_idx] = frame.data[2 + i]; + } + } + + // 3648 / 6 = 608 data frames total + if (frames_received_ >= 608.0) { + waiting_for_byte_data_ = false; + acquisition_in_progress_ = 0.0; + data_ready_ = 1.0; + RCLCPP_INFO( + rclcpp::get_logger("CCDHardwareInterface"), + "8-bit CCD acquisition complete"); + } + } +} + +std::vector +CCDHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + + const std::string& name = info_.gpios[0].name; + + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "is_connected", &is_connected_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "command_success", &command_success_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "acquisition_in_progress", &acquisition_in_progress_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "data_ready", &data_ready_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "frames_received", &frames_received_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "last_frame_id", &last_frame_id_)); + + RCLCPP_INFO( + rclcpp::get_logger("CCDHardwareInterface"), + "Exported %zu state interfaces", state_interfaces.size()); + + return state_interfaces; +} + +std::vector +CCDHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + + const std::string& name = info_.gpios[0].name; + + command_interfaces.emplace_back( + hardware_interface::CommandInterface(name, "capture_binary", &capture_binary_cmd_)); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(name, "capture_byte", &capture_byte_cmd_)); + + RCLCPP_INFO( + rclcpp::get_logger("CCDHardwareInterface"), + "Exported %zu command interfaces", command_interfaces.size()); + + return command_interfaces; +} + +hardware_interface::CallbackReturn CCDHardwareInterface::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("CCDHardwareInterface"), + "Activating CCD hardware..."); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn CCDHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("CCDHardwareInterface"), + "Deactivating CCD hardware..."); + + acquisition_in_progress_ = 0.0; + waiting_for_binary_data_ = false; + waiting_for_byte_data_ = false; + + if (can_connected_) { + canBus_.close(); + can_connected_ = false; + is_connected_ = 0.0; + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn CCDHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("CCDHardwareInterface"), + "Cleaning up CCD hardware..."); + + if (can_connected_) { + canBus_.close(); + } + + can_connected_ = false; + is_connected_ = 0.0; + acquisition_in_progress_ = 0.0; + waiting_for_binary_data_ = false; + waiting_for_byte_data_ = false; + + RCLCPP_INFO( + rclcpp::get_logger("CCDHardwareInterface"), + "CCD hardware cleanup complete"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn CCDHardwareInterface::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + RCLCPP_INFO( + rclcpp::get_logger("CCDHardwareInterface"), + "Shutting down CCD hardware..."); + + 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 &) +{ + + if (capture_binary_cmd_ > 0.5) { + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_id_; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = CMD_REQUEST_BINARY; + can_tx_frame_.data[1] = 1; + canBus_.send(can_tx_frame_); + } + + data_ready_ = 0.0; + command_success_ = 0.0; + capture_binary_cmd_ = 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("CCDHardwareInterface"), + "Requested binary CCD acquisition%s", + can_connected_ ? "" : " (simulated)"); + } + + if (capture_byte_cmd_ > 0.5) { + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_id_; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = CMD_REQUEST_BYTE; + can_tx_frame_.data[1] = 1; + canBus_.send(can_tx_frame_); + } + + data_ready_ = 0.0; + command_success_ = 0.0; + capture_byte_cmd_ = 0.0; + + + RCLCPP_INFO( + rclcpp::get_logger("CCDHardwareInterface"), + "Requested byte CCD acquisition%s", + can_connected_ ? "" : " (simulated)"); + } + + return hardware_interface::return_type::OK; +} + +} // namespace ccd_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +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 65206ab3..e59ac544 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 @@ -42,10 +42,9 @@ namespace laser_ros2_control * This interface controls a spectrometry laser through CAN bus. * * CAN Protocol (ID: 0x130): - * - ON: DATA[0] = 0x60 - * - OFF: DATA[0] = 0x80 - * - Read Temperature: DATA[0] = 0x85 - * - Read Wavelength: DATA[0] = 0x90 + * - ON: DATA[0] = 0x20 + port_id, DATA[1] = 0x01 + * - OFF: DATA[0] = 0x20 + port_id, DATA[1] = 0x00 + * - Status: DATA[0] = 0x30 + port_id, DATA[1] = 0x01 * * State Interfaces (read by controllers): * - laser_state: 0.0 = OFF, 1.0 = ON @@ -109,17 +108,17 @@ class LaserHardwareInterface : public hardware_interface::SystemInterface // State variables (hardware → ros2_control) double laser_state_; // Current state: 0.0 = OFF, 1.0 = ON - double temperature_; // Laser temperature double is_connected_; // CAN connected status // Command variables (ros2_control → hardware) double laser_command_; // Commanded state // CAN command bytes - static constexpr uint8_t CMD_LASER_ON = 0x60; - static constexpr uint8_t CMD_LASER_OFF = 0x80; - static constexpr uint8_t CMD_READ_TEMP = 0x85; - static constexpr uint8_t CMD_READ_WAVELENGTH = 0x90; + static constexpr uint8_t CMD_LASER_CONTROL = 0x20; // + port_id + static constexpr uint8_t CMD_LASER_STATUS = 0x30; // + port_id + + // Port ID for this laser instance + uint8_t port_id_; }; } // 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 1e74bdea..901dc415 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 @@ -24,19 +24,16 @@ namespace laser_ros2_control hardware_interface::CallbackReturn LaserHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) { - if (hardware_interface::SystemInterface::on_init(info) != + if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } - // Initialize state variables - laser_state_ = 0.0; // OFF - temperature_ = 0.0; + laser_state_ = 0.0; is_connected_ = 0.0; - - // Initialize command variables - laser_command_ = 0.0; // OFF + laser_command_ = 0.0; + can_connected_ = false; // Parse hardware parameters if (info_.hardware_parameters.count("can_interface")) { @@ -46,18 +43,23 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_init( } if (info_.hardware_parameters.count("can_id")) { - // Parse hex string (e.g., "0x130") - base 0 auto-detects hex/decimal - can_id_ = static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); + can_id_ = static_cast( + std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); } else { - can_id_ = 0x130; // Default laser CAN ID + can_id_ = 0x130; } - can_connected_ = false; + if (info_.hardware_parameters.count("port_id")) { + port_id_ = static_cast( + std::stoul(info_.hardware_parameters.at("port_id"), nullptr, 0)); + } else { + port_id_ = 0; + } RCLCPP_INFO( rclcpp::get_logger("LaserHardwareInterface"), - "Initialized laser on CAN interface %s with ID 0x%X", - can_interface_.c_str(), can_id_); + "Initialized laser on CAN interface %s with ID 0x%X and port ID %u", + can_interface_.c_str(), can_id_, port_id_); return hardware_interface::CallbackReturn::SUCCESS; } @@ -69,13 +71,18 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_configure( rclcpp::get_logger("LaserHardwareInterface"), "Configuring laser hardware..."); - // Open CAN bus - if (!canBus_.open(can_interface_, - std::bind(&LaserHardwareInterface::onCanMessage, this, std::placeholders::_1))) + if (can_connected_) { + canBus_.close(); + can_connected_ = false; + } + + if (!canBus_.open( + can_interface_, + std::bind(&LaserHardwareInterface::onCanMessage, this, std::placeholders::_1))) { RCLCPP_WARN( rclcpp::get_logger("LaserHardwareInterface"), - "Failed to open CAN interface %s - running in SIMULATION mode", + "Failed to open CAN interface %s - running in SIMULATION mode", can_interface_.c_str()); can_connected_ = false; } else { @@ -94,37 +101,30 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_configure( return hardware_interface::CallbackReturn::SUCCESS; } -void LaserHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) +void LaserHardwareInterface::onCanMessage(const CANLib::CanFrame & frame) { - // Handle incoming CAN messages (e.g., temperature readings) - if (frame.id == can_id_) { - // Parse response based on command byte - if (frame.dlc > 0 && frame.data[0] == CMD_READ_TEMP) { - // Temperature response - parse if available - if (frame.dlc >= 2) { - temperature_ = static_cast(frame.data[1]); - } - } + if (frame.id != can_id_ || frame.dlc < 2) { + return; + } + + if (frame.data[0] == static_cast(CMD_LASER_CONTROL + port_id_)) { + laser_state_ = frame.data[1] ? 1.0 : 0.0; + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Laser state confirmed from CAN: %s", laser_state_ > 0.5 ? "ON" : "OFF"); } } -std::vector +std::vector LaserHardwareInterface::export_state_interfaces() { std::vector state_interfaces; - // Use the gpio name from URDF - const std::string& name = info_.gpios[0].name; + const std::string & name = info_.gpios[0].name; - // Laser state (ON/OFF) state_interfaces.emplace_back( hardware_interface::StateInterface(name, "laser_state", &laser_state_)); - // Temperature - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "temperature", &temperature_)); - - // Connection status state_interfaces.emplace_back( hardware_interface::StateInterface(name, "is_connected", &is_connected_)); @@ -135,15 +135,13 @@ LaserHardwareInterface::export_state_interfaces() return state_interfaces; } -std::vector +std::vector LaserHardwareInterface::export_command_interfaces() { std::vector command_interfaces; - // Use the gpio name from URDF - const std::string& name = info_.gpios[0].name; + const std::string & name = info_.gpios[0].name; - // Laser command (ON/OFF) command_interfaces.emplace_back( hardware_interface::CommandInterface(name, "laser_command", &laser_command_)); @@ -175,16 +173,18 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_deactivate( if (can_connected_) { can_tx_frame_ = CANLib::CanFrame(); can_tx_frame_.id = can_id_; - can_tx_frame_.dlc = 1; - can_tx_frame_.data[0] = CMD_LASER_OFF; + 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_); - - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Laser turned OFF (safety)"); + + canBus_.close(); + can_connected_ = false; + is_connected_ = 0.0; } laser_state_ = 0.0; + laser_command_ = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } @@ -196,19 +196,21 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_cleanup( rclcpp::get_logger("LaserHardwareInterface"), "Cleaning up laser hardware..."); - // Ensure laser is OFF before cleanup if (can_connected_) { + // Ensure laser is OFF before cleanup can_tx_frame_ = CANLib::CanFrame(); can_tx_frame_.id = can_id_; - can_tx_frame_.dlc = 1; - can_tx_frame_.data[0] = CMD_LASER_OFF; + 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; is_connected_ = 0.0; + laser_state_ = 0.0; + laser_command_ = 0.0; RCLCPP_INFO( rclcpp::get_logger("LaserHardwareInterface"), @@ -240,7 +242,6 @@ hardware_interface::return_type LaserHardwareInterface::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - // Check if laser command changed bool commanded_on = (laser_command_ > 0.5); bool currently_on = (laser_state_ > 0.5); @@ -248,17 +249,21 @@ hardware_interface::return_type LaserHardwareInterface::write( if (can_connected_) { can_tx_frame_ = CANLib::CanFrame(); can_tx_frame_.id = can_id_; - can_tx_frame_.dlc = 1; - can_tx_frame_.data[0] = commanded_on ? CMD_LASER_ON : CMD_LASER_OFF; + 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_); - } - laser_state_ = commanded_on ? 1.0 : 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Laser turned %s%s", commanded_on ? "ON" : "OFF", - can_connected_ ? "" : " (simulated)"); + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Laser command sent: %s", commanded_on ? "ON" : "OFF"); + } else { + // Simulation mode — update state immediately, no hardware to confirm + laser_state_ = commanded_on ? 1.0 : 0.0; + RCLCPP_INFO( + rclcpp::get_logger("LaserHardwareInterface"), + "Laser turned %s (simulated)", commanded_on ? "ON" : "OFF"); + } } return hardware_interface::return_type::OK; @@ -270,4 +275,4 @@ hardware_interface::return_type LaserHardwareInterface::write( PLUGINLIB_EXPORT_CLASS( laser_ros2_control::LaserHardwareInterface, - hardware_interface::SystemInterface) + 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..9f7543f4 --- /dev/null +++ b/src/hardware_interfaces/sensor_diode_ros2_control/include/sensor_diode_ros2_control/sensor_diode_hardware_interface.hpp @@ -0,0 +1,111 @@ +// Copyright (c) 2024 UMD Loop +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#ifndef 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/SocketCanBus.hpp" +#include "umdloop_can_library/CanFrame.hpp" + +namespace sensor_diode_ros2_control +{ + +/** + * @brief Hardware interface for CAN-controlled spectrometry sensor diode via ros2_control + * + * This interface controls a spectrometry sensor_diode through CAN bus. + * +**/ + +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; + +private: + // CAN message handler + void onCanMessage(const CANLib::CanFrame& frame); + + // Configuration parameters + std::string can_interface_; + uint32_t can_id_; + uint8_t port_id_; + + // CAN bus + CANLib::SocketCanBus canBus_; + CANLib::CanFrame can_tx_frame_; + bool can_connected_; + + // State variables (hardware -> ros2_control) + double is_connected_; + double command_success_; + double wavelength_intensity_; + + // Command variables (ros2_control -> hardware) + double request_measurement_cmd_; + + bool awaiting_response_; + + // CAN command bytes + static constexpr uint8_t CMD_READ_DIODE_VALUE = 0x20; +}; + +} // 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..2958f711 --- /dev/null +++ b/src/hardware_interfaces/sensor_diode_ros2_control/src/sensor_diode_hardware_interface.cpp @@ -0,0 +1,285 @@ +// Copyright (c) 2024 UMD Loop +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// + +#include "sensor_diode_ros2_control/sensor_diode_hardware_interface.hpp" + +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace sensor_diode_ros2_control +{ + +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; + } + + // Initialize states + wavelength_intensity_ = 0.0; + command_success_ = 0.0; + is_connected_ = 0.0; + + // Initialize commands + request_measurement_cmd_ = 0.0; + + // Internal + awaiting_response_ = false; + can_connected_ = false; + + // Parse parameters + if (info_.hardware_parameters.count("can_interface")) { + can_interface_ = info_.hardware_parameters.at("can_interface"); + } else { + can_interface_ = "can0"; + } + + if (info_.hardware_parameters.count("can_id")) { + can_id_ = static_cast( + std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); + } else { + can_id_ = 0x110; + } + + if (info_.hardware_parameters.count("port_id")) { + port_id_ = static_cast( + std::stoul(info_.hardware_parameters.at("port_id"), nullptr, 0)); + } else { + port_id_ = 0; + } + + RCLCPP_INFO( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Initialized sensor diode on CAN interface %s with CAN ID 0x%X and port ID %u", + can_interface_.c_str(), can_id_, port_id_); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_configure( + const rclcpp_lifecycle::State & /*previous_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()); + } + + is_connected_ = can_connected_ ? 1.0 : 0.0; + command_success_ = 0.0; + awaiting_response_ = false; + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector +SensorDiodeHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + + const std::string & name = info_.gpios[0].name; + + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "wavelength_intensity", &wavelength_intensity_)); + + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "command_success", &command_success_)); + + state_interfaces.emplace_back( + hardware_interface::StateInterface(name, "is_connected", &is_connected_)); + + RCLCPP_INFO( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Exported %zu state interfaces", + state_interfaces.size()); + + return state_interfaces; +} + +std::vector +SensorDiodeHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + + const std::string & name = info_.gpios[0].name; + + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + name, "request_measurement", &request_measurement_cmd_)); + + RCLCPP_INFO( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Exported %zu command interfaces", + command_interfaces.size()); + + return command_interfaces; +} + +hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Activating sensor diode hardware..."); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Deactivating sensor diode hardware..."); + + awaiting_response_ = false; + request_measurement_cmd_ = 0.0; + + if (can_connected_) { + canBus_.close(); + can_connected_ = false; + is_connected_ = 0.0; + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Cleaning up sensor diode hardware..."); + + if (can_connected_) { + canBus_.close(); + } + + can_connected_ = false; + is_connected_ = 0.0; + awaiting_response_ = false; + wavelength_intensity_ = 0.0; + command_success_ = 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Sensor diode hardware cleanup complete"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + RCLCPP_INFO( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Shutting down sensor diode hardware..."); + + return on_cleanup(previous_state); +} + +void SensorDiodeHardwareInterface::onCanMessage(const CANLib::CanFrame & frame) +{ + if (frame.id != can_id_ || frame.dlc < 2) { + return; + } + + const uint8_t expected_cmd = static_cast(CMD_READ_DIODE_VALUE + port_id_); + + if (frame.data[0] != expected_cmd) { + return; + } + + // Response format: + // data[0] = 0x20 + port_id + // data[1] = wavelength_intensity [0 to 255] + wavelength_intensity_ = static_cast(frame.data[1]); + command_success_ = 1.0; + awaiting_response_ = false; + + RCLCPP_DEBUG( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Wavelength Intensity: %.0f", + wavelength_intensity_); +} + +hardware_interface::return_type SensorDiodeHardwareInterface::read( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +{ + // CAN messages are processed asynchronously in the callback. + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type SensorDiodeHardwareInterface::write( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +{ + if (request_measurement_cmd_ > 0.5) { + command_success_ = 0.0; + awaiting_response_ = true; + + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = 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] = 1; // validate request + + canBus_.send(can_tx_frame_); + + RCLCPP_INFO( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Requested diode measurement on port %u", + port_id_); + } else { + RCLCPP_INFO( + rclcpp::get_logger("SensorDiodeHardwareInterface"), + "Requested diode measurement on port %u (simulated)", + port_id_); + } + + // Reset trigger after sending + request_measurement_cmd_ = 0.0; + } + + 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/msg/raman_preprocessing/CMakeLists.txt b/src/msgs/msg/raman_preprocessing/CMakeLists.txt new file mode 100644 index 00000000..e953e7f3 --- /dev/null +++ b/src/msgs/msg/raman_preprocessing/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(raman_preprocessing) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/msgs/msg/raman_preprocessing/package.xml b/src/msgs/msg/raman_preprocessing/package.xml new file mode 100644 index 00000000..a572825d --- /dev/null +++ b/src/msgs/msg/raman_preprocessing/package.xml @@ -0,0 +1,18 @@ + + + + raman_preprocessing + 0.0.0 + TODO: Package description + henry + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/msgs/package.xml b/src/msgs/package.xml.bak similarity index 100% rename from src/msgs/package.xml rename to src/msgs/package.xml.bak diff --git a/src/msgs/raman_msgs/CMakeLists.txt b/src/msgs/raman_msgs/CMakeLists.txt new file mode 100644 index 00000000..85eaecdf --- /dev/null +++ b/src/msgs/raman_msgs/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.8) +project(raman_msgs) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/RamanSpectrum.msg" + "msg/RamanPeaks.msg" + DEPENDENCIES std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() \ No newline at end of file diff --git a/src/msgs/raman_msgs/msg/RamanPeaks.msg b/src/msgs/raman_msgs/msg/RamanPeaks.msg new file mode 100644 index 00000000..420b2da0 --- /dev/null +++ b/src/msgs/raman_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/raman_msgs/msg/RamanSpectrum.msg b/src/msgs/raman_msgs/msg/RamanSpectrum.msg new file mode 100644 index 00000000..70995382 --- /dev/null +++ b/src/msgs/raman_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/msgs/raman_msgs/package.xml b/src/msgs/raman_msgs/package.xml new file mode 100644 index 00000000..89a172e5 --- /dev/null +++ b/src/msgs/raman_msgs/package.xml @@ -0,0 +1,22 @@ + + + + raman_msgs + 0.1.0 + Custom message definitions for Raman spectrometer data + UMD Loop + Apache-2.0 + + ament_cmake + rosidl_default_generators + + std_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file diff --git a/src/msgs/raman_msgs/raman_driver b/src/msgs/raman_msgs/raman_driver new file mode 120000 index 00000000..06030c86 --- /dev/null +++ b/src/msgs/raman_msgs/raman_driver @@ -0,0 +1 @@ +/home/henry/ros2_ws/src/athena-code-fresh/src/msgs/msg/raman_driver \ 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/config/athena_science_controllers.yaml b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml index d47bf6dd..86d27e37 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -15,10 +15,17 @@ controller_manager: 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 @@ -77,6 +84,26 @@ science_controller: 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: "~/snapshot" + + publish_rate: 10.0 + acquisition_timeout_sec: 5.0 + expected_total_frames: 609 + # Laser GPIO Controller Configuration laser_gpio_controller: ros__parameters: @@ -90,7 +117,22 @@ laser_gpio_controller: spectrometry_laser: interfaces: - laser_state - - temperature + - is_connected + +# 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 motor_status_broadcaster: 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 32ea8109..e0e28d7a 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", @@ -198,7 +199,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 += [ @@ -210,14 +211,18 @@ def generate_launch_description(): ] # GPIO controller spawner for Laser - gpio_controller_names = ["laser_gpio_controller"] + gpio_controller_names = ["laser_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 89dc983b..ac57a2b3 100644 --- a/src/subsystems/science/science_bringup/package.xml +++ b/src/subsystems/science/science_bringup/package.xml @@ -25,6 +25,7 @@ joint_state_broadcaster general_controllers + gpio_controllers joint_trajectory_controller diff --git a/src/subsystems/science/science_controllers/CMakeLists.txt b/src/subsystems/science/science_controllers/CMakeLists.txt index 362ec6cf..6c5bede2 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 + raman_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 athena_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..56ef244c --- /dev/null +++ b/src/subsystems/science/science_controllers/include/science_controllers/ccd_controller.hpp @@ -0,0 +1,170 @@ +// 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" + +// Replace later with your real custom messages +#include "raman_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() const; + + 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_; + + + 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_; + + 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/package.xml b/src/subsystems/science/science_controllers/package.xml index 0917cbe1..89b6e369 100644 --- a/src/subsystems/science/science_controllers/package.xml +++ b/src/subsystems/science/science_controllers/package.xml @@ -27,6 +27,8 @@ std_srvs + raman_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..07f7e896 100644 --- a/src/subsystems/science/science_controllers/science_controllers.xml +++ b/src/subsystems/science/science_controllers/science_controllers.xml @@ -25,4 +25,14 @@ Source of this file are templates in 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..7f6c3e27 --- /dev/null +++ b/src/subsystems/science/science_controllers/src/ccd_controller.cpp @@ -0,0 +1,399 @@ +#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)); + + 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)) { + if (!snapshot_complete_published_) { + publish_spectrum(time); + publish_status(time); + snapshot_complete_published_ = true; + } + + 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 = raman_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() const +{ + // TODO: + // Your current HWI only exposes scalar ros2_control states. + // It does NOT expose the completed 3648-value CCD buffer to this controller yet. + // + // For now this publishes a correctly-sized placeholder vector so the RamanSpectrum + // pipeline, GUI subscription, and service flow can be tested. + // + // Next required HWI change: + // expose byte_pixels_ after acquisition completion, or publish RamanSpectrum + // directly from the HWI/driver layer. + + 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 From c3f45f544dd5374ce4e287e6d2aee66ea402cf24 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Sun, 3 May 2026 15:23:02 -0400 Subject: [PATCH 03/16] Xacro Maintenance Interfaces --- .../science/science.ccd.ros2_control.xacro | 14 +++++++++++--- .../science/science.dc.ros2_control.xacro | 11 +++++++++++ .../science.sensor_diode.ros2_control.xacro | 10 ++++++++-- 3 files changed, 30 insertions(+), 5 deletions(-) diff --git a/src/description/ros2_control/science/science.ccd.ros2_control.xacro b/src/description/ros2_control/science/science.ccd.ros2_control.xacro index 5b2fe10e..fff27d3a 100644 --- a/src/description/ros2_control/science/science.ccd.ros2_control.xacro +++ b/src/description/ros2_control/science/science.ccd.ros2_control.xacro @@ -12,15 +12,23 @@ + + + + + + + + + - - - + + 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 27eb71b2..353f1a64 100644 --- a/src/description/ros2_control/science/science.dc.ros2_control.xacro +++ b/src/description/ros2_control/science/science.dc.ros2_control.xacro @@ -22,10 +22,16 @@ true + + + + + + @@ -36,10 +42,15 @@ true + + + + + 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 index 57e70dc5..854a589d 100644 --- a/src/description/ros2_control/science/science.sensor_diode.ros2_control.xacro +++ b/src/description/ros2_control/science/science.sensor_diode.ros2_control.xacro @@ -12,11 +12,17 @@ + + + + + + + - - + From 9001b4893c61577090a6e159dc8c71db89e44a76 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Mon, 11 May 2026 00:49:55 -0400 Subject: [PATCH 04/16] HWI Upgrades for CCD, Diode & Laser + Fixes --- .../science/science.ccd.ros2_control.xacro | 3 + .../science/science.laser.ros2_control.xacro | 7 +- .../science.sensor_diode.ros2_control.xacro | 3 + .../ccd_hardware_interface.hpp | 184 ++++--- .../src/ccd_hardware_interface.cpp | 506 ++++++++++-------- .../laser_hardware_interface.hpp | 86 +-- .../src/laser_hardware_interface.cpp | 301 ++++++----- .../sensor_diode_hardware_interface.hpp | 128 +++-- .../src/sensor_diode_hardware_interface.cpp | 363 +++++++------ 9 files changed, 887 insertions(+), 694 deletions(-) diff --git a/src/description/ros2_control/science/science.ccd.ros2_control.xacro b/src/description/ros2_control/science/science.ccd.ros2_control.xacro index fff27d3a..4f12db44 100644 --- a/src/description/ros2_control/science/science.ccd.ros2_control.xacro +++ b/src/description/ros2_control/science/science.ccd.ros2_control.xacro @@ -9,6 +9,9 @@ ccd_ros2_control/CCDHardwareInterface ${can_interface} 0x100 + 10 + 5 + 0 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 d1fe479b..250f0aea 100644 --- a/src/description/ros2_control/science/science.laser.ros2_control.xacro +++ b/src/description/ros2_control/science/science.laser.ros2_control.xacro @@ -9,13 +9,18 @@ laser_ros2_control/LaserHardwareInterface ${can_interface} 0x130 - ${port_id} + 10 + 5 + 0 + + + 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 index 854a589d..1fdeb90d 100644 --- a/src/description/ros2_control/science/science.sensor_diode.ros2_control.xacro +++ b/src/description/ros2_control/science/science.sensor_diode.ros2_control.xacro @@ -9,6 +9,9 @@ sensor_diode_ros2_control/SensorDiodeHardwareInterface ${can_interface} 0x110 + 10 + 5 + 0 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 index 123d61f2..ad80bc8b 100644 --- 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 @@ -1,26 +1,10 @@ -// Copyright (c) 2024 UMD Loop -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// - #ifndef CCD_ROS2_CONTROL__CCD_HARDWARE_INTERFACE_HPP_ #define CCD_ROS2_CONTROL__CCD_HARDWARE_INTERFACE_HPP_ +#include #include #include #include -#include -#include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -29,36 +13,17 @@ #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" - -#include "umdloop_can_library/SocketCanBus.hpp" #include "umdloop_can_library/CanFrame.hpp" +#include "umdloop_can_library/SocketCanBus.hpp" + +namespace CANLib +{ +struct CanFrame; +} namespace ccd_ros2_control { -/** - * @brief Hardware interface for CAN-controlled spectrometry ccd via ros2_control - * - * This interface controls a spectrometry ccd through CAN bus. - * - * CAN Protocol (ID: 0x100): - * - ON: DATA[0] = 0x60 - * - OFF: DATA[0] = 0x80 - * - Read Temperature: DATA[0] = 0x85 - * - Read Wavelength: DATA[0] = 0x90 - * - * State Interfaces (read by controllers): - * - ccd_state: 0.0 = OFF, 1.0 = ON - * - temperature: Ccd temperature (if available) - * - is_connected: Is CAN connected (0.0 or 1.0) - * - * Command Interfaces (written by controllers): - * - ccd_command: 0.0 = turn OFF, 1.0 = turn ON - * - * Hardware Parameters (from URDF): - * - can_interface: CAN interface name (default: "can0") - * - can_id: CAN ID for ccd commands (default: 0x100) - */ class CCDHardwareInterface : public hardware_interface::SystemInterface { public: @@ -66,67 +31,130 @@ class CCDHardwareInterface : 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(); + + // 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: - // CAN message handler - void onCanMessage(const CANLib::CanFrame& frame); + void onCanMessage(const CANLib::CanFrame & frame); + bool format_maintenance_command( + CANLib::CanFrame & frame, uint32_t can_id, const DecodedCommand & decoded_cmd); - // Configuration parameters std::string can_interface_; - uint32_t can_id_; - - // CAN bus CANLib::SocketCanBus canBus_; CANLib::CanFrame can_tx_frame_; bool can_connected_; - // State variables (hardware -> ros2_control) - double is_connected_; - double command_success_; - double acquisition_in_progress_; - double data_ready_; - double frames_received_; - double last_frame_id_; - - // Command variables (ros2_control -> hardware) - double capture_binary_cmd_; - double capture_byte_cmd_; - - bool waiting_for_binary_data_; - bool waiting_for_byte_data_; - std::vector binary_pixels_; - std::vector byte_pixels_; - - // CAN command bytes - static constexpr uint8_t CMD_REQUEST_BINARY = 0x20; // Request Binary (1-bit) Measurements - static constexpr uint8_t CMD_REQUEST_BYTE = 0x30; // Request 8-bit Measurements + int update_rate_; + int logger_rate_; + int logger_state_; + double elapsed_time_; + double elapsed_logger_time_; + + 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; + + 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 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 index 06dd64a5..e3dfcd0a 100644 --- a/src/hardware_interfaces/ccd_ros2_control/src/ccd_hardware_interface.cpp +++ b/src/hardware_interfaces/ccd_ros2_control/src/ccd_hardware_interface.cpp @@ -1,367 +1,419 @@ -// Copyright (c) 2026 UMD Loop -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// - #include "ccd_ros2_control/ccd_hardware_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "pluginlib/class_list_macros.hpp" #include "rclcpp/rclcpp.hpp" #include -#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) != + if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } - // Initialize state variables - is_connected_ = 0.0; - command_success_ = 0.0; - acquisition_in_progress_ = 0.0; - data_ready_ = 0.0; - frames_received_ = 0.0; - last_frame_id_ = 0.0; - - // Initialize command variables - capture_binary_cmd_ = 0.0; - capture_byte_cmd_ = 0.0; - - // Initialize internal acquisition state - waiting_for_binary_data_ = false; - waiting_for_byte_data_ = false; - binary_pixels_.assign(3648, 0); - byte_pixels_.assign(3648, 0); - - // Parse hardware parameters - if (info_.hardware_parameters.count("can_interface")) { - can_interface_ = info_.hardware_parameters.at("can_interface"); - } else { - can_interface_ = "can0"; - } + 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")) { - can_id_ = static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); - } else { - can_id_ = 0x100; + 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; + } } - can_connected_ = false; - - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "Initialized CCD on CAN interface %s with ID 0x%X", - can_interface_.c_str(), can_id_); + 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 & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "Configuring CCD hardware..."); + auto & joint = CCDJoints_.front(); if (can_connected_) { canBus_.close(); can_connected_ = false; } - if (!canBus_.open(can_interface_, - std::bind(&CCDHardwareInterface::onCanMessage, this, std::placeholders::_1))) + if (!canBus_.open(can_interface_, + std::bind(&CCDHardwareInterface::onCanMessage, this, std::placeholders::_1))) { - RCLCPP_WARN( - rclcpp::get_logger("CCDHardwareInterface"), - "Failed to open CAN interface %s - running in SIMULATION mode", - can_interface_.c_str()); + RCLCPP_WARN(rclcpp::get_logger("CCDHardwareInterface"), + "Failed to open CAN interface %s", can_interface_.c_str()); can_connected_ = false; } else { can_connected_ = true; - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "Successfully opened CAN interface %s", can_interface_.c_str()); } - is_connected_ = can_connected_ ? 1.0 : 0.0; - command_success_ = 0.0; - acquisition_in_progress_ = 0.0; - data_ready_ = 0.0; - frames_received_ = 0.0; - last_frame_id_ = 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "CCD hardware configured (%s)", can_connected_ ? "CAN MODE" : "SIMULATION"); + 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; return hardware_interface::CallbackReturn::SUCCESS; } -void CCDHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) +void CCDHardwareInterface::onCanMessage(const CANLib::CanFrame & frame) { - if (frame.data[0] == CMD_REQUEST_BINARY && frame.dlc >= 2) { - command_success_ = static_cast(frame.data[1] ? 1.0 : 0.0); - - if (frame.data[1]) { - waiting_for_binary_data_ = true; - waiting_for_byte_data_ = false; - acquisition_in_progress_ = 1.0; - data_ready_ = 0.0; - frames_received_ = 0.0; - last_frame_id_ = 0.0; - std::fill(binary_pixels_.begin(), binary_pixels_.end(), 0); - } else { - acquisition_in_progress_ = 0.0; - } - return; - } - - if (frame.data[0] == CMD_REQUEST_BYTE && frame.dlc >= 2) { - command_success_ = static_cast(frame.data[1] ? 1.0 : 0.0); - - if (frame.data[1]) { - waiting_for_byte_data_ = true; - waiting_for_binary_data_ = false; - acquisition_in_progress_ = 1.0; - data_ready_ = 0.0; - frames_received_ = 0.0; - last_frame_id_ = 0.0; - std::fill(byte_pixels_.begin(), byte_pixels_.end(), 0); - } else { - acquisition_in_progress_ = 0.0; + 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; } - if (waiting_for_binary_data_ && frame.dlc == 8) { + // 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]); - last_frame_id_ = static_cast(frame_id); - frames_received_ += 1.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 < binary_pixels_.size()) { - binary_pixels_[pixel_idx] = static_cast((packed >> bit) & 0x01); + if (pixel_idx < joint.binary_pixels.size()) { + joint.binary_pixels[pixel_idx] = static_cast((packed >> bit) & 0x01); } } } - // 3648 / 56 = 65 remainder 8, so 66 data frames total - if (frames_received_ >= 66.0) { - waiting_for_binary_data_ = false; - acquisition_in_progress_ = 0.0; - data_ready_ = 1.0; - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "Binary CCD acquisition complete"); - return; + 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; } - if (waiting_for_byte_data_ && frame.dlc == 8) { + // 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); - last_frame_id_ = static_cast(frame_id); - frames_received_ += 1.0; + 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 < byte_pixels_.size()) { - byte_pixels_[pixel_idx] = frame.data[2 + i]; + if (pixel_idx < joint.byte_pixels.size()) { + joint.byte_pixels[pixel_idx] = frame.data[2 + i]; } } - // 3648 / 6 = 608 data frames total - if (frames_received_ >= 608.0) { - waiting_for_byte_data_ = false; - acquisition_in_progress_ = 0.0; - data_ready_ = 1.0; - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "8-bit CCD acquisition complete"); + if (joint.frames_received >= 608.0) { + joint.waiting_for_byte_data = false; + joint.acquisition_in_progress = 0.0; + joint.data_ready = 1.0; + RCLCPP_INFO(rclcpp::get_logger("CCDHardwareInterface"), "8-bit acquisition complete"); } + return; } } -std::vector +std::vector CCDHardwareInterface::export_state_interfaces() { + auto & joint = CCDJoints_.front(); std::vector state_interfaces; - - const std::string& name = info_.gpios[0].name; - - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "is_connected", &is_connected_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "command_success", &command_success_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "acquisition_in_progress", &acquisition_in_progress_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "data_ready", &data_ready_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "frames_received", &frames_received_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "last_frame_id", &last_frame_id_)); - - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "Exported %zu state interfaces", state_interfaces.size()); - + 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 +std::vector CCDHardwareInterface::export_command_interfaces() { + auto & joint = CCDJoints_.front(); std::vector command_interfaces; - - const std::string& name = info_.gpios[0].name; - - command_interfaces.emplace_back( - hardware_interface::CommandInterface(name, "capture_binary", &capture_binary_cmd_)); - command_interfaces.emplace_back( - hardware_interface::CommandInterface(name, "capture_byte", &capture_byte_cmd_)); - - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "Exported %zu command interfaces", command_interfaces.size()); - + 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 & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "Activating CCD hardware..."); - return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn CCDHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "Deactivating CCD hardware..."); - - acquisition_in_progress_ = 0.0; - waiting_for_binary_data_ = false; - waiting_for_byte_data_ = false; + 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; - is_connected_ = 0.0; + 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 & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "Cleaning up CCD hardware..."); - + auto & joint = CCDJoints_.front(); if (can_connected_) { canBus_.close(); } - - can_connected_ = false; - is_connected_ = 0.0; - acquisition_in_progress_ = 0.0; - waiting_for_binary_data_ = false; - waiting_for_byte_data_ = false; - - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "CCD hardware cleanup complete"); - + 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) { - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "Shutting down CCD hardware..."); - return on_cleanup(previous_state); } hardware_interface::return_type CCDHardwareInterface::read( - const rclcpp::Time &, - const rclcpp::Duration &) + const rclcpp::Time &, const rclcpp::Duration &) { return hardware_interface::return_type::OK; } hardware_interface::return_type CCDHardwareInterface::write( - const rclcpp::Time &, - const rclcpp::Duration &) + 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(); + } - if (capture_binary_cmd_ > 0.5) { + // --- 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 = can_id_; - can_tx_frame_.dlc = 2; + 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] = 1; + can_tx_frame_.data[1] = CONFIRM_SEND; canBus_.send(can_tx_frame_); } - - data_ready_ = 0.0; - command_success_ = 0.0; - capture_binary_cmd_ = 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "Requested binary CCD acquisition%s", - can_connected_ ? "" : " (simulated)"); + joint.data_ready = 0.0; + joint.command_success = 0.0; + joint.capture_binary_cmd = 0.0; } - if (capture_byte_cmd_ > 0.5) { + if (joint.capture_byte_cmd > 0.5) { if (can_connected_) { can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_id_; - can_tx_frame_.dlc = 2; + 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] = 1; + 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; + } - data_ready_ = 0.0; - command_success_ = 0.0; - capture_byte_cmd_ = 0.0; - - - RCLCPP_INFO( - rclcpp::get_logger("CCDHardwareInterface"), - "Requested byte CCD acquisition%s", - can_connected_ ? "" : " (simulated)"); + // --- 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; @@ -369,8 +421,6 @@ hardware_interface::return_type CCDHardwareInterface::write( } // namespace ccd_ros2_control -#include "pluginlib/class_list_macros.hpp" - 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 e59ac544..81aaad54 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 @@ -1,18 +1,3 @@ -// Copyright (c) 2024 UMD Loop -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// - #ifndef LASER_ROS2_CONTROL__LASER_HARDWARE_INTERFACE_HPP_ #define LASER_ROS2_CONTROL__LASER_HARDWARE_INTERFACE_HPP_ @@ -20,7 +5,6 @@ #include #include #include -#include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -29,35 +13,12 @@ #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" - -#include "umdloop_can_library/SocketCanBus.hpp" #include "umdloop_can_library/CanFrame.hpp" +#include "umdloop_can_library/SocketCanBus.hpp" namespace laser_ros2_control { -/** - * @brief Hardware interface for CAN-controlled spectrometry laser via ros2_control - * - * This interface controls a spectrometry laser through CAN bus. - * - * CAN Protocol (ID: 0x130): - * - ON: DATA[0] = 0x20 + port_id, DATA[1] = 0x01 - * - OFF: DATA[0] = 0x20 + port_id, DATA[1] = 0x00 - * - Status: DATA[0] = 0x30 + port_id, DATA[1] = 0x01 - * - * State Interfaces (read by controllers): - * - laser_state: 0.0 = OFF, 1.0 = ON - * - temperature: Laser temperature (if available) - * - is_connected: Is CAN connected (0.0 or 1.0) - * - * Command Interfaces (written by controllers): - * - laser_command: 0.0 = turn OFF, 1.0 = turn ON - * - * Hardware Parameters (from URDF): - * - can_interface: CAN interface name (default: "can0") - * - can_id: CAN ID for laser commands (default: 0x130) - */ class LaserHardwareInterface : public hardware_interface::SystemInterface { public: @@ -93,32 +54,41 @@ class LaserHardwareInterface : public hardware_interface::SystemInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; -private: - // CAN message handler - void onCanMessage(const CANLib::CanFrame& frame); + void logger_function(); - // Configuration parameters +private: + struct LaserJoint + { + std::string name; + uint32_t can_id; + double laser_state; + double temperature; + double is_connected; + double status; + double laser_command; + double status_request; + double prev_status_request; + double elapsed_status_request_time; + }; + + void onCanMessage(const CANLib::CanFrame & frame); std::string can_interface_; - uint32_t can_id_; + uint32_t can_id_; + uint8_t port_id_; - // CAN bus CANLib::SocketCanBus canBus_; CANLib::CanFrame can_tx_frame_; bool can_connected_; - - // State variables (hardware → ros2_control) - double laser_state_; // Current state: 0.0 = OFF, 1.0 = ON - double is_connected_; // CAN connected status - - // Command variables (ros2_control → hardware) - double laser_command_; // Commanded state + std::vector LASERJoints_; + int update_rate_; + int logger_rate_; + int logger_state_; + double elapsed_time_; + double elapsed_logger_time_; // CAN command bytes - static constexpr uint8_t CMD_LASER_CONTROL = 0x20; // + port_id - static constexpr uint8_t CMD_LASER_STATUS = 0x30; // + port_id - - // Port ID for this laser instance - uint8_t port_id_; + 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 901dc415..65d2f6bb 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,26 +1,43 @@ -// Copyright (c) 2024 UMD Loop -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// - #include "laser_ros2_control/laser_hardware_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "pluginlib/class_list_macros.hpp" #include "rclcpp/rclcpp.hpp" +#include + namespace laser_ros2_control { +void LaserHardwareInterface::logger_function() +{ + if (LASERJoints_.empty()) { + return; + } + + const auto & joint = LASERJoints_.front(); + std::ostringstream oss; + oss << "\033[2J\033[H \nLASER Logger" + << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface_ + << " | HWI Update Rate: " << update_rate_ + << " | Logger Update Rate: " << logger_rate_ << "\n" + << "Elapsed Time since first update: " << elapsed_time_ << "\n" + << "\n--- Joint Specific ---\n" + << "JOINT: " << joint.name << "\n" + << "Parameters: CAN ID: 0x" << std::hex << std::uppercase << joint.can_id << std::dec << "\n" + << "-- Commands --\n" + << "Laser Command: " << joint.laser_command + << " | Status Request: " << joint.status_request << "\n" + << "-- State --\n" + << "Laser State: " << joint.laser_state + << " | Temperature: " << joint.temperature + << " | Is Connected: " << joint.is_connected + << " | Status: " << joint.status << "\n"; + + RCLCPP_INFO(rclcpp::get_logger("LaserHardwareInterface"), "%s", oss.str().c_str()); +} + hardware_interface::CallbackReturn LaserHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) { @@ -30,47 +47,51 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_init( return hardware_interface::CallbackReturn::ERROR; } - laser_state_ = 0.0; - is_connected_ = 0.0; - laser_command_ = 0.0; - can_connected_ = false; - - // Parse hardware parameters - if (info_.hardware_parameters.count("can_interface")) { - can_interface_ = info_.hardware_parameters.at("can_interface"); - } else { - can_interface_ = "can0"; - } - - if (info_.hardware_parameters.count("can_id")) { - can_id_ = static_cast( - std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); - } else { - can_id_ = 0x130; - } - - if (info_.hardware_parameters.count("port_id")) { - port_id_ = static_cast( - std::stoul(info_.hardware_parameters.at("port_id"), nullptr, 0)); - } else { - port_id_ = 0; - } - - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Initialized laser on CAN interface %s with ID 0x%X and port ID %u", - can_interface_.c_str(), can_id_, port_id_); + 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")) : 0; + logger_rate_ = info_.hardware_parameters.count("logger_rate") ? + 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; + + 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, // 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; + elapsed_logger_time_ = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn LaserHardwareInterface::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) + 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; @@ -92,7 +113,9 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_configure( "Successfully opened CAN interface %s", can_interface_.c_str()); } - is_connected_ = can_connected_ ? 1.0 : 0.0; + auto & joint = LASERJoints_.front(); + joint.is_connected = can_connected_ ? 1.0 : 0.0; + joint.status = joint.is_connected; RCLCPP_INFO( rclcpp::get_logger("LaserHardwareInterface"), @@ -103,118 +126,109 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_configure( void LaserHardwareInterface::onCanMessage(const CANLib::CanFrame & frame) { + auto & joint = LASERJoints_.front(); + 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_)) { - laser_state_ = frame.data[1] ? 1.0 : 0.0; + 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 state confirmed from CAN: %s", laser_state_ > 0.5 ? "ON" : "OFF"); + "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; } } -std::vector -LaserHardwareInterface::export_state_interfaces() +std::vector LaserHardwareInterface::export_state_interfaces() { + auto & joint = LASERJoints_.front(); std::vector state_interfaces; - - const std::string & name = info_.gpios[0].name; - - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "laser_state", &laser_state_)); - - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "is_connected", &is_connected_)); - - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Exported %zu state interfaces", state_interfaces.size()); - + 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); return state_interfaces; } -std::vector -LaserHardwareInterface::export_command_interfaces() +std::vector LaserHardwareInterface::export_command_interfaces() { + auto & joint = LASERJoints_.front(); std::vector command_interfaces; - - const std::string & name = info_.gpios[0].name; - - command_interfaces.emplace_back( - hardware_interface::CommandInterface(name, "laser_command", &laser_command_)); - - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Exported %zu command interfaces", command_interfaces.size()); - + 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; } hardware_interface::CallbackReturn LaserHardwareInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Activating laser hardware..."); - return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn LaserHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Deactivating laser hardware..."); + auto & joint = LASERJoints_.front(); - // Safety: Turn laser OFF on deactivation + // Safety: send laser OFF before deactivating if (can_connected_) { - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_id_; - can_tx_frame_.dlc = 2; + 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; - is_connected_ = 0.0; + can_connected_ = false; + joint.is_connected = 0.0; } - laser_state_ = 0.0; - laser_command_ = 0.0; + joint.laser_state = 0.0; + joint.laser_command = 0.0; + joint.status = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn LaserHardwareInterface::on_cleanup( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Cleaning up laser hardware..."); + auto & joint = LASERJoints_.front(); if (can_connected_) { - // Ensure laser is OFF before cleanup - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_id_; - can_tx_frame_.dlc = 2; + 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; - is_connected_ = 0.0; - laser_state_ = 0.0; - laser_command_ = 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Laser hardware cleanup complete"); + can_connected_ = false; + joint.is_connected = 0.0; + joint.laser_state = 0.0; + joint.laser_command = 0.0; + joint.status = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } @@ -222,57 +236,86 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_cleanup( hardware_interface::CallbackReturn LaserHardwareInterface::on_shutdown( const rclcpp_lifecycle::State & previous_state) { - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Shutting down laser hardware..."); - return on_cleanup(previous_state); } hardware_interface::return_type LaserHardwareInterface::read( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) + const rclcpp::Time &, const rclcpp::Duration &) { - // CAN messages are handled asynchronously via callback - // State is updated in onCanMessage() return hardware_interface::return_type::OK; } hardware_interface::return_type LaserHardwareInterface::write( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) + const rclcpp::Time &, const rclcpp::Duration & period) { - bool commanded_on = (laser_command_ > 0.5); - bool currently_on = (laser_state_ > 0.5); + auto & joint = LASERJoints_.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(); + } + } + + // --- 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; if (commanded_on != currently_on) { if (can_connected_) { - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_id_; - can_tx_frame_.dlc = 2; + 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 - laser_state_ = commanded_on ? 1.0 : 0.0; + // 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"); } } + // --- 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_LASER_STATUS + port_id_); + can_tx_frame_.data[1] = 1; + 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_LASER_STATUS + port_id_); + can_tx_frame_.data[1] = 1; + canBus_.send(can_tx_frame_); + } + } + } + joint.prev_status_request = curr_status_req; + return hardware_interface::return_type::OK; } } // namespace laser_ros2_control -#include "pluginlib/class_list_macros.hpp" - PLUGINLIB_EXPORT_CLASS( laser_ros2_control::LaserHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file 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 index 9f7543f4..d2157d5b 100644 --- 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 @@ -1,26 +1,11 @@ -// Copyright (c) 2024 UMD Loop -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// - #ifndef 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 #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -29,20 +14,12 @@ #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" - -#include "umdloop_can_library/SocketCanBus.hpp" #include "umdloop_can_library/CanFrame.hpp" +#include "umdloop_can_library/SocketCanBus.hpp" namespace sensor_diode_ros2_control { -/** - * @brief Hardware interface for CAN-controlled spectrometry sensor diode via ros2_control - * - * This interface controls a spectrometry sensor_diode through CAN bus. - * -**/ - class SensorDiodeHardwareInterface : public hardware_interface::SystemInterface { public: @@ -50,7 +27,7 @@ class SensorDiodeHardwareInterface : 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; @@ -69,41 +46,104 @@ class SensorDiodeHardwareInterface : public hardware_interface::SystemInterface 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(); private: - // CAN message handler - void onCanMessage(const CANLib::CanFrame& frame); + 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; + + // Internal + bool awaiting_response; + double maintenance_frame; + }; + + void onCanMessage(const CANLib::CanFrame & frame); - // Configuration parameters std::string can_interface_; uint32_t can_id_; uint8_t port_id_; - // CAN bus CANLib::SocketCanBus canBus_; CANLib::CanFrame can_tx_frame_; bool can_connected_; - // State variables (hardware -> ros2_control) - double is_connected_; - double command_success_; - double wavelength_intensity_; - - // Command variables (ros2_control -> hardware) - double request_measurement_cmd_; + std::vector DIODEJoints_; - bool awaiting_response_; + int update_rate_; + int logger_rate_; + int logger_state_; + double elapsed_time_; + double elapsed_logger_time_; - // CAN command bytes 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 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 index 2958f711..149b4ed5 100644 --- 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 @@ -1,23 +1,43 @@ -// Copyright (c) 2024 UMD Loop -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// - #include "sensor_diode_ros2_control/sensor_diode_hardware_interface.hpp" -#include - #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) { @@ -27,49 +47,50 @@ hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_init( return hardware_interface::CallbackReturn::ERROR; } - // Initialize states - wavelength_intensity_ = 0.0; - command_success_ = 0.0; - is_connected_ = 0.0; - - // Initialize commands - request_measurement_cmd_ = 0.0; - - // Internal - awaiting_response_ = false; - can_connected_ = false; - - // Parse parameters - if (info_.hardware_parameters.count("can_interface")) { - can_interface_ = info_.hardware_parameters.at("can_interface"); - } else { - can_interface_ = "can0"; - } - - if (info_.hardware_parameters.count("can_id")) { - can_id_ = static_cast( - std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); - } else { - can_id_ = 0x110; - } - - if (info_.hardware_parameters.count("port_id")) { - port_id_ = static_cast( - std::stoul(info_.hardware_parameters.at("port_id"), nullptr, 0)); - } else { - port_id_ = 0; - } + 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, + // 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 CAN interface %s with CAN ID 0x%X and port ID %u", + "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 & /*previous_state*/) + const rclcpp_lifecycle::State &) { RCLCPP_INFO( rclcpp::get_logger("SensorDiodeHardwareInterface"), @@ -93,109 +114,103 @@ hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_configure( can_connected_ = true; RCLCPP_INFO( rclcpp::get_logger("SensorDiodeHardwareInterface"), - "Successfully opened CAN interface %s", - can_interface_.c_str()); + "Successfully opened CAN interface %s", can_interface_.c_str()); } - is_connected_ = can_connected_ ? 1.0 : 0.0; - command_success_ = 0.0; - awaiting_response_ = false; + 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; } -std::vector -SensorDiodeHardwareInterface::export_state_interfaces() +void SensorDiodeHardwareInterface::onCanMessage(const CANLib::CanFrame & frame) { - std::vector state_interfaces; - - const std::string & name = info_.gpios[0].name; - - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "wavelength_intensity", &wavelength_intensity_)); + if (frame.id != can_id_ || frame.dlc < 2) return; - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "command_success", &command_success_)); + auto & joint = DIODEJoints_.front(); - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "is_connected", &is_connected_)); - - RCLCPP_INFO( - rclcpp::get_logger("SensorDiodeHardwareInterface"), - "Exported %zu state interfaces", - state_interfaces.size()); + // 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; - - const std::string & name = info_.gpios[0].name; - - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - name, "request_measurement", &request_measurement_cmd_)); - - RCLCPP_INFO( - rclcpp::get_logger("SensorDiodeHardwareInterface"), - "Exported %zu command interfaces", - command_interfaces.size()); - + 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 & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("SensorDiodeHardwareInterface"), - "Activating sensor diode hardware..."); - return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("SensorDiodeHardwareInterface"), - "Deactivating sensor diode hardware..."); - - awaiting_response_ = false; - request_measurement_cmd_ = 0.0; + auto & joint = DIODEJoints_.front(); + joint.awaiting_response = false; + joint.request_measurement_cmd = 0.0; if (can_connected_) { canBus_.close(); - can_connected_ = false; - is_connected_ = 0.0; + 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 & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("SensorDiodeHardwareInterface"), - "Cleaning up sensor diode hardware..."); + auto & joint = DIODEJoints_.front(); if (can_connected_) { canBus_.close(); } - can_connected_ = false; - is_connected_ = 0.0; - awaiting_response_ = false; - wavelength_intensity_ = 0.0; - command_success_ = 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("SensorDiodeHardwareInterface"), - "Sensor diode hardware cleanup complete"); + 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; } @@ -203,76 +218,112 @@ hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_cleanup( hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_shutdown( const rclcpp_lifecycle::State & previous_state) { - RCLCPP_INFO( - rclcpp::get_logger("SensorDiodeHardwareInterface"), - "Shutting down sensor diode hardware..."); - return on_cleanup(previous_state); } -void SensorDiodeHardwareInterface::onCanMessage(const CANLib::CanFrame & frame) -{ - if (frame.id != can_id_ || frame.dlc < 2) { - return; - } - - const uint8_t expected_cmd = static_cast(CMD_READ_DIODE_VALUE + port_id_); - - if (frame.data[0] != expected_cmd) { - return; - } - - // Response format: - // data[0] = 0x20 + port_id - // data[1] = wavelength_intensity [0 to 255] - wavelength_intensity_ = static_cast(frame.data[1]); - command_success_ = 1.0; - awaiting_response_ = false; - - RCLCPP_DEBUG( - rclcpp::get_logger("SensorDiodeHardwareInterface"), - "Wavelength Intensity: %.0f", - wavelength_intensity_); -} - hardware_interface::return_type SensorDiodeHardwareInterface::read( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) + const rclcpp::Time &, const rclcpp::Duration &) { - // CAN messages are processed asynchronously in the callback. return hardware_interface::return_type::OK; } hardware_interface::return_type SensorDiodeHardwareInterface::write( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) + const rclcpp::Time &, const rclcpp::Duration & period) { - if (request_measurement_cmd_ > 0.5) { - command_success_ = 0.0; - awaiting_response_ = true; + 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.command_success = 0.0; + joint.awaiting_response = true; if (can_connected_) { - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_id_; - can_tx_frame_.dlc = 2; + 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] = 1; // validate request - + 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_); + "Requested diode measurement on port %u", port_id_); } else { RCLCPP_INFO( rclcpp::get_logger("SensorDiodeHardwareInterface"), - "Requested diode measurement on port %u (simulated)", - port_id_); + "Requested diode measurement on port %u (simulated)", port_id_); } + joint.request_measurement_cmd = 0.0; + } - // Reset trigger after sending - request_measurement_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] = 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; From 05bf697546755c3c6dddb659d70756ba999e0556 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Wed, 13 May 2026 18:58:57 -0400 Subject: [PATCH 05/16] CCD Controller Fixes + CCD CANbus flow py script --- src/msgs/{package.xml.bak => package.xml} | 0 .../science/science_bringup/CMakeLists.txt | 5 + .../config/athena_science_controllers.yaml | 10 + .../scripts/ccd_can_simulator.py | 283 ++++++++++++++++++ 4 files changed, 298 insertions(+) rename src/msgs/{package.xml.bak => package.xml} (100%) create mode 100644 src/subsystems/science/science_bringup/scripts/ccd_can_simulator.py diff --git a/src/msgs/package.xml.bak b/src/msgs/package.xml similarity index 100% rename from src/msgs/package.xml.bak rename to src/msgs/package.xml diff --git a/src/subsystems/science/science_bringup/CMakeLists.txt b/src/subsystems/science/science_bringup/CMakeLists.txt index 280abdff..cd5d947c 100644 --- a/src/subsystems/science/science_bringup/CMakeLists.txt +++ b/src/subsystems/science/science_bringup/CMakeLists.txt @@ -20,6 +20,11 @@ install( DESTINATION share/${PROJECT_NAME} ) +install(PROGRAMS + scripts/ccd_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 83d1e867..33643131 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -140,6 +140,7 @@ sensor_diode_gpio_controller: - wavelength_intensity - command_success - is_connected + - status fluoro_led_gpio_controller: ros__parameters: @@ -184,6 +185,9 @@ motor_status_controller: spectrometry_laser: interfaces: - status_request + sensor_diode: + interfaces: + - status_request fluoro_led: interfaces: - status_request @@ -238,10 +242,16 @@ motor_status_controller: 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/scripts/ccd_can_simulator.py b/src/subsystems/science/science_bringup/scripts/ccd_can_simulator.py new file mode 100644 index 00000000..296c84fb --- /dev/null +++ b/src/subsystems/science/science_bringup/scripts/ccd_can_simulator.py @@ -0,0 +1,283 @@ +#!/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") + + +# ── Main loop ──────────────────────────────────────────────────────────────── + +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 From b936635718cf6d08dc2c01099b522b680ab6eae0 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Wed, 13 May 2026 19:35:12 -0400 Subject: [PATCH 06/16] CCD Controller Edits --- .../ccd_hardware_interface.hpp | 7 +++++ .../src/ccd_hardware_interface.cpp | 20 ++++++++++++++ .../config/athena_science_controllers.yaml | 2 +- .../science_controllers/ccd_controller.hpp | 5 ++++ .../src/ccd_controller.cpp | 26 ++++++++++--------- 5 files changed, 47 insertions(+), 13 deletions(-) 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 index ad80bc8b..f2257552 100644 --- 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 @@ -15,6 +15,9 @@ #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 "raman_msgs/msg/raman_spectrum.hpp" namespace CANLib { @@ -116,6 +119,9 @@ class CCDHardwareInterface : public hardware_interface::SystemInterface 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; @@ -126,6 +132,7 @@ class CCDHardwareInterface : public hardware_interface::SystemInterface 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) { 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 index e3dfcd0a..164844a1 100644 --- a/src/hardware_interfaces/ccd_ros2_control/src/ccd_hardware_interface.cpp +++ b/src/hardware_interfaces/ccd_ros2_control/src/ccd_hardware_interface.cpp @@ -143,6 +143,10 @@ hardware_interface::CallbackReturn CCDHardwareInterface::on_configure( 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; } @@ -232,10 +236,26 @@ void CCDHardwareInterface::onCanMessage(const CANLib::CanFrame & frame) joint.waiting_for_byte_data = false; joint.acquisition_in_progress = 0.0; joint.data_ready = 1.0; + + // Publish completed pixel buffer + if (pixel_publisher_) { + raman_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 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 33643131..ddf9d87d 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -104,7 +104,7 @@ ccd_controller: snapshot_service_name: "~/request_snapshot" status_publish_topic: "~/status" - snapshot_publish_topic: "~/snapshot" + snapshot_publish_topic: "/raman/raw_spectrum" publish_rate: 10.0 acquisition_timeout_sec: 5.0 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 index 56ef244c..f36c2ff0 100644 --- a/src/subsystems/science/science_controllers/include/science_controllers/ccd_controller.hpp +++ b/src/subsystems/science/science_controllers/include/science_controllers/ccd_controller.hpp @@ -32,6 +32,7 @@ // Replace later with your real custom messages #include "raman_msgs/msg/raman_spectrum.hpp" + namespace science_controllers { @@ -133,6 +134,10 @@ class CCDSnapshotController : public controller_interface::ControllerInterface 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_; diff --git a/src/subsystems/science/science_controllers/src/ccd_controller.cpp b/src/subsystems/science/science_controllers/src/ccd_controller.cpp index 7f6c3e27..4cabd23f 100644 --- a/src/subsystems/science/science_controllers/src/ccd_controller.cpp +++ b/src/subsystems/science/science_controllers/src/ccd_controller.cpp @@ -136,6 +136,13 @@ controller_interface::CallbackReturn CCDSnapshotController::on_configure( std::placeholders::_1, std::placeholders::_2)); + pixel_subscriber_ = get_node()->create_subscription( + "/raman/raw_pixels", + rclcpp::QoS(1).reliable(), + [this](const raman_msgs::msg::RamanSpectrum::SharedPtr msg) { + pixel_buffer_.writeFromNonRT(msg->intensities); + }); + status_publisher_ = get_node()->create_publisher( status_publish_topic_, rclcpp::QoS(10)); @@ -233,7 +240,8 @@ controller_interface::return_type CCDSnapshotController::update( } if (snapshot_in_progress_) { - if (get_state_bool(STATE_DATA_READY)) { + if (get_state_bool(STATE_DATA_READY) || + get_state_double(STATE_FRAMES_RECEIVED) >= static_cast(expected_total_frames_ - 1)) { if (!snapshot_complete_published_) { publish_spectrum(time); publish_status(time); @@ -378,17 +386,11 @@ std::vector CCDSnapshotController::make_wavenumber_axis() const std::vector CCDSnapshotController::get_latest_intensities() const { - // TODO: - // Your current HWI only exposes scalar ros2_control states. - // It does NOT expose the completed 3648-value CCD buffer to this controller yet. - // - // For now this publishes a correctly-sized placeholder vector so the RamanSpectrum - // pipeline, GUI subscription, and service flow can be tested. - // - // Next required HWI change: - // expose byte_pixels_ after acquisition completion, or publish RamanSpectrum - // directly from the HWI/driver layer. - + 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); } From d44bfd1fba4f878d27f5e4694085add8dbd58ac8 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Fri, 15 May 2026 19:08:50 -0400 Subject: [PATCH 07/16] add raman_msgs dependency to ccd_ros2_control, fix ccd controller pixel buffer --- .../ccd_ros2_control/CMakeLists.txt | 2 ++ src/hardware_interfaces/ccd_ros2_control/package.xml | 1 + .../include/science_controllers/ccd_controller.hpp | 5 ++--- .../science/science_controllers/src/ccd_controller.cpp | 10 ++++++++-- 4 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt b/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt index d7a0ce8d..ba66dda2 100644 --- a/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt +++ b/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt @@ -18,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_auto REQUIRED) +find_package(raman_msgs REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() @@ -51,6 +52,7 @@ ament_target_dependencies( rclcpp_lifecycle realtime_tools msgs + raman_msgs ) # Export hardware plugins diff --git a/src/hardware_interfaces/ccd_ros2_control/package.xml b/src/hardware_interfaces/ccd_ros2_control/package.xml index dc766c5a..91e5aea4 100644 --- a/src/hardware_interfaces/ccd_ros2_control/package.xml +++ b/src/hardware_interfaces/ccd_ros2_control/package.xml @@ -13,6 +13,7 @@ pluginlib rclcpp rclcpp_lifecycle + raman_msgs ament_cmake realtime_tools 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 index f36c2ff0..679cf1f2 100644 --- a/src/subsystems/science/science_controllers/include/science_controllers/ccd_controller.hpp +++ b/src/subsystems/science/science_controllers/include/science_controllers/ccd_controller.hpp @@ -93,7 +93,7 @@ class CCDSnapshotController : public controller_interface::ControllerInterface double get_state_double(size_t index) const; std::vector make_wavenumber_axis() const; - std::vector get_latest_intensities() const; + std::vector get_latest_intensities(); std::string ccd_name_; std::vector command_interface_names_; @@ -110,11 +110,10 @@ class CCDSnapshotController : public controller_interface::ControllerInterface 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 { diff --git a/src/subsystems/science/science_controllers/src/ccd_controller.cpp b/src/subsystems/science/science_controllers/src/ccd_controller.cpp index 4cabd23f..1cd4cca4 100644 --- a/src/subsystems/science/science_controllers/src/ccd_controller.cpp +++ b/src/subsystems/science/science_controllers/src/ccd_controller.cpp @@ -243,9 +243,15 @@ controller_interface::return_type CCDSnapshotController::update( 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; @@ -297,7 +303,7 @@ void CCDSnapshotController::handle_snapshot_request( response->message = "CCD snapshot request accepted"; } -void CCDSnapshotController::publish_status(const rclcpp::Time & time) +void CCDSnapshotController::publish_status(const rclcpp::Time & /*time*/) { if (!realtime_status_publisher_ || !realtime_status_publisher_->trylock()) { return; @@ -384,7 +390,7 @@ std::vector CCDSnapshotController::make_wavenumber_axis() const return axis; } -std::vector CCDSnapshotController::get_latest_intensities() const +std::vector CCDSnapshotController::get_latest_intensities() { auto buffer_ptr = pixel_buffer_.readFromRT(); if (buffer_ptr && !(*buffer_ptr).empty()) { From 8a7055b0b93b31c3846a080e763e06c9f30d9778 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Fri, 15 May 2026 21:14:16 -0400 Subject: [PATCH 08/16] Build dependency for raman_msgs to prevent colcon error --- src/hardware_interfaces/ccd_ros2_control/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hardware_interfaces/ccd_ros2_control/package.xml b/src/hardware_interfaces/ccd_ros2_control/package.xml index 91e5aea4..cc33e431 100644 --- a/src/hardware_interfaces/ccd_ros2_control/package.xml +++ b/src/hardware_interfaces/ccd_ros2_control/package.xml @@ -13,8 +13,8 @@ pluginlib rclcpp rclcpp_lifecycle - raman_msgs + raman_msgs ament_cmake realtime_tools msgs From 1e59e5a7447bb1bd291fe0c45e4646e7966e66dd Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Fri, 15 May 2026 23:04:17 -0400 Subject: [PATCH 09/16] Modified github workflows to pass colcon build for custom raman msgs --- .github/workflows/main.yml | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index bb9126b9..9260fd6f 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -33,9 +33,20 @@ jobs: rosdep install --from-paths . --ignore-src -r -y shell: bash + - name: Create raman_msgs symlink + run: | + ln -s $GITHUB_WORKSPACE/src/msgs/raman_msgs $GITHUB_WORKSPACE/../raman_msgs + shell: bash + + - name: Build raman_msgs first + run: | + source /opt/ros/humble/setup.bash + colcon build --packages-select raman_msgs + shell: bash + - name: Build workspace 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 From cc80593b041589379ff134c8f4d4b394e8dfe472 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Sun, 24 May 2026 11:22:41 -0400 Subject: [PATCH 10/16] PR Fixes --- .../science/science.ccd.ros2_control.xacro | 1 + .../science.mock_talon.ros2_control.xacro | 17 -- .../ccd_ros2_control/CMakeLists.txt | 2 +- .../ccd_hardware_interface.hpp | 6 +- .../ccd_ros2_control/package.xml | 2 +- .../src/ccd_hardware_interface.cpp | 4 +- .../laser_hardware_interface.hpp | 2 +- .../src/laser_hardware_interface.cpp | 4 +- .../sensor_diode_hardware_interface.hpp | 1 + .../src/sensor_diode_hardware_interface.cpp | 10 +- src/msgs/CMakeLists.txt | 2 + src/msgs/{raman_msgs => }/msg/RamanPeaks.msg | 0 .../{raman_msgs => }/msg/RamanSpectrum.msg | 0 .../msg/raman_preprocessing/CMakeLists.txt | 26 --- src/msgs/msg/raman_preprocessing/package.xml | 18 --- src/msgs/raman_msgs/CMakeLists.txt | 20 --- src/msgs/raman_msgs/package.xml | 22 --- src/msgs/raman_msgs/raman_driver | 1 - .../science/science_bringup/CMakeLists.txt | 1 + .../scripts/ccd_can_simulator.py | 1 - .../scripts/sensor_diode_can_simulator.py | 148 ++++++++++++++++++ .../science_controllers/CMakeLists.txt | 2 +- .../science_controllers/ccd_controller.hpp | 8 +- .../science/science_controllers/package.xml | 2 +- .../src/ccd_controller.cpp | 8 +- 25 files changed, 175 insertions(+), 133 deletions(-) delete mode 100644 src/description/ros2_control/science/science.mock_talon.ros2_control.xacro rename src/msgs/{raman_msgs => }/msg/RamanPeaks.msg (100%) rename src/msgs/{raman_msgs => }/msg/RamanSpectrum.msg (100%) delete mode 100644 src/msgs/msg/raman_preprocessing/CMakeLists.txt delete mode 100644 src/msgs/msg/raman_preprocessing/package.xml delete mode 100644 src/msgs/raman_msgs/CMakeLists.txt delete mode 100644 src/msgs/raman_msgs/package.xml delete mode 120000 src/msgs/raman_msgs/raman_driver create mode 100755 src/subsystems/science/science_bringup/scripts/sensor_diode_can_simulator.py diff --git a/src/description/ros2_control/science/science.ccd.ros2_control.xacro b/src/description/ros2_control/science/science.ccd.ros2_control.xacro index 4f12db44..0d51b324 100644 --- a/src/description/ros2_control/science/science.ccd.ros2_control.xacro +++ b/src/description/ros2_control/science/science.ccd.ros2_control.xacro @@ -15,6 +15,7 @@ + 0 diff --git a/src/description/ros2_control/science/science.mock_talon.ros2_control.xacro b/src/description/ros2_control/science/science.mock_talon.ros2_control.xacro deleted file mode 100644 index 5df33c15..00000000 --- a/src/description/ros2_control/science/science.mock_talon.ros2_control.xacro +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - mock_components/GenericSystem - - - - - - - - - - - \ No newline at end of file diff --git a/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt b/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt index ba66dda2..26ba4df4 100644 --- a/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt +++ b/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt @@ -18,7 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS # find dependencies find_package(ament_cmake REQUIRED) find_package(ament_cmake_auto REQUIRED) -find_package(raman_msgs REQUIRED) +find_package(msgs REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() 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 index f2257552..b1301df6 100644 --- 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 @@ -17,7 +17,7 @@ #include "umdloop_can_library/SocketCanBus.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "raman_msgs/msg/raman_spectrum.hpp" +#include "msgs/msg/raman_spectrum.hpp" namespace CANLib { @@ -104,7 +104,7 @@ class CCDHardwareInterface : public hardware_interface::SystemInterface }; private: - void onCanMessage(const CANLib::CanFrame & frame); + void on_can_message(const CANLib::CanFrame & frame); bool format_maintenance_command( CANLib::CanFrame & frame, uint32_t can_id, const DecodedCommand & decoded_cmd); @@ -120,7 +120,7 @@ class CCDHardwareInterface : public hardware_interface::SystemInterface double elapsed_logger_time_; rclcpp::Node::SharedPtr pixel_pub_node_; - rclcpp::Publisher::SharedPtr pixel_publisher_; + rclcpp::Publisher::SharedPtr pixel_publisher_; std::vector CCDJoints_; diff --git a/src/hardware_interfaces/ccd_ros2_control/package.xml b/src/hardware_interfaces/ccd_ros2_control/package.xml index cc33e431..e1d7afb8 100644 --- a/src/hardware_interfaces/ccd_ros2_control/package.xml +++ b/src/hardware_interfaces/ccd_ros2_control/package.xml @@ -14,7 +14,7 @@ rclcpp rclcpp_lifecycle - raman_msgs + msgs ament_cmake realtime_tools msgs 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 index 164844a1..4e059cf6 100644 --- a/src/hardware_interfaces/ccd_ros2_control/src/ccd_hardware_interface.cpp +++ b/src/hardware_interfaces/ccd_ros2_control/src/ccd_hardware_interface.cpp @@ -144,7 +144,7 @@ hardware_interface::CallbackReturn CCDHardwareInterface::on_configure( joint.last_frame_id = 0.0; pixel_pub_node_ = rclcpp::Node::make_shared("ccd_hwi_pixel_publisher"); - pixel_publisher_ = pixel_pub_node_->create_publisher( + pixel_publisher_ = pixel_pub_node_->create_publisher( "/raman/raw_pixels", rclcpp::QoS(1).reliable()); return hardware_interface::CallbackReturn::SUCCESS; @@ -239,7 +239,7 @@ void CCDHardwareInterface::onCanMessage(const CANLib::CanFrame & frame) // Publish completed pixel buffer if (pixel_publisher_) { - raman_msgs::msg::RamanSpectrum msg; + msgs::msg::RamanSpectrum msg; msg.header.stamp = rclcpp::Clock().now(); msg.header.frame_id = "ccd_sensor"; msg.spectrometer_id = "pda_spectrometer"; 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 81aaad54..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 @@ -71,7 +71,7 @@ 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_; 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 65d2f6bb..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 @@ -99,7 +99,7 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_configure( if (!canBus_.open( can_interface_, - std::bind(&LaserHardwareInterface::onCanMessage, this, std::placeholders::_1))) + std::bind(&LaserHardwareInterface::on_can_message, this, std::placeholders::_1))) { RCLCPP_WARN( rclcpp::get_logger("LaserHardwareInterface"), @@ -124,7 +124,7 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_configure( 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(); 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 index d2157d5b..bbc071d7 100644 --- 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 @@ -80,6 +80,7 @@ class SensorDiodeHardwareInterface : public hardware_interface::SystemInterface double prev_maintenance_request; double elapsed_status_request_time; double elapsed_maintenance_request_time; + double prev_request_measurement_cmd; // Internal bool awaiting_response; 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 index 149b4ed5..0419e7cc 100644 --- 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 @@ -72,7 +72,7 @@ hardware_interface::CallbackReturn SensorDiodeHardwareInterface::on_init( // command 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // tracking - 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, // internal false, 0.0 }); @@ -242,7 +242,7 @@ hardware_interface::return_type SensorDiodeHardwareInterface::write( } // --- Measurement request: one-shot on rising edge > 0.5 --- - if (joint.request_measurement_cmd > 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_) { @@ -255,13 +255,9 @@ hardware_interface::return_type SensorDiodeHardwareInterface::write( RCLCPP_INFO( rclcpp::get_logger("SensorDiodeHardwareInterface"), "Requested diode measurement on port %u", port_id_); - } else { - RCLCPP_INFO( - rclcpp::get_logger("SensorDiodeHardwareInterface"), - "Requested diode measurement on port %u (simulated)", port_id_); } - joint.request_measurement_cmd = 0.0; } + 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; diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index 9617b224..b178ef1a 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" "srv/SetController.srv" "srv/StatusReq.srv" "srv/MaintenanceReq.srv" diff --git a/src/msgs/raman_msgs/msg/RamanPeaks.msg b/src/msgs/msg/RamanPeaks.msg similarity index 100% rename from src/msgs/raman_msgs/msg/RamanPeaks.msg rename to src/msgs/msg/RamanPeaks.msg diff --git a/src/msgs/raman_msgs/msg/RamanSpectrum.msg b/src/msgs/msg/RamanSpectrum.msg similarity index 100% rename from src/msgs/raman_msgs/msg/RamanSpectrum.msg rename to src/msgs/msg/RamanSpectrum.msg diff --git a/src/msgs/msg/raman_preprocessing/CMakeLists.txt b/src/msgs/msg/raman_preprocessing/CMakeLists.txt deleted file mode 100644 index e953e7f3..00000000 --- a/src/msgs/msg/raman_preprocessing/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(raman_preprocessing) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/src/msgs/msg/raman_preprocessing/package.xml b/src/msgs/msg/raman_preprocessing/package.xml deleted file mode 100644 index a572825d..00000000 --- a/src/msgs/msg/raman_preprocessing/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - raman_preprocessing - 0.0.0 - TODO: Package description - henry - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/msgs/raman_msgs/CMakeLists.txt b/src/msgs/raman_msgs/CMakeLists.txt deleted file mode 100644 index 85eaecdf..00000000 --- a/src/msgs/raman_msgs/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(raman_msgs) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/RamanSpectrum.msg" - "msg/RamanPeaks.msg" - DEPENDENCIES std_msgs -) - -ament_export_dependencies(rosidl_default_runtime) - -ament_package() \ No newline at end of file diff --git a/src/msgs/raman_msgs/package.xml b/src/msgs/raman_msgs/package.xml deleted file mode 100644 index 89a172e5..00000000 --- a/src/msgs/raman_msgs/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - raman_msgs - 0.1.0 - Custom message definitions for Raman spectrometer data - UMD Loop - Apache-2.0 - - ament_cmake - rosidl_default_generators - - std_msgs - - rosidl_default_runtime - - rosidl_interface_packages - - - ament_cmake - - \ No newline at end of file diff --git a/src/msgs/raman_msgs/raman_driver b/src/msgs/raman_msgs/raman_driver deleted file mode 120000 index 06030c86..00000000 --- a/src/msgs/raman_msgs/raman_driver +++ /dev/null @@ -1 +0,0 @@ -/home/henry/ros2_ws/src/athena-code-fresh/src/msgs/msg/raman_driver \ No newline at end of file diff --git a/src/subsystems/science/science_bringup/CMakeLists.txt b/src/subsystems/science/science_bringup/CMakeLists.txt index cd5d947c..09ac8e6b 100644 --- a/src/subsystems/science/science_bringup/CMakeLists.txt +++ b/src/subsystems/science/science_bringup/CMakeLists.txt @@ -22,6 +22,7 @@ install( install(PROGRAMS scripts/ccd_can_simulator.py + scripts/sensor_diode_can_simulator.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/subsystems/science/science_bringup/scripts/ccd_can_simulator.py b/src/subsystems/science/science_bringup/scripts/ccd_can_simulator.py index 296c84fb..8b2d19f8 100644 --- a/src/subsystems/science/science_bringup/scripts/ccd_can_simulator.py +++ b/src/subsystems/science/science_bringup/scripts/ccd_can_simulator.py @@ -205,7 +205,6 @@ def simulate_byte_acquisition(sock: socket.socket, port_id: int): print(f"[SIM] 8-bit acquisition complete — {BYTE_FRAMES} frames sent") -# ── Main loop ──────────────────────────────────────────────────────────────── def run_simulator(interface: str, force_mode: str = None, delay: float = INTER_FRAME_DELAY_S): global INTER_FRAME_DELAY_S 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 6c5bede2..c6cba21f 100644 --- a/src/subsystems/science/science_controllers/CMakeLists.txt +++ b/src/subsystems/science/science_controllers/CMakeLists.txt @@ -15,7 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_lifecycle realtime_tools std_srvs - raman_msgs + msgs ) find_package(ament_cmake REQUIRED) 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 index 679cf1f2..49292371 100644 --- a/src/subsystems/science/science_controllers/include/science_controllers/ccd_controller.hpp +++ b/src/subsystems/science/science_controllers/include/science_controllers/ccd_controller.hpp @@ -28,9 +28,7 @@ #include "science_controllers/visibility_control.h" #include "std_srvs/srv/trigger.hpp" #include "std_msgs/msg/string.hpp" - -// Replace later with your real custom messages -#include "raman_msgs/msg/raman_spectrum.hpp" +#include "msgs/msg/raman_spectrum.hpp" namespace science_controllers @@ -131,10 +129,10 @@ class CCDSnapshotController : public controller_interface::ControllerInterface rclcpp::Publisher::SharedPtr status_publisher_; std::unique_ptr realtime_status_publisher_; - rclcpp::Publisher::SharedPtr spectrum_publisher_; + rclcpp::Publisher::SharedPtr spectrum_publisher_; // Subscribers - rclcpp::Subscription::SharedPtr pixel_subscriber_; + rclcpp::Subscription::SharedPtr pixel_subscriber_; realtime_tools::RealtimeBuffer> pixel_buffer_; bool snapshot_requested_; diff --git a/src/subsystems/science/science_controllers/package.xml b/src/subsystems/science/science_controllers/package.xml index 89b6e369..6bafc952 100644 --- a/src/subsystems/science/science_controllers/package.xml +++ b/src/subsystems/science/science_controllers/package.xml @@ -27,7 +27,7 @@ std_srvs - raman_msgs + msgs diff --git a/src/subsystems/science/science_controllers/src/ccd_controller.cpp b/src/subsystems/science/science_controllers/src/ccd_controller.cpp index 1cd4cca4..6b1ad753 100644 --- a/src/subsystems/science/science_controllers/src/ccd_controller.cpp +++ b/src/subsystems/science/science_controllers/src/ccd_controller.cpp @@ -136,10 +136,10 @@ controller_interface::CallbackReturn CCDSnapshotController::on_configure( std::placeholders::_1, std::placeholders::_2)); - pixel_subscriber_ = get_node()->create_subscription( + pixel_subscriber_ = get_node()->create_subscription( "/raman/raw_pixels", rclcpp::QoS(1).reliable(), - [this](const raman_msgs::msg::RamanSpectrum::SharedPtr msg) { + [this](const msgs::msg::RamanSpectrum::SharedPtr msg) { pixel_buffer_.writeFromNonRT(msg->intensities); }); @@ -148,7 +148,7 @@ controller_interface::CallbackReturn CCDSnapshotController::on_configure( realtime_status_publisher_ = std::make_unique(status_publisher_); - spectrum_publisher_ = get_node()->create_publisher( + spectrum_publisher_ = get_node()->create_publisher( snapshot_publish_topic_, rclcpp::QoS(10)); auto initial_request = std::make_shared(); @@ -325,7 +325,7 @@ void CCDSnapshotController::publish_status(const rclcpp::Time & /*time*/) void CCDSnapshotController::publish_spectrum(const rclcpp::Time & time) { - auto msg = raman_msgs::msg::RamanSpectrum(); + auto msg = msgs::msg::RamanSpectrum(); msg.header.stamp = time; msg.header.frame_id = "raman_probe"; From a08659d5eaeb6af0190ccc2999ccabd727013465 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Sun, 24 May 2026 11:45:27 -0400 Subject: [PATCH 11/16] Removed raman_msgs pkg from main yaml file --- .github/workflows/main.yml | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 9260fd6f..5f2544ff 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -33,17 +33,6 @@ jobs: rosdep install --from-paths . --ignore-src -r -y shell: bash - - name: Create raman_msgs symlink - run: | - ln -s $GITHUB_WORKSPACE/src/msgs/raman_msgs $GITHUB_WORKSPACE/../raman_msgs - shell: bash - - - name: Build raman_msgs first - run: | - source /opt/ros/humble/setup.bash - colcon build --packages-select raman_msgs - shell: bash - - name: Build workspace run: | source /opt/ros/humble/setup.bash From 2a9496f09998e8c8ae82a86bf5ea3df3641ac8e9 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Sun, 24 May 2026 11:53:08 -0400 Subject: [PATCH 12/16] Small package.xml ccd fix --- src/hardware_interfaces/ccd_ros2_control/package.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/hardware_interfaces/ccd_ros2_control/package.xml b/src/hardware_interfaces/ccd_ros2_control/package.xml index e1d7afb8..2ed0f15f 100644 --- a/src/hardware_interfaces/ccd_ros2_control/package.xml +++ b/src/hardware_interfaces/ccd_ros2_control/package.xml @@ -14,10 +14,10 @@ rclcpp rclcpp_lifecycle - msgs ament_cmake realtime_tools - msgs + msgs + msgs ament_cmake rosidl_default_runtime rosidl_interface_packages @@ -32,4 +32,4 @@ ament_cmake - + \ No newline at end of file From 81d63fa12faf7e55c61a39e61ced0cf389f359a6 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Sun, 24 May 2026 12:07:55 -0400 Subject: [PATCH 13/16] snake case fix in CCD HWI --- src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt | 1 - .../ccd_ros2_control/src/ccd_hardware_interface.cpp | 4 ++-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt b/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt index 26ba4df4..c1c6aa68 100644 --- a/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt +++ b/src/hardware_interfaces/ccd_ros2_control/CMakeLists.txt @@ -52,7 +52,6 @@ ament_target_dependencies( rclcpp_lifecycle realtime_tools msgs - raman_msgs ) # Export hardware plugins 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 index 4e059cf6..eb20e702 100644 --- a/src/hardware_interfaces/ccd_ros2_control/src/ccd_hardware_interface.cpp +++ b/src/hardware_interfaces/ccd_ros2_control/src/ccd_hardware_interface.cpp @@ -126,7 +126,7 @@ hardware_interface::CallbackReturn CCDHardwareInterface::on_configure( } if (!canBus_.open(can_interface_, - std::bind(&CCDHardwareInterface::onCanMessage, this, std::placeholders::_1))) + 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()); @@ -150,7 +150,7 @@ hardware_interface::CallbackReturn CCDHardwareInterface::on_configure( return hardware_interface::CallbackReturn::SUCCESS; } -void CCDHardwareInterface::onCanMessage(const CANLib::CanFrame & frame) +void CCDHardwareInterface::on_can_message(const CANLib::CanFrame & frame) { auto & joint = CCDJoints_.front(); From 82203ab19cddbbe8f09c046958f5fdb80a4ead8b Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Sun, 24 May 2026 12:57:09 -0400 Subject: [PATCH 14/16] Fix CMakeList --- src/subsystems/science/science_controllers/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/subsystems/science/science_controllers/CMakeLists.txt b/src/subsystems/science/science_controllers/CMakeLists.txt index c6cba21f..204447cb 100644 --- a/src/subsystems/science/science_controllers/CMakeLists.txt +++ b/src/subsystems/science/science_controllers/CMakeLists.txt @@ -99,7 +99,7 @@ if(BUILD_TESTING) 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) + target_link_libraries(test_athena_science_manual science_manual) ament_target_dependencies( test_athena_science_manual controller_interface @@ -108,7 +108,7 @@ if(BUILD_TESTING) 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) + target_link_libraries(test_athena_science_manual_preceeding science_manual) ament_target_dependencies( test_athena_science_manual_preceeding controller_interface From 7b394e388778d53a615b54d0ecb0150d727bc552 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Sun, 24 May 2026 13:37:58 -0400 Subject: [PATCH 15/16] Fix inconsistent naming conventions --- src/subsystems/science/science_controllers/CMakeLists.txt | 2 +- .../{science_controller.hpp => science_manual.hpp} | 0 .../science/science_controllers/science_controllers.xml | 2 +- .../science/science_controllers/src/science_controller.cpp | 2 +- 4 files changed, 3 insertions(+), 3 deletions(-) rename src/subsystems/science/science_controllers/include/science_controllers/{science_controller.hpp => science_manual.hpp} (100%) diff --git a/src/subsystems/science/science_controllers/CMakeLists.txt b/src/subsystems/science/science_controllers/CMakeLists.txt index 204447cb..152ea4ef 100644 --- a/src/subsystems/science/science_controllers/CMakeLists.txt +++ b/src/subsystems/science/science_controllers/CMakeLists.txt @@ -99,7 +99,7 @@ if(BUILD_TESTING) 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 science_manual) + target_link_libraries(test_athena_science_manual athena_science_manual) ament_target_dependencies( test_athena_science_manual controller_interface 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/science_controllers.xml b/src/subsystems/science/science_controllers/science_controllers.xml index 07f7e896..b55b221c 100644 --- a/src/subsystems/science/science_controllers/science_controllers.xml +++ b/src/subsystems/science/science_controllers/science_controllers.xml @@ -18,7 +18,7 @@ Source of this file are templates in [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. --> - + 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 From 6938590936c65fc12a0fe42834613c1c1d693b55 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Sun, 24 May 2026 16:33:11 -0400 Subject: [PATCH 16/16] Added arg can_interfaces to ccd & sensor diode --- src/description/urdf/athena_science.urdf.xacro | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/description/urdf/athena_science.urdf.xacro b/src/description/urdf/athena_science.urdf.xacro index 2a047a78..d7075d5e 100644 --- a/src/description/urdf/athena_science.urdf.xacro +++ b/src/description/urdf/athena_science.urdf.xacro @@ -47,10 +47,10 @@ - + - +