diff --git a/.gitignore b/.gitignore index 86668ec0..2bf691f1 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,7 @@ build/ install/ log/ .vscode/ +.codex # Byte-compiled / optimized / DLL files __pycache__/ diff --git a/src/description/ros2_control/arm/arm.base_limit_switch.ros2_control.xacro b/src/description/ros2_control/arm/arm.base_limit_switch.ros2_control.xacro new file mode 100644 index 00000000..29735835 --- /dev/null +++ b/src/description/ros2_control/arm/arm.base_limit_switch.ros2_control.xacro @@ -0,0 +1,27 @@ + + + + + + limit_switch_ros2_control/LimitSwitchHardwareInterface + can1 + 0x130 + 10 + 5 + 0 + + + + 0 + + + + + + 1 + + + + + + diff --git a/src/description/ros2_control/arm/arm.end_effector_limit_switch.ros2_control.xacro b/src/description/ros2_control/arm/arm.end_effector_limit_switch.ros2_control.xacro new file mode 100644 index 00000000..597e9576 --- /dev/null +++ b/src/description/ros2_control/arm/arm.end_effector_limit_switch.ros2_control.xacro @@ -0,0 +1,33 @@ + + + + + + limit_switch_ros2_control/LimitSwitchHardwareInterface + can1 + 0x60 + 10 + 5 + 0 + + + + 2 + + + + + + 3 + + + + + + 4 + + + + + + diff --git a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro index 37d243cf..15c0a997 100644 --- a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro @@ -7,7 +7,7 @@ 10 5 0 - can0 + can1 @@ -15,10 +15,18 @@ 100 1.0 150 + + + + + + + + @@ -26,10 +34,18 @@ 100 -1.0 150 + + + + + + + + diff --git a/src/description/ros2_control/arm/arm.rotary_encoder.ros2_control.xacro b/src/description/ros2_control/arm/arm.rotary_encoder.ros2_control.xacro new file mode 100644 index 00000000..e24ece2c --- /dev/null +++ b/src/description/ros2_control/arm/arm.rotary_encoder.ros2_control.xacro @@ -0,0 +1,39 @@ + + + + + + rotary_encoder_ros2_control/RotaryEncoderHardwareInterface + can1 + 0x130 + 10 + 5 + 0 + + + + 5 + 0 + + + + + + + 5 + 1 + + + + + + + 5 + 2 + + + + + + + diff --git a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro index 434514fc..16885fab 100644 --- a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro @@ -7,7 +7,7 @@ 1 5 0 - can0 + can1 0x80 @@ -20,7 +20,50 @@ prismatic - + + + + + + + + + + + + 0x2 + 1 + 180 + standard + revolute + + + + + + + + + + + + + + 0x3 + 1 + 180 + standard + revolute + + + + + + + + + + diff --git a/src/description/ros2_control/arm/arm.smc_2dof.ros2_control.xacro b/src/description/ros2_control/arm/arm.smc_2dof.ros2_control.xacro index 1988b0f4..05a1446d 100644 --- a/src/description/ros2_control/arm/arm.smc_2dof.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.smc_2dof.ros2_control.xacro @@ -7,7 +7,7 @@ 10 5 0 - can0 + can1 @@ -17,36 +17,54 @@ 300 + + + + + + - 0x145 + 0x147 40 1.0 1000 + + + + + + - 0x147 + 0x145 40 1.0 1000 + + + + + + diff --git a/src/description/ros2_control/arm/arm.smc_3dof.ros2_control.xacro b/src/description/ros2_control/arm/arm.smc_3dof.ros2_control.xacro index e50c884a..a79076ce 100644 --- a/src/description/ros2_control/arm/arm.smc_3dof.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.smc_3dof.ros2_control.xacro @@ -7,7 +7,7 @@ 10 5 0 - can0 + can1 @@ -17,11 +17,18 @@ 300 + + + + + + + diff --git a/src/description/ros2_control/arm/arm.talon_2dof.ros2_control.xacro b/src/description/ros2_control/arm/arm.talon_2dof.ros2_control.xacro index d2372c73..d4ae7274 100644 --- a/src/description/ros2_control/arm/arm.talon_2dof.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.talon_2dof.ros2_control.xacro @@ -12,7 +12,7 @@ 10 5 0 - can0 + can1 @@ -32,8 +32,12 @@ 0.0 + + + + diff --git a/src/description/ros2_control/arm/arm.talon_3dof.ros2_control.xacro b/src/description/ros2_control/arm/arm.talon_3dof.ros2_control.xacro index c795063a..392d4e55 100644 --- a/src/description/ros2_control/arm/arm.talon_3dof.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.talon_3dof.ros2_control.xacro @@ -11,7 +11,7 @@ 10 5 0 - can0 + can1 @@ -30,10 +30,12 @@ 0.0 + + @@ -51,10 +53,12 @@ 0.0 + + @@ -72,10 +76,12 @@ 0.0 + + @@ -94,8 +100,12 @@ 0.0 + + + + diff --git a/src/description/ros2_control/drive/drive.led.ros2_control.xacro b/src/description/ros2_control/drive/drive.led.ros2_control.xacro index b560b895..24628c1d 100644 --- a/src/description/ros2_control/drive/drive.led.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.led.ros2_control.xacro @@ -1,19 +1,27 @@ - + led_ros2_control/LEDHardwareInterface - ${gpio_pin} - ${default_state} + ${can_interface} + 0x120 + 10 + 5 + 0 - - - + true + 0 + + + + + + diff --git a/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro b/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro index 2551097d..ec25c66b 100644 --- a/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro @@ -1,12 +1,21 @@ - + - odrive_ros2_control_plugin/ODriveHardwareInterface - can1 + ${deactivate_odrive} + + mock_components/GenericSystem + + + odrive_ros2_control_plugin/ODriveHardwareInterface + 30 + 5 + 0 + can1 + @@ -14,10 +23,13 @@ 26 - - 0.0 - + + + + + @@ -25,10 +37,13 @@ 26 - - 0.0 - + + + + + diff --git a/src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro b/src/description/ros2_control/drive/drive.power_module.ros2_control.xacro similarity index 53% rename from src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro rename to src/description/ros2_control/drive/drive.power_module.ros2_control.xacro index 2ee52595..8f6c90bc 100644 --- a/src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.power_module.ros2_control.xacro @@ -1,23 +1,29 @@ - + - killswitch_ros2_control/KillswitchHardwareInterface + power_module_ros2_control/PowerModuleHardwareInterface ${can_interface} 0x100 + 10 + 5 + 0 + - - + + + + diff --git a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro index 9b2a21bf..593bd109 100644 --- a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro @@ -15,12 +15,18 @@ 1 -1.0 150 - + + + + + + + @@ -28,12 +34,18 @@ 1 1.0 150 - + + + + + + + @@ -41,12 +53,18 @@ 1 1.0 150 - + + + + + + + @@ -54,12 +72,18 @@ 1 -1.0 150 - + + + + + + + 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..49687248 --- /dev/null +++ b/src/description/ros2_control/science/science.dc.ros2_control.xacro @@ -0,0 +1,61 @@ + + + + + + + + + dc_ros2_control/DCHardwareInterface + 20 + 5 + 0 + can0 + 0x70 + + + + 0 + revolute + + 50.9 + true + + + + + + + + + + + + + + + + 1 + revolute + + 50.9 + true + + + + + + + + + + + + + + + + + + + diff --git a/src/description/ros2_control/science/science.laser.ros2_control.xacro b/src/description/ros2_control/science/science.laser.ros2_control.xacro index ee7f97c7..6fea9233 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 + 10 + 5 + 0 + + @@ -23,4 +28,3 @@ - diff --git a/src/description/ros2_control/science/science.led.ros2_control.xacro b/src/description/ros2_control/science/science.led.ros2_control.xacro new file mode 100644 index 00000000..4b583700 --- /dev/null +++ b/src/description/ros2_control/science/science.led.ros2_control.xacro @@ -0,0 +1,23 @@ + + + + + + led_ros2_control/LEDHardwareInterface + can0 + 0x110 + 10 + 5 + 0 + + + + false + 0 + + + + + + + diff --git a/src/description/ros2_control/science/science.limit_switch.ros2_control.xacro b/src/description/ros2_control/science/science.limit_switch.ros2_control.xacro new file mode 100644 index 00000000..698ced10 --- /dev/null +++ b/src/description/ros2_control/science/science.limit_switch.ros2_control.xacro @@ -0,0 +1,21 @@ + + + + + + limit_switch_ros2_control/LimitSwitchHardwareInterface + can0 + 0x100 + 10 + 5 + 0 + + + + 0 + + + + + + diff --git a/src/description/ros2_control/science/science.mock.ros2_control.xacro b/src/description/ros2_control/science/science.mock.ros2_control.xacro index f1686fcc..37987cf4 100644 --- a/src/description/ros2_control/science/science.mock.ros2_control.xacro +++ b/src/description/ros2_control/science/science.mock.ros2_control.xacro @@ -55,13 +55,13 @@ - + - + diff --git a/src/description/ros2_control/science/science.servo.ros2_control.xacro b/src/description/ros2_control/science/science.servo.ros2_control.xacro index c441e9b5..d20d1578 100644 --- a/src/description/ros2_control/science/science.servo.ros2_control.xacro +++ b/src/description/ros2_control/science/science.servo.ros2_control.xacro @@ -6,7 +6,7 @@ servo_ros2_control/SERVOHardwareInterface 20 5 - 1 + 0 can0 0x80 @@ -20,8 +20,14 @@ revolute + + + + + + @@ -33,8 +39,14 @@ revolute + + + + + + @@ -45,7 +57,14 @@ revolute + + + + + + + @@ -56,7 +75,14 @@ revolute + + + + + + + @@ -68,8 +94,14 @@ revolute + + + + + + @@ -81,8 +113,14 @@ revolute + + + + + + @@ -94,7 +132,14 @@ revolute + + + + + + + diff --git a/src/description/ros2_control/science/science.stepper.ros2_control.xacro b/src/description/ros2_control/science/science.stepper.ros2_control.xacro index 0b4e8e57..8d8fc00f 100644 --- a/src/description/ros2_control/science/science.stepper.ros2_control.xacro +++ b/src/description/ros2_control/science/science.stepper.ros2_control.xacro @@ -7,39 +7,49 @@ stepper_ros2_control/STEPPERHardwareInterface + 10 + 5 + 0 + can0 + 0x90 + 0x0 + 1 + 20 + continuous + + + + + - 0.0 - - - 1 - - - 1 - + + + 0x1 + 1 + 20 + continuous + + + + + - 0.0 - - - 1 - - - 1 - + - + \ No newline at end of file diff --git a/src/description/ros2_control/science/science.talon.ros2_control.xacro b/src/description/ros2_control/science/science.talon.ros2_control.xacro index 7d9b663c..5629a1da 100644 --- a/src/description/ros2_control/science/science.talon.ros2_control.xacro +++ b/src/description/ros2_control/science/science.talon.ros2_control.xacro @@ -30,10 +30,12 @@ 0.0 + + @@ -50,10 +52,12 @@ 0.0 + + diff --git a/src/description/urdf/athena_arm.urdf.xacro b/src/description/urdf/athena_arm.urdf.xacro index f7691179..3d4453d8 100644 --- a/src/description/urdf/athena_arm.urdf.xacro +++ b/src/description/urdf/athena_arm.urdf.xacro @@ -32,6 +32,13 @@ + + + + + + + @@ -59,7 +66,10 @@ + + + - \ No newline at end of file + diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index 66e8d7a2..11119ab5 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -3,11 +3,11 @@ - + @@ -39,8 +39,8 @@ - - + + @@ -70,14 +70,18 @@ simulation_controllers="$(arg simulation_controllers)"/> - + + + + + - + - - + + diff --git a/src/description/urdf/athena_science.urdf.xacro b/src/description/urdf/athena_science.urdf.xacro index b81b61e3..92b023a4 100644 --- a/src/description/urdf/athena_science.urdf.xacro +++ b/src/description/urdf/athena_science.urdf.xacro @@ -6,10 +6,14 @@ - - + + + + + + @@ -30,11 +34,18 @@ + + + - + + + + + - \ No newline at end of file + diff --git a/src/hardware_interfaces/README.md b/src/hardware_interfaces/README.md index a083d586..60fea846 100644 --- a/src/hardware_interfaces/README.md +++ b/src/hardware_interfaces/README.md @@ -4,7 +4,7 @@ This directory contains ROS2 Control hardware interface implementations for vari ## Available Interfaces -- `killswitch_ros2_control/` - Killswitch hardware interface +- `power_module_ros2_control/` - PowerModule hardware interface - `laser_ros2_control/` - Laser hardware interface - `led_ros2_control/` - LED hardware interface - `rmd_ros2_control/` - RMD motor controller interface 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..a9f9e196 --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/include/dc_ros2_control/dc_hardware_interface.hpp @@ -0,0 +1,322 @@ +#ifndef DC_HARDWARE_INTERFACE_HPP_ +#define DC_HARDWARE_INTERFACE_HPP_ + +#include +#include +#include +#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 CANLib +{ +struct CanFrame; +} + +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); + + struct DecodedCommand + { + uint8_t command_id; + std::vector u8_data; + std::vector i16_data; + std::vector i32_data; + }; + + void format_control_command(CANLib::CanFrame & frame, size_t joint_index); + bool format_status_command(CANLib::CanFrame & frame, uint8_t command_id, uint8_t node_id); + bool format_maintenance_command( + CANLib::CanFrame & frame, uint8_t node_id, const DecodedCommand & decoded_cmd); + +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_; + std::vector motor_status_; + + std::vector motor_status_req_; + std::vector motor_maintenance_req_; + std::vector maintenance_frame_high_; + std::vector maintenance_frame_low_; + std::vector maintenance_frame_; + std::vector maintenance_data_count_; + std::vector> decoded_maintenance_frame_; + + std::vector prev_status_req_; + std::vector prev_maintenance_req_; + std::vector elapsed_status_request_time_; + std::vector elapsed_maintenance_request_time_; + + // 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_; + + struct DCJoint + { + std::string name; + uint8_t node_id; + int gear_ratio; + bool inverted; + double max_disp; + double distance_per_rev; + integration_level_t control_level; + joint_type_t joint_type; + + double joint_state_position; + double joint_state_velocity; + double motor_position; + double motor_velocity; + double motor_status; + double motor_temperature; + double motor_torque_current; + + double joint_command_position; + double joint_command_velocity; + double motor_status_req; + double motor_maintenance_req; + double maintenance_frame_high; + double maintenance_frame_low; + double maintenance_frame; + double maintenance_data_count; + std::vector decoded_maintenance_frame; + + double prev_status_req; + double prev_maintenance_req; + double elapsed_status_request_time; + double elapsed_maintenance_request_time; + double prev_joint_command_position; + double prev_joint_command_velocity; + int device_status; + + std::vector state_interface_names; + std::vector command_interface_names; + std::unordered_map parameters; + }; + + std::vector DCJoints_; + + enum class ControlCommands : uint8_t + { + ABSOLUTE_POS_CONTROL_CMD = 0x20, + VELOCITY_CONTROL_CMD = 0x30, + }; + + enum class MaintenanceCommands : uint8_t + { + PCB_HEARTBEAT_CMD = 0x10, + MAINTENANCE_CMD = 0x60, + DC_SPECS_CMD = 0x70, + }; + + enum class MaintenanceCommandOptions : uint8_t + { + SET_CURRENT_MULTI_TURN_POS_ZERO_TO_ROM_CMD = 0x00, + REQUEST_VECTORS_CMD = 0x01, + MOTOR_STOP_CMD = 0x02, + MOTOR_SHUTDOWN_CMD = 0x03, + CLEAR_ERRORS_CMD = 0x04, + }; + + enum class StatusCommands : uint8_t + { + MOTOR_STATE = 0x40, + MOTOR_STATUS = 0x50, + }; + + enum class MotorStatus : uint8_t + { + UNDEFINED = 0, + IDLE = 1, + STARTUP_SEQUENCE = 2, + + ERROR_INVALID_REQUEST = 3, + ERROR_MOTOR_DISARMED = 4, + ERROR_MOTOR_FAILED = 5, + ERROR_CONTROLLER_FAILED = 6, + ERROR_ESTOP_REQUESTED = 7, + ERROR_UNKNOWN_POSITION = 8, + + POSITION_CONTROL = 9, + VELOCITY_CONTROL = 10, + MOTOR_STOPPED = 11 + }; + + enum class ValidateRequest : uint8_t + { + INVALID = 0, + VALID = 1, + }; + + static constexpr std::array kStatusCommands = { + StatusCommands::MOTOR_STATE, + StatusCommands::MOTOR_STATUS, + }; + + 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 dc_ros2_control + +#endif // DC_HARDWARE_INTERFACE_HPP_ 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..0c505a78 --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp @@ -0,0 +1,837 @@ +#include "dc_ros2_control/dc_hardware_interface.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/system_interface.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)); +} + +void DCHardwareInterface::format_control_command(CANLib::CanFrame & frame, size_t joint_index) +{ + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); + + auto & joint = DCJoints_[joint_index]; + const double dir = joint.inverted ? -1.0 : 1.0; + + if ( + joint.control_level == integration_level_t::POSITION && + std::isfinite(joint.joint_command_position) && + joint.joint_command_position != joint.prev_joint_command_position) + { + joint.prev_joint_command_position = joint.joint_command_position; + if (joint.joint_type == joint_type_t::PRISMATIC) { + joint.joint_command_position = std::clamp( + joint.joint_command_position, 0.0, joint.max_disp); + } + + int16_t motor_pos = 0; + if (joint.joint_type == joint_type_t::REVOLUTE) { + motor_pos = calculate_motor_position_from_desired_joint_position( + dir * joint.joint_command_position, joint.gear_ratio); + } else { + motor_pos = calculate_motor_position_from_desired_joint_displacement( + dir * joint.joint_command_position, joint.gear_ratio, joint.distance_per_rev); + } + + frame.dlc = 3; + frame.data[0] = static_cast( + static_cast(ControlCommands::ABSOLUTE_POS_CONTROL_CMD) + joint.node_id); + frame.data[1] = static_cast(motor_pos & 0xFF); + frame.data[2] = static_cast((motor_pos >> 8) & 0xFF); + return; + } + + if ( + joint.control_level == integration_level_t::VELOCITY && + std::isfinite(joint.joint_command_velocity) && + joint.joint_command_velocity != joint.prev_joint_command_velocity) + { + joint.prev_joint_command_velocity = joint.joint_command_velocity; + int16_t motor_vel = 0; + if (joint.joint_type == joint_type_t::REVOLUTE) { + motor_vel = calculate_motor_velocity_from_desired_joint_angular_velocity( + dir * joint.joint_command_velocity, joint.gear_ratio); + } else { + motor_vel = calculate_motor_velocity_from_desired_joint_linear_velocity( + dir * joint.joint_command_velocity, joint.gear_ratio, joint.distance_per_rev); + } + + frame.dlc = 3; + frame.data[0] = static_cast( + static_cast(ControlCommands::VELOCITY_CONTROL_CMD) + joint.node_id); + frame.data[1] = static_cast(motor_vel & 0xFF); + frame.data[2] = static_cast((motor_vel >> 8) & 0xFF); + return; + } + + frame.dlc = 2; + frame.data[0] = static_cast( + static_cast(StatusCommands::MOTOR_STATE) + joint.node_id); + frame.data[1] = static_cast(ValidateRequest::VALID); +} + +bool DCHardwareInterface::format_status_command( + CANLib::CanFrame & frame, uint8_t command_id, uint8_t node_id) +{ + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); + frame.dlc = 2; + switch (static_cast(command_id)) { + case StatusCommands::MOTOR_STATE: + frame.data[0] = static_cast(static_cast(StatusCommands::MOTOR_STATE) + node_id); + frame.data[1] = static_cast(ValidateRequest::VALID); + return true; + case StatusCommands::MOTOR_STATUS: + frame.data[0] = static_cast(static_cast(StatusCommands::MOTOR_STATUS) + node_id); + frame.data[1] = static_cast(ValidateRequest::VALID); + return true; + default: + return false; + } +} + +bool DCHardwareInterface::format_maintenance_command( + CANLib::CanFrame & frame, uint8_t node_id, const DecodedCommand & decoded_cmd) +{ + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); + frame.dlc = 2; + frame.data[0] = static_cast( + static_cast(MaintenanceCommands::MAINTENANCE_CMD) + node_id); + frame.data[1] = decoded_cmd.command_id; + + switch (static_cast(decoded_cmd.command_id)) { + case MaintenanceCommands::PCB_HEARTBEAT_CMD: + if (decoded_cmd.u8_data.size() != 1 || !decoded_cmd.i16_data.empty() || + !decoded_cmd.i32_data.empty()) + { + return false; + } + frame.data[0] = static_cast(MaintenanceCommands::PCB_HEARTBEAT_CMD); + frame.data[1] = decoded_cmd.u8_data[0]; + return true; + case MaintenanceCommands::MAINTENANCE_CMD: + if (decoded_cmd.u8_data.size() != 1 || !decoded_cmd.i16_data.empty() || + !decoded_cmd.i32_data.empty()) + { + return false; + } + frame.data[0] = static_cast(static_cast(MaintenanceCommands::MAINTENANCE_CMD) + node_id); + frame.data[1] = decoded_cmd.u8_data[0]; + return true; + case MaintenanceCommands::DC_SPECS_CMD: + if (decoded_cmd.u8_data.size() != 1 || !decoded_cmd.i16_data.empty() || + !decoded_cmd.i32_data.empty()) + { + return false; + } + frame.data[0] = static_cast(static_cast(MaintenanceCommands::DC_SPECS_CMD) + node_id); + frame.data[1] = static_cast(ValidateRequest::VALID); + return true; + default: + return false; + } +} + +// ============================================================================= +// 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++) { + const auto & joint = DCJoints_[static_cast(i)]; + switch (joint.control_level) { + 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.joint_type) { + case joint_type_t::REVOLUTE: joint_type_str = "REVOLUTE"; break; + case joint_type_t::PRISMATIC: joint_type_str = "PRISMATIC"; break; + } + + switch (joint.device_status) { + case static_cast(MotorStatus::UNDEFINED): status = "Undefined"; break; + case static_cast(MotorStatus::IDLE): status = "Idle"; break; + case static_cast(MotorStatus::STARTUP_SEQUENCE): status = "Startup Sequence"; break; + case static_cast(MotorStatus::ERROR_INVALID_REQUEST): status = "Error (Invalid Request)"; break; + case static_cast(MotorStatus::ERROR_MOTOR_DISARMED): status = "Error (Motor Disarmed)"; break; + case static_cast(MotorStatus::ERROR_MOTOR_FAILED): status = "Error (Motor Failed)"; break; + case static_cast(MotorStatus::ERROR_CONTROLLER_FAILED): status = "Error (Controller Failed)"; break; + case static_cast(MotorStatus::ERROR_ESTOP_REQUESTED): status = "Error (ESTOP Requested)"; break; + case static_cast(MotorStatus::ERROR_UNKNOWN_POSITION): status = "Error (Unknown Position)"; break; + case static_cast(MotorStatus::POSITION_CONTROL): status = "Position Control"; break; + case static_cast(MotorStatus::VELOCITY_CONTROL): status = "Velocity Control"; break; + case static_cast(MotorStatus::MOTOR_STOPPED): status = "Motor Stopped"; break; + default: status = "UNKNOWN (" + std::to_string(joint.device_status) + ")"; break; + } + + oss << "\nJOINT: " << joint.name << "\n" + << "Parameters: Node ID: 0x" << std::hex << std::uppercase << static_cast(joint.node_id) + << " | Gear Ratio: " << std::dec << joint.gear_ratio + << " | Inverted: " << (joint.inverted ? "true" : "false") + << " | Device Status: " << status + << " | Control Mode: " << control_mode + << " | Joint Type: " << joint_type_str << "\n" + << "-- Commands --\n" + << "Joint Command Position: " << joint.joint_command_position + << " | Joint Command Velocity: " << joint.joint_command_velocity << "\n" + << "-- State --\n" + << "Joint Position: " << joint.joint_state_position + << " | Joint Velocity: " << joint.joint_state_velocity << "\n" + << "-- Telemetry --\n" + << "Temperature: " << joint.motor_temperature << " C" + << " | Torque Current: " << joint.motor_torque_current << " 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; + } + + DCJoints_.clear(); + for (const auto & joint : info_.joints) { + std::vector state_if_names; + for (const auto & si : joint.state_interfaces) { + state_if_names.push_back(si.name); + } + + std::vector command_if_names; + for (const auto & ci : joint.command_interfaces) { + command_if_names.push_back(ci.name); + } + + std::unordered_map params_map; + for (const auto & p : joint.parameters) { + params_map.emplace(p.first, p.second); + } + + const uint8_t node_id = static_cast( + std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0, 15)); + const int gear_ratio = joint.parameters.count("gear_ratio") ? + std::abs(std::stoi(joint.parameters.at("gear_ratio"))) : 1; + const bool inverted = joint.parameters.count("inverted") && + joint.parameters.at("inverted") == "true"; + + joint_type_t joint_type = joint_type_t::REVOLUTE; + double joint_max_disp = std::numeric_limits::quiet_NaN(); + double joint_distance_per_rev = 0.0; + const std::string jtype = joint.parameters.count("joint_type") ? + joint.parameters.at("joint_type") : "revolute"; + if (jtype == "revolute") { + joint_type = joint_type_t::REVOLUTE; + } else if ( + jtype == "prismatic" && + joint.parameters.count("max_disp") && + joint.parameters.count("distance_per_rev")) + { + joint_type = joint_type_t::PRISMATIC; + joint_max_disp = std::abs(std::stod(joint.parameters.at("max_disp"))); + joint_distance_per_rev = 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; + } + + const integration_level_t initial_mode = + jtype == "continuous" ? integration_level_t::VELOCITY : integration_level_t::POSITION; + + DCJoints_.push_back(DCJoint{ + joint.name, + node_id, + gear_ratio, + inverted, + joint_max_disp, + joint_distance_per_rev, + initial_mode, + joint_type, + std::numeric_limits::quiet_NaN(), + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + std::numeric_limits::quiet_NaN(), + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + {}, + 0.0, + 0.0, + 0.0, + 0.0, + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + static_cast(MotorStatus::UNDEFINED), + state_if_names, + command_if_names, + params_map + }); + } + + // -- 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; + + 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 (size_t i = 0; i < DCJoints_.size(); i++) { + auto & joint = DCJoints_[i]; + for (const auto & iface : joint.state_interface_names) { + double * value = nullptr; + if (iface == hardware_interface::HW_IF_POSITION) { + value = &joint.joint_state_position; + } else if (iface == hardware_interface::HW_IF_VELOCITY) { + value = &joint.joint_state_velocity; + } else if (iface == "status") { + value = &joint.motor_status; + } else if (iface == "motor_temperature") { + value = &joint.motor_temperature; + } else if (iface == "torque_current") { + value = &joint.motor_torque_current; + } + + if (value == nullptr) { + continue; + } + state_interfaces.emplace_back(joint.name, iface, value); + } + } + + return state_interfaces; +} + +std::vector +DCHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + + for (size_t i = 0; i < DCJoints_.size(); i++) { + auto & joint = DCJoints_[i]; + for (const auto & iface : joint.command_interface_names) { + double * value = nullptr; + if (iface == hardware_interface::HW_IF_POSITION) { + value = &joint.joint_command_position; + } else if (iface == hardware_interface::HW_IF_VELOCITY) { + value = &joint.joint_command_velocity; + } else if (iface == "status_request") { + value = &joint.motor_status_req; + } else if (iface == "maintenance_request") { + value = &joint.motor_maintenance_req; + } else if (iface == "maintenance_frame_high") { + value = &joint.maintenance_frame_high; + } else if (iface == "maintenance_frame_low") { + value = &joint.maintenance_frame_low; + } else if (iface == "maintenance_data_count") { + value = &joint.maintenance_data_count; + } + + if (value == nullptr) { + continue; + } + command_interfaces.emplace_back(joint.name, iface, value); + } + } + + 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..."); + + for (const auto & joint : DCJoints_) { + CANLib::CanFrame frame; + frame.id = can_command_id; + if (format_maintenance_command( + frame, joint.node_id, + DecodedCommand{ + static_cast(MaintenanceCommands::MAINTENANCE_CMD), + {static_cast(MaintenanceCommandOptions::MOTOR_SHUTDOWN_CMD)}, + {}, + {} + })) + { + canBus.send(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 + for (auto & joint : DCJoints_) { + joint.joint_command_position = joint.joint_state_position; + joint.joint_command_velocity = 0.0; + } + + for (size_t i = 0; i < DCJoints_.size(); ++i) { + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "Joint %zu command position initialized to: %f", i, DCJoints_[i].joint_command_position); + } + + 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..."); + + for (const auto & joint : DCJoints_) { + CANLib::CanFrame frame; + frame.id = can_command_id; + if (format_maintenance_command( + frame, joint.node_id, + DecodedCommand{ + static_cast(MaintenanceCommands::MAINTENANCE_CMD), + {static_cast(MaintenanceCommandOptions::MOTOR_STOP_CMD)}, + {}, + {} + })) + { + canBus.send(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; + + if (frame.id != can_response_id) { + return; + } + + const uint8_t command_nibble = static_cast((frame.data[0] >> 4) & 0x0F); + const uint8_t device_id_nibble = static_cast(frame.data[0] & 0x0F); + + for (size_t i = 0; i < DCJoints_.size(); i++) { + auto & joint = DCJoints_[i]; + if (joint.node_id != device_id_nibble) { + continue; + } + + if (command_nibble == (static_cast(StatusCommands::MOTOR_STATE) >> 4) || + command_nibble == (static_cast(ControlCommands::ABSOLUTE_POS_CONTROL_CMD) >> 4) || + command_nibble == (static_cast(ControlCommands::VELOCITY_CONTROL_CMD) >> 4)) { + const double raw_motor_position = static_cast( + static_cast((frame.data[2] << 8) | frame.data[1])); + const double raw_motor_velocity = static_cast( + static_cast((frame.data[4] << 8) | frame.data[3])); + const double dir = joint.inverted ? -1.0 : 1.0; + + if (joint.joint_type == joint_type_t::REVOLUTE) { + joint.motor_position = dir * calculate_joint_position_from_motor_position( + raw_motor_position, joint.gear_ratio); + joint.motor_velocity = dir * calculate_joint_angular_velocity_from_motor_velocity( + raw_motor_velocity, joint.gear_ratio); + } else { + joint.motor_position = dir * calculate_joint_displacement_from_motor_position( + raw_motor_position, joint.gear_ratio, joint.distance_per_rev); + joint.motor_velocity = dir * calculate_joint_linear_velocity_from_motor_velocity( + raw_motor_velocity, joint.gear_ratio, joint.distance_per_rev); + } + return; + } + + if (command_nibble == (static_cast(StatusCommands::MOTOR_STATUS) >> 4)) { + joint.device_status = frame.data[1]; + joint.motor_status = static_cast(frame.data[1]); + return; + } + + if (command_nibble == (static_cast(MaintenanceCommands::MAINTENANCE_CMD) >> 4) && frame.data[0] == 1) { + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Successfully sent maintenance command"); + return; + } + + if (command_nibble == (static_cast(MaintenanceCommands::DC_SPECS_CMD) >> 4)) { + const uint8_t command_id = frame.data[0]; + const uint8_t motor_type = frame.data[1]; + + const uint16_t max_motor_position = + static_cast(frame.data[2]) | + (static_cast(frame.data[3]) << 8); + + const uint16_t max_motor_velocity = + static_cast(frame.data[4]) | + (static_cast(frame.data[5]) << 8); + + RCLCPP_INFO( + rclcpp::get_logger("DCHardwareInterface"), + "DC Config Reply | " + "command_id: 0x%02X (%u) | " + "motor_type: %u | " + "max_motor_position: %u | " + "max_motor_velocity: %u", + command_id, + command_id, + motor_type, + max_motor_position, + max_motor_velocity + ); + return; + } + } +} + +// ============================================================================= +// Read (update state from CAN data) +// ============================================================================= + +hardware_interface::return_type DCHardwareInterface::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + for (auto & joint : DCJoints_) { + joint.joint_state_velocity = joint.motor_velocity; + joint.joint_state_position = joint.motor_position; + + // Check device status — allow Undefined(0), Idle(1), Position Control(9), Velocity Control(10) + const int s = joint.device_status; + if (s != 0 && s != 1 && s != 9 && s != 10) { + RCLCPP_ERROR(rclcpp::get_logger("DCHardwareInterface"), + "Joint %s in error state: %d", joint.name.c_str(), s); + return hardware_interface::return_type::ERROR; + } + + // TODO: Telemetry — populate motor_temperature and motor_torque_current 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_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(); + } + } + + for (size_t i = 0; i < DCJoints_.size(); ++i) { + auto & joint = DCJoints_[i]; + const double curr_status_req = joint.motor_status_req; + if (curr_status_req < 0.0 && joint.prev_status_req >= 0.0) { + for (auto status_cmd : kStatusCommands) { + CANLib::CanFrame frame; + frame.id = can_command_id; + if (format_status_command(frame, static_cast(status_cmd), joint.node_id)) { + canBus.send(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; + for (auto status_cmd : kStatusCommands) { + CANLib::CanFrame frame; + frame.id = can_command_id; + if (format_status_command(frame, static_cast(status_cmd), joint.node_id)) { + canBus.send(frame); + } + } + } + } + joint.prev_status_req = curr_status_req; + } + + for (size_t i = 0; i < DCJoints_.size(); ++i) { + auto & joint = DCJoints_[i]; + 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)); + joint.decoded_maintenance_frame.clear(); + joint.decoded_maintenance_frame.push_back(decoded_maintenance_cmd.command_id); + joint.decoded_maintenance_frame.insert( + joint.decoded_maintenance_frame.end(), + decoded_maintenance_cmd.u8_data.begin(), + decoded_maintenance_cmd.u8_data.end()); + + CANLib::CanFrame frame; + frame.id = can_command_id; + if (!format_maintenance_command(frame, joint.node_id, decoded_maintenance_cmd)) { + joint.prev_maintenance_req = joint.motor_maintenance_req; + continue; + } + + const double curr_maintenance_req = joint.motor_maintenance_req; + if (curr_maintenance_req < 0.0 && joint.prev_maintenance_req >= 0.0) { + canBus.send(frame); + } else if (curr_maintenance_req > 0.0) { + joint.elapsed_maintenance_request_time += period.seconds(); + if (joint.elapsed_maintenance_request_time > (1.0 / curr_maintenance_req)) { + joint.elapsed_maintenance_request_time = 0.0; + canBus.send(frame); + } + } + joint.prev_maintenance_req = curr_maintenance_req; + } + + elapsed_update_time += period.seconds(); + if (update_rate > 0 && elapsed_update_time > (1.0 / static_cast(update_rate))) { + elapsed_update_time = 0.0; + if (!DCJoints_.empty()) { + const size_t joint_index = static_cast(write_count % num_joints); + ++write_count; + CANLib::CanFrame frame; + frame.id = can_command_id; + format_control_command(frame, joint_index); + canBus.send(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(DCJoints_[static_cast(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 auto & joint = DCJoints_[static_cast(i)]; + const std::string pos_if = joint.name + "/" + std::string(hardware_interface::HW_IF_POSITION); + const std::string vel_if = joint.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) { + auto & joint = DCJoints_[static_cast(i)]; + if (requested_modes[i] == integration_level_t::UNDEFINED) { + bool was_stopped = false; + for (const auto &ifname : stop_interfaces) { + if (ifname.find(joint.name) != std::string::npos) { + was_stopped = true; + break; + } + } + if (was_stopped) { + joint.control_level = integration_level_t::UNDEFINED; + joint.joint_command_velocity = 0.0; + joint.joint_command_position = joint.joint_state_position; + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "Joint %s: stopped -> UNDEFINED", joint.name.c_str()); + } + } else { + joint.control_level = requested_modes[i]; + if (requested_modes[i] == integration_level_t::VELOCITY) { + joint.joint_command_velocity = 0.0; + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "Joint %s: switched to VELOCITY", joint.name.c_str()); + } else if (requested_modes[i] == integration_level_t::POSITION) { + joint.joint_command_position = joint.joint_state_position; + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "Joint %s: switched to POSITION (initialized to %f)", + joint.name.c_str(), joint.joint_command_position); + } + } + } + + 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) diff --git a/src/hardware_interfaces/killswitch_ros2_control/include/killswitch_ros2_control/killswitch_hardware_interface.hpp b/src/hardware_interfaces/killswitch_ros2_control/include/killswitch_ros2_control/killswitch_hardware_interface.hpp deleted file mode 100644 index 4636862b..00000000 --- a/src/hardware_interfaces/killswitch_ros2_control/include/killswitch_ros2_control/killswitch_hardware_interface.hpp +++ /dev/null @@ -1,131 +0,0 @@ -// Copyright (c) 2024 UMD Loop -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// - -#ifndef KILLSWITCH_ROS2_CONTROL__KILLSWITCH_HARDWARE_INTERFACE_HPP_ -#define KILLSWITCH_ROS2_CONTROL__KILLSWITCH_HARDWARE_INTERFACE_HPP_ - -#include -#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 killswitch_ros2_control -{ - -/** - * @brief Hardware interface for CAN-controlled killswitch via ros2_control - * - * This interface sends kill commands to the voltage monitoring board via CAN. - * - * CAN Protocol (ID: 0x100): - * - Kill All Power: DATA[0] = 0x01 - * - Kill Main Power: DATA[0] = 0x03 - * - Kill Jetson Power: DATA[0] = 0x05 - * - * State Interfaces (read by controllers): - * - kill_all_sent: 1.0 if kill all command was sent - * - kill_main_sent: 1.0 if kill main command was sent - * - kill_jetson_sent: 1.0 if kill jetson command was sent - * - is_connected: Is CAN connected (0.0 or 1.0) - * - * Command Interfaces (written by controllers): - * - kill_all: Set to 1.0 to kill all power - * - kill_main: Set to 1.0 to kill main power - * - kill_jetson: Set to 1.0 to kill jetson power - * - * Hardware Parameters (from URDF): - * - can_interface: CAN interface name (default: "can0") - * - can_id: CAN ID for killswitch commands (default: 0x100) - */ -class KillswitchHardwareInterface : public hardware_interface::SystemInterface -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(KillswitchHardwareInterface) - - hardware_interface::CallbackReturn on_init( - const hardware_interface::HardwareInfo & info) override; - - hardware_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - std::vector 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 (track what was sent) - double kill_all_sent_; - double kill_main_sent_; - double kill_jetson_sent_; - double is_connected_; - - // Command variables - double cmd_kill_all_; - double cmd_kill_main_; - double cmd_kill_jetson_; - - // CAN command bytes - static constexpr uint8_t CMD_KILL_ALL = 0x01; - static constexpr uint8_t CMD_KILL_MAIN = 0x03; - static constexpr uint8_t CMD_KILL_JETSON = 0x05; -}; - -} // namespace killswitch_ros2_control - -#endif // KILLSWITCH_ROS2_CONTROL__KILLSWITCH_HARDWARE_INTERFACE_HPP_ diff --git a/src/hardware_interfaces/killswitch_ros2_control/killswitch_hardware_interface.xml b/src/hardware_interfaces/killswitch_ros2_control/killswitch_hardware_interface.xml deleted file mode 100644 index 91620043..00000000 --- a/src/hardware_interfaces/killswitch_ros2_control/killswitch_hardware_interface.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - Killswitch hardware interface via ros2_control. - CAN-based power kill commands for main, jetson, and all systems. - - - diff --git a/src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp b/src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp deleted file mode 100644 index 3d335e8b..00000000 --- a/src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp +++ /dev/null @@ -1,286 +0,0 @@ -// 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 "killswitch_ros2_control/killswitch_hardware_interface.hpp" - -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace killswitch_ros2_control -{ - -hardware_interface::CallbackReturn KillswitchHardwareInterface::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 - kill_all_sent_ = 0.0; - kill_main_sent_ = 0.0; - kill_jetson_sent_ = 0.0; - is_connected_ = 0.0; - - // Initialize command variables - cmd_kill_all_ = 0.0; - cmd_kill_main_ = 0.0; - cmd_kill_jetson_ = 0.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")) { - // Parse hex string (e.g., "0x100") - base 0 auto-detects hex/decimal - can_id_ = static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); - } else { - can_id_ = 0x100; // Default killswitch CAN ID - } - - can_connected_ = false; - - RCLCPP_INFO( - rclcpp::get_logger("KillswitchHardwareInterface"), - "Initialized killswitch on CAN interface %s with ID 0x%X", - can_interface_.c_str(), can_id_); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -hardware_interface::CallbackReturn KillswitchHardwareInterface::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - RCLCPP_INFO( - rclcpp::get_logger("KillswitchHardwareInterface"), - "Configuring killswitch hardware..."); - - // Open CAN bus - if (!canBus_.open(can_interface_, - std::bind(&KillswitchHardwareInterface::onCanMessage, this, std::placeholders::_1))) - { - RCLCPP_WARN( - rclcpp::get_logger("KillswitchHardwareInterface"), - "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("KillswitchHardwareInterface"), - "Successfully opened CAN interface %s", can_interface_.c_str()); - } - - is_connected_ = can_connected_ ? 1.0 : 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("KillswitchHardwareInterface"), - "Killswitch hardware configured (%s)", can_connected_ ? "CAN MODE" : "SIMULATION"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -void KillswitchHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) -{ - // Handle incoming CAN messages from voltage monitoring board if needed - (void)frame; // Currently not expecting responses -} - -std::vector -KillswitchHardwareInterface::export_state_interfaces() -{ - std::vector state_interfaces; - - // Use the gpio name from URDF - const std::string& name = info_.gpios[0].name; - - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "kill_all_sent", &kill_all_sent_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "kill_main_sent", &kill_main_sent_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "kill_jetson_sent", &kill_jetson_sent_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "is_connected", &is_connected_)); - - RCLCPP_INFO( - rclcpp::get_logger("KillswitchHardwareInterface"), - "Exported %zu state interfaces", state_interfaces.size()); - - return state_interfaces; -} - -std::vector -KillswitchHardwareInterface::export_command_interfaces() -{ - std::vector command_interfaces; - - // Use the gpio name from URDF - const std::string& name = info_.gpios[0].name; - - command_interfaces.emplace_back( - hardware_interface::CommandInterface(name, "kill_all", &cmd_kill_all_)); - command_interfaces.emplace_back( - hardware_interface::CommandInterface(name, "kill_main", &cmd_kill_main_)); - command_interfaces.emplace_back( - hardware_interface::CommandInterface(name, "kill_jetson", &cmd_kill_jetson_)); - - RCLCPP_INFO( - rclcpp::get_logger("KillswitchHardwareInterface"), - "Exported %zu command interfaces", command_interfaces.size()); - - return command_interfaces; -} - -hardware_interface::CallbackReturn KillswitchHardwareInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - RCLCPP_INFO( - rclcpp::get_logger("KillswitchHardwareInterface"), - "Activating killswitch hardware..."); - - // Reset commands to safe state - cmd_kill_all_ = 0.0; - cmd_kill_main_ = 0.0; - cmd_kill_jetson_ = 0.0; - - return hardware_interface::CallbackReturn::SUCCESS; -} - -hardware_interface::CallbackReturn KillswitchHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - RCLCPP_INFO( - rclcpp::get_logger("KillswitchHardwareInterface"), - "Deactivating killswitch hardware..."); - - // Note: We intentionally do NOT send kill commands on deactivate - // Killswitch should only respond to explicit commands - - return hardware_interface::CallbackReturn::SUCCESS; -} - -hardware_interface::CallbackReturn KillswitchHardwareInterface::on_cleanup( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - RCLCPP_INFO( - rclcpp::get_logger("KillswitchHardwareInterface"), - "Cleaning up killswitch hardware..."); - - if (can_connected_) { - canBus_.close(); - } - - can_connected_ = false; - is_connected_ = 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("KillswitchHardwareInterface"), - "Killswitch hardware cleanup complete"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -hardware_interface::CallbackReturn KillswitchHardwareInterface::on_shutdown( - const rclcpp_lifecycle::State & previous_state) -{ - RCLCPP_INFO( - rclcpp::get_logger("KillswitchHardwareInterface"), - "Shutting down killswitch hardware..."); - - return on_cleanup(previous_state); -} - -hardware_interface::return_type KillswitchHardwareInterface::read( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) -{ - // CAN messages are handled asynchronously via callback - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type KillswitchHardwareInterface::write( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) -{ - // Check kill_all command (rising edge detection) - if (cmd_kill_all_ > 0.5 && kill_all_sent_ < 0.5) { - 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_KILL_ALL; - canBus_.send(can_tx_frame_); - } - - kill_all_sent_ = 1.0; - RCLCPP_ERROR( - rclcpp::get_logger("KillswitchHardwareInterface"), - "KILL ALL POWER COMMAND SENT%s", can_connected_ ? "" : " (simulated)"); - } else if (cmd_kill_all_ < 0.5) { - kill_all_sent_ = 0.0; // Reset when command goes low - } - - // Check kill_main command (rising edge detection) - if (cmd_kill_main_ > 0.5 && kill_main_sent_ < 0.5) { - 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_KILL_MAIN; - canBus_.send(can_tx_frame_); - } - - kill_main_sent_ = 1.0; - RCLCPP_WARN( - rclcpp::get_logger("KillswitchHardwareInterface"), - "KILL MAIN POWER COMMAND SENT%s", can_connected_ ? "" : " (simulated)"); - } else if (cmd_kill_main_ < 0.5) { - kill_main_sent_ = 0.0; // Reset when command goes low - } - - // Check kill_jetson command (rising edge detection) - if (cmd_kill_jetson_ > 0.5 && kill_jetson_sent_ < 0.5) { - 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_KILL_JETSON; - canBus_.send(can_tx_frame_); - } - - kill_jetson_sent_ = 1.0; - RCLCPP_WARN( - rclcpp::get_logger("KillswitchHardwareInterface"), - "KILL JETSON POWER COMMAND SENT%s", can_connected_ ? "" : " (simulated)"); - } else if (cmd_kill_jetson_ < 0.5) { - kill_jetson_sent_ = 0.0; // Reset when command goes low - } - - return hardware_interface::return_type::OK; -} - -} // namespace killswitch_ros2_control - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - killswitch_ros2_control::KillswitchHardwareInterface, - hardware_interface::SystemInterface) 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..a8e5d56b 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,26 +1,9 @@ -// 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_ #include #include #include -#include -#include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -29,36 +12,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] = 0x60 - * - OFF: DATA[0] = 0x80 - * - Read Temperature: DATA[0] = 0x85 - * - Read Wavelength: DATA[0] = 0x90 - * - * 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: @@ -66,60 +25,59 @@ class LaserHardwareInterface : public hardware_interface::SystemInterface hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; std::vector export_state_interfaces() override; - std::vector export_command_interfaces() override; hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - hardware_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - hardware_interface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; - hardware_interface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & previous_state) override; hardware_interface::return_type read( - const rclcpp::Time & time, - const rclcpp::Duration & period) override; - + const rclcpp::Time & time, const rclcpp::Duration & period) override; hardware_interface::return_type write( - const rclcpp::Time & time, - const rclcpp::Duration & period) override; + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + void logger_function(); private: - // CAN message handler - void onCanMessage(const CANLib::CanFrame& frame); + 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); - // Configuration parameters std::string can_interface_; - uint32_t can_id_; - - // CAN bus CANLib::SocketCanBus canBus_; CANLib::CanFrame can_tx_frame_; bool can_connected_; + std::vector LASERJoints_; + int update_rate_; + int logger_rate_; + int logger_state_; + double elapsed_time_; + double elapsed_logger_time_; - // 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; }; } // 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..7e3c528e 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,273 +1,233 @@ -// 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) { - if (hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) + 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; - is_connected_ = 0.0; - - // Initialize command variables - laser_command_ = 0.0; // OFF - - // 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")) { - // 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)); - } else { - can_id_ = 0x130; // Default laser CAN 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; + const uint32_t can_id = info_.hardware_parameters.count("can_id") ? + static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)) : 0x130; + + LASERJoints_.clear(); + LASERJoints_.push_back(LaserJoint{ + info_.gpios[0].name, + can_id, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + }); can_connected_ = false; - - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Initialized laser on CAN interface %s with ID 0x%X", - can_interface_.c_str(), can_id_); - + 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..."); - - // Open CAN bus - if (!canBus_.open(can_interface_, - std::bind(&LaserHardwareInterface::onCanMessage, this, std::placeholders::_1))) + 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", - can_interface_.c_str()); can_connected_ = false; } else { can_connected_ = true; - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Successfully opened CAN interface %s", can_interface_.c_str()); } - - is_connected_ = can_connected_ ? 1.0 : 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Laser hardware configured (%s)", can_connected_ ? "CAN MODE" : "SIMULATION"); - + auto & joint = LASERJoints_.front(); + joint.is_connected = can_connected_ ? 1.0 : 0.0; + joint.status = joint.is_connected; 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]); - } - } + auto & joint = LASERJoints_.front(); + if (frame.id == joint.can_id && frame.dlc > 1 && frame.data[0] == CMD_READ_TEMP) { + joint.temperature = static_cast(frame.data[1]); + joint.status = 1.0; } } -std::vector -LaserHardwareInterface::export_state_interfaces() +std::vector LaserHardwareInterface::export_state_interfaces() { + auto & joint = LASERJoints_.front(); std::vector state_interfaces; - - // Use the gpio name from URDF - 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_)); - - 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; - - // Use the gpio name from URDF - const std::string& name = info_.gpios[0].name; - - // Laser command (ON/OFF) - 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..."); - - // Safety: Turn laser OFF on deactivation + auto & joint = LASERJoints_.front(); if (can_connected_) { can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_id_; + can_tx_frame_.id = joint.can_id; can_tx_frame_.dlc = 1; can_tx_frame_.data[0] = CMD_LASER_OFF; canBus_.send(can_tx_frame_); - - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Laser turned OFF (safety)"); } - - laser_state_ = 0.0; - + joint.laser_state = 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..."); - - // Ensure laser is OFF before cleanup + auto & joint = LASERJoints_.front(); if (can_connected_) { can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_id_; + can_tx_frame_.id = joint.can_id; can_tx_frame_.dlc = 1; can_tx_frame_.data[0] = CMD_LASER_OFF; canBus_.send(can_tx_frame_); - canBus_.close(); } - can_connected_ = false; - is_connected_ = 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("LaserHardwareInterface"), - "Laser hardware cleanup complete"); - + joint.is_connected = 0.0; + joint.status = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } 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) { - // Check if laser command changed - 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(); + } + } + 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_.id = joint.can_id; can_tx_frame_.dlc = 1; can_tx_frame_.data[0] = commanded_on ? CMD_LASER_ON : CMD_LASER_OFF; canBus_.send(can_tx_frame_); } + joint.laser_state = commanded_on ? 1.0 : 0.0; + } - 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)"); + if (joint.status_request < 0.0 && joint.prev_status_request >= 0.0) { + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.can_id; + can_tx_frame_.dlc = 1; + can_tx_frame_.data[0] = CMD_READ_TEMP; + canBus_.send(can_tx_frame_); + } + } else if (joint.status_request > 0.0) { + joint.elapsed_status_request_time += period.seconds(); + if (joint.elapsed_status_request_time > (1.0 / joint.status_request)) { + joint.elapsed_status_request_time = 0.0; + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.can_id; + can_tx_frame_.dlc = 1; + can_tx_frame_.data[0] = CMD_READ_TEMP; + canBus_.send(can_tx_frame_); + } + } } + joint.prev_status_request = joint.status_request; 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) + laser_ros2_control::LaserHardwareInterface, hardware_interface::SystemInterface) diff --git a/src/hardware_interfaces/led_ros2_control/CMakeLists.txt b/src/hardware_interfaces/led_ros2_control/CMakeLists.txt index 01af46cc..82b121bd 100644 --- a/src/hardware_interfaces/led_ros2_control/CMakeLists.txt +++ b/src/hardware_interfaces/led_ros2_control/CMakeLists.txt @@ -10,6 +10,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib rclcpp rclcpp_lifecycle + umdloop_can_library ) # find dependencies @@ -41,6 +42,10 @@ ament_target_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} ) +# Link the library target explicitly to ensure its include dirs and +# compile definitions are available when building this target. +target_link_libraries(led_ros2_control PUBLIC umdloop_can_library::umdloop_can_library) + # Export hardware plugins pluginlib_export_plugin_description_file(hardware_interface led_hardware_interface.xml) diff --git a/src/hardware_interfaces/led_ros2_control/include/led_ros2_control/led_hardware_interface.hpp b/src/hardware_interfaces/led_ros2_control/include/led_ros2_control/led_hardware_interface.hpp index 046e01fb..780f1875 100644 --- a/src/hardware_interfaces/led_ros2_control/include/led_ros2_control/led_hardware_interface.hpp +++ b/src/hardware_interfaces/led_ros2_control/include/led_ros2_control/led_hardware_interface.hpp @@ -1,24 +1,12 @@ -// 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 LED_ROS2_CONTROL__LED_HARDWARE_INTERFACE_HPP_ #define LED_ROS2_CONTROL__LED_HARDWARE_INTERFACE_HPP_ #include #include #include +#include +#include +#include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -28,33 +16,17 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -namespace led_ros2_control -{ +#include "umdloop_can_library/SocketCanBus.hpp" +#include "umdloop_can_library/CanFrame.hpp" -// GPIO utility functions (Linux sysfs) -namespace gpio_utils +namespace CANLib { - int setup_gpio_output(int pin); - void cleanup_gpio(int pin, int fd); - bool write_gpio(int fd, bool value); + struct CanFrame; } -/** - * @brief Hardware interface for LED control via ros2_control - * - * This is a SystemInterface for controlling status LEDs through GPIO. - * - * State Interfaces (read by controllers): - * - led_state: Current LED state (0.0 = OFF, 1.0 = ON) - * - is_connected: Is hardware connected and ready (0.0 or 1.0) - * - * Command Interfaces (written by controllers): - * - led_command: LED command (0.0 = turn OFF, 1.0 = turn ON) - * - * Hardware Parameters (from URDF): - * - gpio_pin: GPIO pin number for LED output (required) - * - default_state: "on" or "off" (default: "off") - */ +namespace led_ros2_control +{ + class LEDHardwareInterface : public hardware_interface::SystemInterface { public: @@ -62,52 +34,77 @@ class LEDHardwareInterface : 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; - -private: - // Configuration parameters - int gpio_pin_; - bool default_state_; - - // State variables (hardware → ros2_control) - double led_state_; // Current state: 0.0 = OFF, 1.0 = ON - double is_connected_; // Hardware ready status + const rclcpp::Time & time, const rclcpp::Duration & period) override; - // Command variables (ros2_control → hardware) - double led_command_; // Commanded state + // -- Helper Functions -- + void send_command(int can_id, int cmd_id); + void logger_function(); - // Hardware interface - int gpio_fd_; - bool hw_connected_; +private: + struct LEDGPIO + { + std::string name; + bool is_rgb; + uint32_t node_id; + double status; + double intensity; + double red_command; + double green_command; + double blue_command; + double status_request; + double prev_status_request; + double elapsed_status_request_time; + double prev_intensity; + double prev_red_command; + double prev_green_command; + double prev_blue_command; + + std::vector state_interface_names; + std::vector command_interface_names; + std::unordered_map parameters; + }; + + std::string can_interface_; + CANLib::SocketCanBus canBus; + CANLib::CanFrame can_tx_frame_; + + // CAN bus ID (constant for this HWI) + uint32_t can_id_ = 0; + + std::vector LEDGPIOs_; + int update_rate_; + int logger_rate_; + int logger_state_; + double elapsed_time_; + double elapsed_logger_time_; + + // Multiplexor byte + static constexpr uint8_t CMD_INTENSITY = 0x20; + static constexpr uint8_t CMD_RGB = 0x30; + + // Parameter to confirm whether to send command or not via CAN + static constexpr uint8_t DECLINE_SEND = 0; + static constexpr uint8_t CONFIRM_SEND = 1; }; } // namespace led_ros2_control #endif // LED_ROS2_CONTROL__LED_HARDWARE_INTERFACE_HPP_ - diff --git a/src/hardware_interfaces/led_ros2_control/package.xml b/src/hardware_interfaces/led_ros2_control/package.xml index 10258b2e..f6920a4f 100644 --- a/src/hardware_interfaces/led_ros2_control/package.xml +++ b/src/hardware_interfaces/led_ros2_control/package.xml @@ -15,6 +15,10 @@ rclcpp rclcpp_lifecycle + + umdloop_can_library + umdloop_can_library + ament_lint_auto ament_lint_common diff --git a/src/hardware_interfaces/led_ros2_control/src/led_hardware_interface.cpp b/src/hardware_interfaces/led_ros2_control/src/led_hardware_interface.cpp index 2b2c0b4c..59741686 100644 --- a/src/hardware_interfaces/led_ros2_control/src/led_hardware_interface.cpp +++ b/src/hardware_interfaces/led_ros2_control/src/led_hardware_interface.cpp @@ -1,305 +1,310 @@ -// 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 "led_ros2_control/led_hardware_interface.hpp" #include #include #include -#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "pluginlib/class_list_macros.hpp" #include "rclcpp/rclcpp.hpp" +#include +#include +#include namespace led_ros2_control { -// GPIO utility functions implementation -namespace gpio_utils -{ - -int setup_gpio_output(int pin) -{ - // Export GPIO - int fd = ::open("/sys/class/gpio/export", O_WRONLY); - if (fd >= 0) { - std::string pin_str = std::to_string(pin); - ::write(fd, pin_str.c_str(), pin_str.length()); - ::close(fd); - usleep(100000); // Wait for GPIO to be exported - } - - // Set direction to output - std::stringstream direction_path; - direction_path << "/sys/class/gpio/gpio" << pin << "/direction"; - fd = ::open(direction_path.str().c_str(), O_WRONLY); - if (fd < 0) { - return -1; - } - ::write(fd, "out", 3); - ::close(fd); +void LEDHardwareInterface::send_command(int can_id, int cmd_id){ + CANLib::CanFrame frame; + frame.id = can_id; + frame.dlc = 2; + frame.data.fill(0); + frame.data[0] = cmd_id; + frame.data[1] = CONFIRM_SEND; - // Open value file - std::stringstream value_path; - value_path << "/sys/class/gpio/gpio" << pin << "/value"; - int value_fd = ::open(value_path.str().c_str(), O_RDWR); - - return value_fd; + canBus.send(frame); } -void cleanup_gpio(int pin, int fd) +void LEDHardwareInterface::logger_function() { - if (fd >= 0) { - ::close(fd); + if (LEDGPIOs_.empty()) { + return; } - // Unexport GPIO - int export_fd = ::open("/sys/class/gpio/unexport", O_WRONLY); - if (export_fd >= 0) { - std::string pin_str = std::to_string(pin); - ::write(export_fd, pin_str.c_str(), pin_str.length()); - ::close(export_fd); + for (const auto & gpio : LEDGPIOs_) { + std::ostringstream oss; + oss << "\033[2J\033[H \nLED Logger" + << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface_ + << " | CAN ID: 0x" << std::hex << std::uppercase << can_id_ << std::dec + << " | HWI Update Rate: " << update_rate_ + << " | Logger Update Rate: " << logger_rate_ << "\n" + << "Elapsed Time since first update: " << elapsed_time_ << "\n" + << "\n--- GPIO Specific ---\n" + << "GPIO: " << gpio.name << " | Node ID: 0x" << std::hex << std::uppercase << gpio.node_id << std::dec << "\n" + << "-- Commands --\n" + << "Red Command: " << gpio.red_command + << " | Green Command: " << gpio.green_command + << " | Blue Command: " << gpio.blue_command + << " | Status Request: " << gpio.status_request << "\n" + << "-- State --\n" + << "Status: " << gpio.status << "\n"; + + RCLCPP_INFO(rclcpp::get_logger("LEDHardwareInterface"), "%s", oss.str().c_str()); } } -bool write_gpio(int fd, bool value) -{ - if (fd < 0) return false; - - char val = value ? '1' : '0'; - lseek(fd, 0, SEEK_SET); - return (::write(fd, &val, 1) == 1); -} - -} // namespace gpio_utils - -// Hardware Interface Implementation - hardware_interface::CallbackReturn LEDHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) { - if (hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } - - // Parse hardware parameters - if (!info_.hardware_parameters.count("gpio_pin")) { - RCLCPP_ERROR( - rclcpp::get_logger("LEDHardwareInterface"), - "gpio_pin parameter is required"); - return hardware_interface::CallbackReturn::ERROR; - } - gpio_pin_ = std::stoi(info_.hardware_parameters.at("gpio_pin")); - - // Default state - if (info_.hardware_parameters.count("default_state")) { - default_state_ = (info_.hardware_parameters.at("default_state") == "on"); + + // General HWI parameters + 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")) : 5; + 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; + if (info_.hardware_parameters.count("can_id")) { + try { + can_id_ = static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); + } catch (const std::exception & e) { + RCLCPP_ERROR(rclcpp::get_logger("LEDHardwareInterface"), "Failed to parse 'can_id': %s", e.what()); + return hardware_interface::CallbackReturn::ERROR; + } } else { - default_state_ = false; // OFF by default + RCLCPP_ERROR(rclcpp::get_logger("LEDHardwareInterface"), "CAN ID parameter 'can_id' is missing in hardware configuration."); + return hardware_interface::CallbackReturn::ERROR; } - // Initialize state variables - led_state_ = default_state_ ? 1.0 : 0.0; - is_connected_ = 0.0; + for (auto& gpio : info_.gpios) { + // Collect state interface names + std::vector state_if_names; + for (const auto &si : gpio.state_interfaces) { + state_if_names.push_back(si.name); + } - // Initialize command variables - led_command_ = default_state_ ? 1.0 : 0.0; + // Collect command interface names + std::vector command_if_names; + for (const auto &ci : gpio.command_interfaces) { + command_if_names.push_back(ci.name); + } - gpio_fd_ = -1; - hw_connected_ = false; + // Copy parameters into an unordered_map + std::unordered_map params_map; + for (const auto &p : gpio.parameters) { + params_map.emplace(p.first, p.second); + } + // Determine whether this gpio is rgb from its parameters + bool is_rgb_flag = false; + if (params_map.count("is_rgb")) { + const std::string &v = params_map.at("is_rgb"); + is_rgb_flag = (v == "true" || v == "1"); + } + // parse per-gpio node_id if provided + uint32_t gpio_node_id = 0; + if (params_map.count("node_id")) { + try { + gpio_node_id = static_cast(std::stoul(params_map.at("node_id"))); + } catch (const std::exception & e) { + RCLCPP_WARN(rclcpp::get_logger("LEDHardwareInterface"), "Failed to parse gpio node_id for '%s': %s", gpio.name.c_str(), e.what()); + } + } - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "Initialized LED on GPIO pin %d (default: %s)", - gpio_pin_, default_state_ ? "ON" : "OFF"); + LEDGPIOs_.push_back( + LEDGPIO{ + .name = gpio.name, + .is_rgb = is_rgb_flag, + .node_id = gpio_node_id, + .status = 0.0, + .intensity = 0.0, + .red_command = 0.0, + .green_command = 0.0, + .blue_command = 0.0, + .status_request = 0.0, + .prev_status_request = 0.0, + .elapsed_status_request_time = 0.0, + .prev_intensity = std::numeric_limits::quiet_NaN(), + .prev_red_command = std::numeric_limits::quiet_NaN(), + .prev_green_command = std::numeric_limits::quiet_NaN(), + .prev_blue_command = std::numeric_limits::quiet_NaN(), + + .state_interface_names = state_if_names, + .command_interface_names = command_if_names, + .parameters = params_map + } + ); + } + elapsed_time_ = 0.0; + elapsed_logger_time_ = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn LEDHardwareInterface::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "Configuring LED hardware..."); - - // Setup GPIO output - gpio_fd_ = gpio_utils::setup_gpio_output(gpio_pin_); - if (gpio_fd_ < 0) { - RCLCPP_WARN( - rclcpp::get_logger("LEDHardwareInterface"), - "Failed to setup GPIO output on pin %d - running in SIMULATION mode", gpio_pin_); - hw_connected_ = false; // Mark as simulation mode - } else { - hw_connected_ = true; - // Set initial state on real hardware - gpio_utils::write_gpio(gpio_fd_, default_state_); - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "Successfully configured LED hardware (GPIO mode)"); - } - - is_connected_ = hw_connected_ ? 1.0 : 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "LED hardware configured (%s)", hw_connected_ ? "REAL GPIO" : "SIMULATION"); - return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -LEDHardwareInterface::export_state_interfaces() +std::vector LEDHardwareInterface::export_state_interfaces() { std::vector state_interfaces; - // Use the gpio name from URDF - const std::string& name = info_.gpios[0].name; - - // LED state - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "led_state", &led_state_)); - - // Connection status - state_interfaces.emplace_back( - hardware_interface::StateInterface(name, "is_connected", &is_connected_)); - - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "Exported %zu state interfaces", state_interfaces.size()); - + for (auto & gpio : LEDGPIOs_) { + for (auto & iface : gpio.state_interface_names) { + double * val = nullptr; + if (iface == "status") { + val = &gpio.status; + } else { + RCLCPP_WARN( + rclcpp::get_logger("LEDHardwareInterface"), + "Unknown state interface '%s' for GPIO '%s'", + iface.c_str(), gpio.name.c_str()); + continue; + } + state_interfaces.emplace_back(hardware_interface::StateInterface( + gpio.name, iface, val)); + } + } return state_interfaces; } -std::vector -LEDHardwareInterface::export_command_interfaces() +std::vector LEDHardwareInterface::export_command_interfaces() { std::vector command_interfaces; - // Use the gpio name from URDF - const std::string& name = info_.gpios[0].name; - - // LED command - command_interfaces.emplace_back( - hardware_interface::CommandInterface(name, "led_command", &led_command_)); - - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "Exported %zu command interfaces", command_interfaces.size()); + for (auto & gpio : LEDGPIOs_) { + for (auto & iface : gpio.command_interface_names) { + double * val = nullptr; + // Only export RGB interfaces when is_rgb is true; otherwise export intensity + if (gpio.is_rgb) { + if (iface == "red") { + val = &gpio.red_command; + } else if (iface == "green") { + val = &gpio.green_command; + } else if (iface == "blue") { + val = &gpio.blue_command; + } else if (iface == "status_request") { + val = &gpio.status_request; + } else { + // skip intensity when rgb mode + continue; + } + } else { + if (iface == "intensity") { + val = &gpio.intensity; + } else if (iface == "status_request") { + val = &gpio.status_request; + } else { + // skip rgb channels when not rgb + continue; + } + } + command_interfaces.emplace_back(hardware_interface::CommandInterface( + gpio.name, iface, val)); + } + } return command_interfaces; } hardware_interface::CallbackReturn LEDHardwareInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "Activating LED hardware..."); - return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn LEDHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "Deactivating LED hardware..."); - - // Turn LED off on deactivation (safety) - if (hw_connected_ && gpio_fd_ >= 0) { - gpio_utils::write_gpio(gpio_fd_, false); - led_state_ = 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "LED turned OFF (deactivation)"); - } - return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn LEDHardwareInterface::on_cleanup( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "Cleaning up LED hardware..."); - - // Ensure LED is OFF before cleanup - if (gpio_fd_ >= 0) { - gpio_utils::write_gpio(gpio_fd_, false); - } - - // Cleanup GPIO - unexport and close file descriptor - if (gpio_fd_ >= 0) { - gpio_utils::cleanup_gpio(gpio_pin_, gpio_fd_); - gpio_fd_ = -1; + for (auto & gpio : LEDGPIOs_) { + gpio.status = 0.0; } - - hw_connected_ = false; - is_connected_ = 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "LED hardware cleanup complete"); - return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn LEDHardwareInterface::on_shutdown( const rclcpp_lifecycle::State & previous_state) { - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "Shutting down LED hardware..."); - - // Delegate to cleanup to release resources return on_cleanup(previous_state); } hardware_interface::return_type LEDHardwareInterface::read( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) + const rclcpp::Time &, const rclcpp::Duration &) { - // LED state is known from commands, no need to read from GPIO return hardware_interface::return_type::OK; } hardware_interface::return_type LEDHardwareInterface::write( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) + const rclcpp::Time &, const rclcpp::Duration & period) { - // Check if LED command changed - bool commanded_on = (led_command_ > 0.5); - bool currently_on = (led_state_ > 0.5); - - if (commanded_on != currently_on) { - // Write to real GPIO if available, otherwise simulate - if (hw_connected_ && gpio_fd_ >= 0) { - gpio_utils::write_gpio(gpio_fd_, commanded_on); + 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(); + } + } + + for (auto & gpio : LEDGPIOs_) { + if (gpio.status_request < 0.0 && gpio.prev_status_request >= 0.0) { + } else if (gpio.status_request > 0.0) { + gpio.elapsed_status_request_time += period.seconds(); + if (gpio.elapsed_status_request_time > (1.0 / gpio.status_request)) { + gpio.elapsed_status_request_time = 0.0; + } + } + gpio.prev_status_request = gpio.status_request; + // Build and send CAN frames for LED commands + CANLib::CanFrame frame; + frame.id = can_id_; + frame.dlc = 8; + frame.data.fill(0); + + if (gpio.is_rgb) { + // RGB mode: send when any channel changed + const double r = gpio.red_command; + const double g = gpio.green_command; + const double b = gpio.blue_command; + const bool changed = (std::isnan(gpio.prev_red_command) || r != gpio.prev_red_command) || + (std::isnan(gpio.prev_green_command) || g != gpio.prev_green_command) || + (std::isnan(gpio.prev_blue_command) || b != gpio.prev_blue_command); + if (changed) { + frame.data[0] = static_cast(CMD_RGB + static_cast(gpio.node_id & 0xFF)); + frame.data[1] = std::clamp(static_cast(std::round(r)), 0, 255); + frame.data[2] = std::clamp(static_cast(std::round(g)), 0, 255); + frame.data[3] = std::clamp(static_cast(std::round(b)), 0, 255); + canBus.send(frame); + gpio.prev_red_command = r; + gpio.prev_green_command = g; + gpio.prev_blue_command = b; + RCLCPP_INFO(rclcpp::get_logger("LEDHardwareInterface"), "Sent RGB frame to node 0x%X (CAN ID 0x%X)", gpio.node_id, can_id_); + } + } else { + const double intensity = gpio.intensity; + const bool changed = std::isnan(gpio.prev_intensity) || intensity != gpio.prev_intensity; + if (changed) { + frame.data[0] = static_cast(CMD_INTENSITY + static_cast(gpio.node_id & 0xFF)); + frame.data[1] = std::clamp(static_cast(std::round(intensity)), 0, 255); + canBus.send(frame); + gpio.prev_intensity = intensity; + RCLCPP_INFO(rclcpp::get_logger("LEDHardwareInterface"), "Sent intensity frame to node 0x%X (CAN ID 0x%X val=%d)", gpio.node_id, can_id_, frame.data[1]); + } } - - led_state_ = commanded_on ? 1.0 : 0.0; - - RCLCPP_DEBUG( - rclcpp::get_logger("LEDHardwareInterface"), - "LED turned %s%s", commanded_on ? "ON" : "OFF", - hw_connected_ ? "" : " (simulated)"); } return hardware_interface::return_type::OK; @@ -307,9 +312,5 @@ hardware_interface::return_type LEDHardwareInterface::write( } // namespace led_ros2_control -#include "pluginlib/class_list_macros.hpp" - PLUGINLIB_EXPORT_CLASS( - led_ros2_control::LEDHardwareInterface, - hardware_interface::SystemInterface) - + led_ros2_control::LEDHardwareInterface, hardware_interface::SystemInterface) diff --git a/src/hardware_interfaces/limit_switch_ros2_control/CMakeLists.txt b/src/hardware_interfaces/limit_switch_ros2_control/CMakeLists.txt new file mode 100644 index 00000000..fa7913cd --- /dev/null +++ b/src/hardware_interfaces/limit_switch_ros2_control/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.8) +project(limit_switch_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) +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() + +add_library( + limit_switch_ros2_control + SHARED + src/limit_switch_hardware_interface.cpp +) + +target_compile_features(limit_switch_ros2_control PUBLIC cxx_std_20) +target_include_directories(limit_switch_ros2_control PUBLIC + $ + $ +) + +ament_target_dependencies(limit_switch_ros2_control PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(limit_switch_ros2_control PUBLIC umdloop_can_library::umdloop_can_library) + +pluginlib_export_plugin_description_file(hardware_interface limit_switch_hardware_interface.xml) + +install( + DIRECTORY include/ + DESTINATION include/limit_switch_ros2_control +) + +install(TARGETS limit_switch_ros2_control + EXPORT export_limit_switch_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_limit_switch_ros2_control) +ament_export_include_directories(include) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/hardware_interfaces/limit_switch_ros2_control/include/limit_switch_ros2_control/limit_switch_hardware_interface.hpp b/src/hardware_interfaces/limit_switch_ros2_control/include/limit_switch_ros2_control/limit_switch_hardware_interface.hpp new file mode 100644 index 00000000..5610c8cd --- /dev/null +++ b/src/hardware_interfaces/limit_switch_ros2_control/include/limit_switch_ros2_control/limit_switch_hardware_interface.hpp @@ -0,0 +1,95 @@ +#ifndef LIMIT_SWITCH_ROS2_CONTROL__LIMIT_SWITCH_HARDWARE_INTERFACE_HPP_ +#define LIMIT_SWITCH_ROS2_CONTROL__LIMIT_SWITCH_HARDWARE_INTERFACE_HPP_ + +#include +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "umdloop_can_library/CanFrame.hpp" +#include "umdloop_can_library/SocketCanBus.hpp" + +namespace CANLib +{ +struct CanFrame; +} + +namespace limit_switch_ros2_control +{ + +class LimitSwitchHardwareInterface : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(LimitSwitchHardwareInterface) + + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + void on_can_message(const CANLib::CanFrame & frame); + void logger_function(); + +private: + struct LimitSwitchGPIO + { + std::string name; + uint32_t node_id; + double status; + double status_request; + double prev_status_request; + double elapsed_status_request_time; + + std::vector state_interface_names; + std::vector command_interface_names; + std::unordered_map parameters; + }; + + void send_status_request(const LimitSwitchGPIO & gpio); + + std::string can_interface_; + CANLib::SocketCanBus canBus; + CANLib::CanFrame can_rx_frame_; + + uint32_t can_write_id_ = 0; + uint32_t can_read_id_ = 0; + int update_rate_ = 0; + int logger_rate_ = 0; + int logger_state_ = 0; + double elapsed_time_ = 0.0; + double elapsed_logger_time_ = 0.0; + + std::vector limit_switches_; + + static constexpr uint8_t LIMIT_SWITCH_STATUS_CMD = 0x20; +}; + +} // namespace limit_switch_ros2_control + +#endif // LIMIT_SWITCH_ROS2_CONTROL__LIMIT_SWITCH_HARDWARE_INTERFACE_HPP_ diff --git a/src/hardware_interfaces/limit_switch_ros2_control/limit_switch_hardware_interface.xml b/src/hardware_interfaces/limit_switch_ros2_control/limit_switch_hardware_interface.xml new file mode 100644 index 00000000..8e2e53c2 --- /dev/null +++ b/src/hardware_interfaces/limit_switch_ros2_control/limit_switch_hardware_interface.xml @@ -0,0 +1,9 @@ + + + + Limit switch hardware interface via ros2_control GPIO command/state interfaces. + + + diff --git a/src/hardware_interfaces/limit_switch_ros2_control/package.xml b/src/hardware_interfaces/limit_switch_ros2_control/package.xml new file mode 100644 index 00000000..98562eac --- /dev/null +++ b/src/hardware_interfaces/limit_switch_ros2_control/package.xml @@ -0,0 +1,29 @@ + + + + limit_switch_ros2_control + 0.1.0 + Limit switch hardware interface for ros2_control + Ishan Dutta + Apache-2.0 + + ament_cmake + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + umdloop_can_library + umdloop_can_library + + ament_lint_auto + ament_lint_common + + ament_cmake + + + ament_cmake + + diff --git a/src/hardware_interfaces/limit_switch_ros2_control/src/limit_switch_hardware_interface.cpp b/src/hardware_interfaces/limit_switch_ros2_control/src/limit_switch_hardware_interface.cpp new file mode 100644 index 00000000..c6b4b8af --- /dev/null +++ b/src/hardware_interfaces/limit_switch_ros2_control/src/limit_switch_hardware_interface.cpp @@ -0,0 +1,260 @@ +#include "limit_switch_ros2_control/limit_switch_hardware_interface.hpp" + +#include + +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace limit_switch_ros2_control +{ + +void LimitSwitchHardwareInterface::send_status_request(const LimitSwitchGPIO & gpio) +{ + CANLib::CanFrame frame; + frame.id = can_write_id_; + frame.dlc = 8; + frame.data.fill(0x00); + frame.data[0] = static_cast( + LIMIT_SWITCH_STATUS_CMD + static_cast(gpio.node_id & 0xFF)); + canBus.send(frame); +} + +void LimitSwitchHardwareInterface::logger_function() +{ + if (limit_switches_.empty()) { + return; + } + + std::ostringstream oss; + oss << "\033[2J\033[H \nLimit Switch Logger" + << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface_ + << " | Write CAN ID: 0x" << std::hex << std::uppercase << can_write_id_ + << " | Read CAN ID: 0x" << can_read_id_ << std::dec + << " | HWI Update Rate: " << update_rate_ + << " | Logger Update Rate: " << logger_rate_ << "\n" + << "Elapsed Time since first update: " << elapsed_time_ << "\n" + << "\n--- GPIO Specific ---"; + + for (const auto & gpio : limit_switches_) { + oss << "\nGPIO: " << gpio.name + << " | Node ID: 0x" << std::hex << std::uppercase << gpio.node_id << std::dec + << "\n-- Commands --\n" + << "Status Request: " << gpio.status_request + << "\n-- State --\n" + << "Status: " << gpio.status << "\n"; + } + + RCLCPP_INFO(rclcpp::get_logger("LimitSwitchHardwareInterface"), "%s", oss.str().c_str()); +} + +hardware_interface::CallbackReturn LimitSwitchHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + can_interface_ = info_.hardware_parameters.count("can_interface") ? + info_.hardware_parameters.at("can_interface") : "can0"; + update_rate_ = info_.hardware_parameters.count("update_rate") ? + std::stoi(info_.hardware_parameters.at("update_rate")) : 10; + logger_rate_ = info_.hardware_parameters.count("logger_rate") ? + std::stoi(info_.hardware_parameters.at("logger_rate")) : 0; + logger_state_ = info_.hardware_parameters.count("logger_state") ? + std::stoi(info_.hardware_parameters.at("logger_state")) : 0; + + if (!info_.hardware_parameters.count("can_id")) { + RCLCPP_ERROR( + rclcpp::get_logger("LimitSwitchHardwareInterface"), + "CAN ID parameter 'can_id' is missing in hardware configuration."); + return hardware_interface::CallbackReturn::ERROR; + } + + try { + can_write_id_ = static_cast( + std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); + } catch (const std::exception & e) { + RCLCPP_ERROR( + rclcpp::get_logger("LimitSwitchHardwareInterface"), + "Failed to parse 'can_id': %s", e.what()); + return hardware_interface::CallbackReturn::ERROR; + } + can_read_id_ = can_write_id_ + 1; + + limit_switches_.clear(); + for (const auto & gpio : info_.gpios) { + std::vector state_if_names; + for (const auto & si : gpio.state_interfaces) { + state_if_names.push_back(si.name); + } + + std::vector command_if_names; + for (const auto & ci : gpio.command_interfaces) { + command_if_names.push_back(ci.name); + } + + std::unordered_map params_map; + for (const auto & p : gpio.parameters) { + params_map.emplace(p.first, p.second); + } + + uint32_t node_id = 0; + if (params_map.count("node_id")) { + node_id = static_cast(std::stoul(params_map.at("node_id"), nullptr, 0)); + } + + limit_switches_.push_back(LimitSwitchGPIO{ + gpio.name, + node_id, + 0.0, + 0.0, + 0.0, + 0.0, + state_if_names, + command_if_names, + params_map}); + } + + elapsed_time_ = 0.0; + elapsed_logger_time_ = 0.0; + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LimitSwitchHardwareInterface::on_configure( + const rclcpp_lifecycle::State &) +{ + if (!canBus.open( + can_interface_, + std::bind(&LimitSwitchHardwareInterface::on_can_message, this, std::placeholders::_1))) + { + RCLCPP_ERROR(rclcpp::get_logger("LimitSwitchHardwareInterface"), "Failed to open CAN interface"); + return hardware_interface::CallbackReturn::ERROR; + } + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector +LimitSwitchHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + for (auto & gpio : limit_switches_) { + for (const auto & iface : gpio.state_interface_names) { + if (iface == "status") { + state_interfaces.emplace_back(gpio.name, iface, &gpio.status); + } else { + RCLCPP_WARN( + rclcpp::get_logger("LimitSwitchHardwareInterface"), + "Unknown state interface '%s' for GPIO '%s'", + iface.c_str(), gpio.name.c_str()); + } + } + } + return state_interfaces; +} + +std::vector +LimitSwitchHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + for (auto & gpio : limit_switches_) { + for (const auto & iface : gpio.command_interface_names) { + if (iface == "status_request") { + command_interfaces.emplace_back(gpio.name, iface, &gpio.status_request); + } else { + RCLCPP_WARN( + rclcpp::get_logger("LimitSwitchHardwareInterface"), + "Unknown command interface '%s' for GPIO '%s'", + iface.c_str(), gpio.name.c_str()); + } + } + } + return command_interfaces; +} + +hardware_interface::CallbackReturn LimitSwitchHardwareInterface::on_activate( + const rclcpp_lifecycle::State &) +{ + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LimitSwitchHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State &) +{ + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LimitSwitchHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State &) +{ + for (auto & gpio : limit_switches_) { + gpio.status = 0.0; + } + canBus.close(); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LimitSwitchHardwareInterface::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + return on_cleanup(previous_state); +} + +void LimitSwitchHardwareInterface::on_can_message(const CANLib::CanFrame & frame) +{ + can_rx_frame_ = frame; + if (can_rx_frame_.id != can_read_id_) { + return; + } + + for (auto & gpio : limit_switches_) { + const uint8_t expected_command = static_cast( + LIMIT_SWITCH_STATUS_CMD + static_cast(gpio.node_id & 0xFF)); + if (can_rx_frame_.data[0] == expected_command) { + gpio.status = static_cast(can_rx_frame_.data[1]); + return; + } + } +} + +hardware_interface::return_type LimitSwitchHardwareInterface::read( + const rclcpp::Time &, const rclcpp::Duration &) +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type LimitSwitchHardwareInterface::write( + const rclcpp::Time &, const rclcpp::Duration & period) +{ + 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(); + } + } + + for (auto & gpio : limit_switches_) { + const double curr_status_req = gpio.status_request; + if (curr_status_req < 0.0 && gpio.prev_status_request >= 0.0) { + send_status_request(gpio); + } else if (curr_status_req > 0.0) { + gpio.elapsed_status_request_time += period.seconds(); + if (gpio.elapsed_status_request_time > (1.0 / curr_status_req)) { + gpio.elapsed_status_request_time = 0.0; + send_status_request(gpio); + } + } + gpio.prev_status_request = curr_status_req; + } + + return hardware_interface::return_type::OK; +} + +} // namespace limit_switch_ros2_control + +PLUGINLIB_EXPORT_CLASS( + limit_switch_ros2_control::LimitSwitchHardwareInterface, hardware_interface::SystemInterface) diff --git a/src/hardware_interfaces/killswitch_ros2_control/CMakeLists.txt b/src/hardware_interfaces/power_module_ros2_control/CMakeLists.txt similarity index 63% rename from src/hardware_interfaces/killswitch_ros2_control/CMakeLists.txt rename to src/hardware_interfaces/power_module_ros2_control/CMakeLists.txt index 4a42db5e..5a14692c 100644 --- a/src/hardware_interfaces/killswitch_ros2_control/CMakeLists.txt +++ b/src/hardware_interfaces/power_module_ros2_control/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(killswitch_ros2_control) +project(power_module_ros2_control) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -26,22 +26,22 @@ ament_auto_find_build_dependencies() include_directories(include) add_library( - killswitch_ros2_control + power_module_ros2_control SHARED - src/killswitch_hardware_interface.cpp + src/power_module_hardware_interface.cpp ) -target_compile_features(killswitch_ros2_control PUBLIC cxx_std_20) -target_include_directories(killswitch_ros2_control PUBLIC +target_compile_features(power_module_ros2_control PUBLIC cxx_std_20) +target_include_directories(power_module_ros2_control PUBLIC $ -$ +$ ) # Link CAN library -target_link_libraries(killswitch_ros2_control PUBLIC umdloop_can_library::umdloop_can_library) +target_link_libraries(power_module_ros2_control PUBLIC umdloop_can_library::umdloop_can_library) ament_target_dependencies( - killswitch_ros2_control PUBLIC + power_module_ros2_control PUBLIC hardware_interface pluginlib rclcpp @@ -49,16 +49,16 @@ ament_target_dependencies( ) # Export hardware plugins -pluginlib_export_plugin_description_file(hardware_interface killswitch_hardware_interface.xml) +pluginlib_export_plugin_description_file(hardware_interface power_module_hardware_interface.xml) # INSTALL install( DIRECTORY include/ - DESTINATION include/killswitch_ros2_control + DESTINATION include/power_module_ros2_control ) -install(TARGETS killswitch_ros2_control - EXPORT export_killswitch_ros2_control +install(TARGETS power_module_ros2_control + EXPORT export_power_module_ros2_control ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -71,7 +71,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_export_targets(export_killswitch_ros2_control) +ament_export_targets(export_power_module_ros2_control) ament_export_include_directories(include) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/src/hardware_interfaces/power_module_ros2_control/include/power_module_ros2_control/power_module_hardware_interface.hpp b/src/hardware_interfaces/power_module_ros2_control/include/power_module_ros2_control/power_module_hardware_interface.hpp new file mode 100644 index 00000000..d5398e09 --- /dev/null +++ b/src/hardware_interfaces/power_module_ros2_control/include/power_module_ros2_control/power_module_hardware_interface.hpp @@ -0,0 +1,106 @@ +#ifndef POWER_MODULE_ROS2_CONTROL__POWER_MODULE_HARDWARE_INTERFACE_HPP_ +#define POWER_MODULE_ROS2_CONTROL__POWER_MODULE_HARDWARE_INTERFACE_HPP_ + +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "umdloop_can_library/CanFrame.hpp" +#include "umdloop_can_library/SocketCanBus.hpp" + +namespace CANLib +{ + struct CanFrame; +} + +namespace power_module_ros2_control +{ + +class PowerModuleHardwareInterface : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(PowerModuleHardwareInterface) + + 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; + + // -- Helper Functions -- + void on_can_message(const CANLib::CanFrame & frame); + void send_command(int can_id, int cmd_id); + void logger_function(); + +private: + struct PowerModuleGPIO + { + std::string name; + uint32_t can_id; + double kill_all_sent; + double kill_jetson_sent; + double kill_main_sent; + double kill_by_voltage_sent; + double status; + double cmd_kill_all; + double cmd_kill_jetson; + double cmd_kill_main; + double cmd_kill_by_voltage; + double status_request; + double prev_status_request; + double elapsed_status_request_time; + + std::vector state_interface_names; + std::vector command_interface_names; + std::unordered_map parameters; + }; + + std::string can_interface_; + uint32_t can_id; + CANLib::SocketCanBus canBus; + CANLib::CanFrame can_tx_frame_; + std::vector PowerModuleGPIOs_; + int update_rate_; + int logger_rate_; + int logger_state_; + double elapsed_time_; + double elapsed_logger_time_; + + // Multiplexor byte + static constexpr uint8_t CMD_KILL_ALL = 0x12; + static constexpr uint8_t CMD_KILL_JETSON = 0x13; + static constexpr uint8_t CMD_KILL_MAIN = 0x14; + static constexpr uint8_t CMD_KILL_BY_VOLTAGE = 0x15; + + // Parameter to confirm whether to send command or not via CAN + static constexpr uint8_t DECLINE_SEND = 0; + static constexpr uint8_t CONFIRM_SEND = 1; +}; + +} // namespace power_module_ros2_control + +#endif // POWER_MODULE_ROS2_CONTROL__POWER_MODULE_HARDWARE_INTERFACE_HPP_ diff --git a/src/hardware_interfaces/killswitch_ros2_control/package.xml b/src/hardware_interfaces/power_module_ros2_control/package.xml similarity index 85% rename from src/hardware_interfaces/killswitch_ros2_control/package.xml rename to src/hardware_interfaces/power_module_ros2_control/package.xml index afa1bc26..84fe4330 100644 --- a/src/hardware_interfaces/killswitch_ros2_control/package.xml +++ b/src/hardware_interfaces/power_module_ros2_control/package.xml @@ -1,9 +1,9 @@ - killswitch_ros2_control + power_module_ros2_control 0.1.0 - Killswitch hardware interface for ros2_control - CAN-based power killswitch system + PowerModule hardware interface for ros2_control - CAN-based power power_module system Dwarakesh Baraneetharan Apache-2.0 diff --git a/src/hardware_interfaces/power_module_ros2_control/power_module_hardware_interface.xml b/src/hardware_interfaces/power_module_ros2_control/power_module_hardware_interface.xml new file mode 100644 index 00000000..552a1338 --- /dev/null +++ b/src/hardware_interfaces/power_module_ros2_control/power_module_hardware_interface.xml @@ -0,0 +1,10 @@ + + + + PowerModule hardware interface via ros2_control. + CAN-based power kill commands for main, jetson, and all systems. + + + diff --git a/src/hardware_interfaces/power_module_ros2_control/src/power_module_hardware_interface.cpp b/src/hardware_interfaces/power_module_ros2_control/src/power_module_hardware_interface.cpp new file mode 100644 index 00000000..09940f14 --- /dev/null +++ b/src/hardware_interfaces/power_module_ros2_control/src/power_module_hardware_interface.cpp @@ -0,0 +1,304 @@ +#include "power_module_ros2_control/power_module_hardware_interface.hpp" + +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/rclcpp.hpp" + +#include + +namespace power_module_ros2_control +{ + +void PowerModuleHardwareInterface::send_command(int can_id, int cmd_id){ + CANLib::CanFrame frame; + frame.id = can_id; + frame.dlc = 2; + frame.data.fill(0); + frame.data[0] = cmd_id; + frame.data[1] = CONFIRM_SEND; + + canBus.send(frame); +} + +void PowerModuleHardwareInterface::logger_function() +{ + if (PowerModuleGPIOs_.empty()) { + return; + } + + const auto & gpio = PowerModuleGPIOs_.front(); + std::ostringstream oss; + oss << "\033[2J\033[H \nPowerModule 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--- GPIO Specific ---\n" + << "GPIO: " << gpio.name << "\n" + << "Parameters: CAN ID: 0x" << std::hex << std::uppercase << gpio.can_id << std::dec << "\n" + << "-- Commands --\n" + << "Kill All: " << gpio.cmd_kill_all + << " | Kill Main: " << gpio.cmd_kill_main + << " | Kill Jetson: " << gpio.cmd_kill_jetson + << " | Kill By Voltage: " << gpio.cmd_kill_by_voltage << "\n" + << "Status Request: " << gpio.status_request << "\n" + << "-- State --\n" + << "Kill All Sent: " << gpio.kill_all_sent + << " | Kill Jetson Sent: " << gpio.kill_jetson_sent + << " | Kill Main Sent: " << gpio.kill_main_sent + << " | Kill By Voltage Sent: " << gpio.kill_by_voltage_sent << "\n" + << " | Status: " << gpio.status << "\n"; + + RCLCPP_INFO(rclcpp::get_logger("PowerModuleHardwareInterface"), "%s", oss.str().c_str()); +} + +hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // General HWI parameters + 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")) : 5; + 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; + if(info_.hardware_parameters.count("can_id")) { + try { + can_id = static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); + } catch (const std::exception & e) { + RCLCPP_ERROR(rclcpp::get_logger("PowerModuleHardwareInterface"), "Failed to parse 'can_id': %s", e.what()); + return hardware_interface::CallbackReturn::ERROR; + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("PowerModuleHardwareInterface"), "CAN ID parameter 'can_id' is missing in hardware configuration."); + return hardware_interface::CallbackReturn::ERROR; + } + + for (auto& gpio : info_.gpios) { + // Collect state interface names + std::vector state_if_names; + for (const auto &si : gpio.state_interfaces) { + state_if_names.push_back(si.name); + } + + // Collect command interface names + std::vector command_if_names; + for (const auto &ci : gpio.command_interfaces) { + command_if_names.push_back(ci.name); + } + + // Copy parameters into an unordered_map + std::unordered_map params_map; + for (const auto &p : gpio.parameters) { + params_map.emplace(p.first, p.second); + } + + PowerModuleGPIOs_.clear(); + PowerModuleGPIOs_.push_back( + PowerModuleGPIO{ + .name = info_.gpios[0].name, + .can_id = can_id, + .kill_all_sent = 0.0, + .kill_jetson_sent = 0.0, + .kill_main_sent = 0.0, + .kill_by_voltage_sent = 0.0, + .status = 0.0, + .cmd_kill_all = 0.0, + .cmd_kill_jetson = 0.0, + .cmd_kill_main = 0.0, + .cmd_kill_by_voltage = 0.0, + .status_request = 0.0, + .prev_status_request = 0.0, + .elapsed_status_request_time = 0.0, + + .state_interface_names = state_if_names, + .command_interface_names = command_if_names, + .parameters = params_map + } + ); + } + + elapsed_time_ = 0.0; + elapsed_logger_time_ = 0.0; + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_configure( + const rclcpp_lifecycle::State &) +{ + if (!canBus.open( + can_interface_, + std::bind(&PowerModuleHardwareInterface::on_can_message, this, std::placeholders::_1))) + { + RCLCPP_ERROR(rclcpp::get_logger("PowerModuleHardwareInterface"), "Failed to open CAN interface"); + return hardware_interface::CallbackReturn::ERROR; + } + return hardware_interface::CallbackReturn::SUCCESS; +} + +void PowerModuleHardwareInterface::on_can_message(const CANLib::CanFrame &) +{ +} + +std::vector PowerModuleHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + for (auto & gpio : PowerModuleGPIOs_) { + for (auto & iface : gpio.state_interface_names) { + double * val = nullptr; + if (iface == "status") { + val = &gpio.status; + } else if (iface == "kill_all_sent") { + val = &gpio.kill_all_sent; + } else if (iface == "kill_jetson_sent") { + val = &gpio.kill_jetson_sent; + } else if (iface == "kill_main_sent") { + val = &gpio.kill_main_sent; + } else if (iface == "kill_by_voltage_sent") { + val = &gpio.kill_by_voltage_sent; + } else { + RCLCPP_WARN( + rclcpp::get_logger("PowerModuleHardwareInterface"), + "Unknown state interface '%s' for gpio '%s'", + iface.c_str(), gpio.name.c_str()); + continue; + } + state_interfaces.emplace_back(hardware_interface::StateInterface( + gpio.name, iface, val)); + } + } + return state_interfaces; +} + +std::vector PowerModuleHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + + for (auto & gpio : PowerModuleGPIOs_) { + for (auto & iface : gpio.command_interface_names) { + double * val = nullptr; + if (iface == "kill_all") { + val = &gpio.cmd_kill_all; + } else if (iface == "kill_jetson") { + val = &gpio.cmd_kill_jetson; + } else if (iface == "kill_main") { + val = &gpio.cmd_kill_main; + } else if (iface == "kill_by_voltage") { + val = &gpio.cmd_kill_by_voltage; + } else if (iface == "status_request") { + val = &gpio.status_request; + } else { + RCLCPP_WARN( + rclcpp::get_logger("PowerModuleHardwareInterface"), + "Unknown command interface '%s' for gpio '%s'", + iface.c_str(), gpio.name.c_str()); + continue; + } + command_interfaces.emplace_back(hardware_interface::CommandInterface( + gpio.name, iface, val)); + } + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_activate( + const rclcpp_lifecycle::State &) +{ + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State &) +{ + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State &) +{ + canBus.close(); + for (auto & gpio : PowerModuleGPIOs_) { + gpio.status = 0.0; + } + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + return on_cleanup(previous_state); +} + +hardware_interface::return_type PowerModuleHardwareInterface::read( + const rclcpp::Time &, const rclcpp::Duration &) +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type PowerModuleHardwareInterface::write( + const rclcpp::Time &, const rclcpp::Duration & period) +{ + 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(); + } + } + + for (auto & gpio : PowerModuleGPIOs_) { + if (gpio.cmd_kill_all > 0.5 && gpio.kill_all_sent < 0.5) { + send_command(gpio.can_id, CMD_KILL_ALL); + RCLCPP_INFO(rclcpp::get_logger("PowerModuleHardwareInterface"), "Sent Kill All Command to CAN ID 0x%X", gpio.can_id); + gpio.kill_all_sent = 1.0; + } else if (gpio.cmd_kill_all < 0.5) { + gpio.kill_all_sent = 0.0; + } + + if (gpio.cmd_kill_jetson > 0.5 && gpio.kill_jetson_sent < 0.5) { + send_command(gpio.can_id, CMD_KILL_JETSON); + RCLCPP_INFO(rclcpp::get_logger("PowerModuleHardwareInterface"), "Sent Kill Jetson Command to CAN ID 0x%X", gpio.can_id); + gpio.kill_jetson_sent = 1.0; + } else if (gpio.cmd_kill_jetson < 0.5) { + gpio.kill_jetson_sent = 0.0; + } + + if (gpio.cmd_kill_main > 0.5 && gpio.kill_main_sent < 0.5) { + send_command(gpio.can_id, CMD_KILL_MAIN); + RCLCPP_INFO(rclcpp::get_logger("PowerModuleHardwareInterface"), "Sent Kill Main Command to CAN ID 0x%X", gpio.can_id); + gpio.kill_main_sent = 1.0; + } else if (gpio.cmd_kill_main < 0.5) { + gpio.kill_main_sent = 0.0; + } + + if (gpio.cmd_kill_by_voltage > 0.5 && gpio.kill_by_voltage_sent < 0.5) { + send_command(gpio.can_id, CMD_KILL_BY_VOLTAGE); + RCLCPP_INFO(rclcpp::get_logger("PowerModuleHardwareInterface"), "Sent Kill By Voltage Command to CAN ID 0x%X", gpio.can_id); + gpio.kill_by_voltage_sent = 1.0; + } else if (gpio.cmd_kill_by_voltage < 0.5) { + gpio.kill_by_voltage_sent = 0.0; + } + + gpio.status = (static_cast(gpio.kill_all_sent) << 3) | + (static_cast(gpio.cmd_kill_jetson) << 2) | + (static_cast(gpio.cmd_kill_main) << 1) | + static_cast(gpio.cmd_kill_by_voltage); + } + return hardware_interface::return_type::OK; +} + +} // namespace power_module_ros2_control + +PLUGINLIB_EXPORT_CLASS( + power_module_ros2_control::PowerModuleHardwareInterface, + hardware_interface::SystemInterface) diff --git a/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp b/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp index 2e33c391..d237696c 100644 --- a/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp +++ b/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "hardware_interface/handle.hpp" @@ -24,6 +25,11 @@ #include "umdloop_can_library/SocketCanBus.hpp" #include "umdloop_can_library/CanFrame.hpp" +namespace CANLib +{ + struct CanFrame; +} + namespace rmd_ros2_control { class RMDHardwareInterface : public hardware_interface::SystemInterface @@ -68,6 +74,90 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; + // Modes for control mode + enum class integration_level_t : std::uint8_t + { + UNDEFINED = 0, + POSITION = 1, + VELOCITY = 2, + }; + + // Specific motor status flags + enum class RMDMotorStatus : uint16_t { + BRAKE_LOCKED = 0x0000, + BRAKE_RELEASED = 0x0001, + MOTOR_STALL = 0x0002, + LOW_PRESSURE = 0x0004, + OVERVOLTAGE = 0x0008, + OVERCURRENT = 0x0010, + POWER_OVERRUN = 0x0040, + CAL_PARAM_WRITE_ERROR = 0x0080, + SPEEDING = 0x0100, + OVER_TEMPERATURE = 0x1000, + ENCODER_CALIBRATION_ERROR = 0x2000 + }; + + struct RMDJoint + { + std::string name; + uint32_t node_write_id; + uint32_t node_read_id; + int gear_ratio; + int orientation; + uint16_t operating_velocity; + integration_level_t control_level; + + double joint_state_position; + double joint_state_velocity; + double motor_temperature; + double motor_torque_current; + double motor_status; + double current_Kp; + double current_Ki; + double speed_Kp; + double speed_Ki; + double position_Kp; + double position_Ki; + double acceleration; + + double joint_command_position; + double joint_command_velocity; + double motor_status_req; + double motor_maintenance_req; + double maintenance_frame_high; + double maintenance_frame_low; + double maintenance_frame; + double maintenance_data_count; + std::vector decoded_maintenance_frame; + + double prev_status_req; + double prev_maintenance_req; + double elapsed_status_request_time; + double elapsed_maintenance_request_time; + double motor_velocity; + double motor_position; + double prev_joint_command_position; + double prev_joint_command_velocity; + + std::vector state_interface_names; + std::vector command_interface_names; + std::unordered_map parameters; + }; + + struct DecodedCommand + { + uint8_t command_id; + std::vector u8_data; + std::vector i16_data; + std::vector i32_data; + }; + + // Corresponding descriptions for specific motor status flags + struct StatusEntry { + RMDMotorStatus flag; + const char* name; + }; + // -- Helper Functions -- void send_command(int can_id, int cmd_id); void on_can_message(const CANLib::CanFrame& frame); @@ -76,6 +166,9 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface double calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); int32_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); int32_t calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio); + bool process_status(uint16_t status, const rclcpp::Logger& logger); + void format_control_command(CANLib::CanFrame & frame, RMDJoint & joint); + bool format_maintenance_command(CANLib::CanFrame & frame, const DecodedCommand & decoded_cmd); private: // Hardware Interface Parameters @@ -88,25 +181,6 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface // Keeps track of amount of joints int num_joints; - - // Store the state for the simulated robot - std::vector joint_state_position_; - std::vector joint_state_velocity_; - - // Store the command for the simulated robot - std::vector joint_command_position_; - std::vector joint_command_velocity_; - - // Place holders for data from the canBus, will be accessed in read() - std::vector motor_velocity; - std::vector motor_position; - - // Telemetry data from CAN responses - std::vector motor_temperature_; // Motor temperature in °C - std::vector motor_torque_current_; // Motor torque current in A - - // Velocity at which **joint** rotates to reach position in 1 dps - uint16_t operating_velocity; // CAN Library Setup CANLib::SocketCanBus canBus; @@ -115,33 +189,139 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface std::string can_interface; // Joint specific parameters - std::vector joint_node_write_ids; - std::vector joint_node_read_ids; - std::vector joint_gear_ratios; - std::vector joint_orientation; + std::vector RMDJoints_; - // Modes for control mode - enum integration_level_t : std::uint8_t + // CAN Commands + // Maintenance commands: Commands specifically sent via maintenance_request interface + enum class MaintenanceCommands : uint8_t { + WRITE_PID_TO_RAM_CMD = 0x31, + WRITE_PID_TO_ROM_CMD = 0x32, + WRITE_ACCELERATION_CMD = 0x43, + WRITE_ENCODER_MULTI_TURN_ZERO_CMD = 0x63, + WRITE_CURRENT_MULTI_TURN_POS_ZERO_CMD = 0x64, + SYSTEM_RESET_CMD = 0x76, + BRAKE_RELEASE_CMD = 0X77, + BRAKE_LOCK_CMD = 0x78, + MOTOR_SHUTDOWN_CMD = 0X80, + MOTOR_STOP_CMD = 0x81, + }; + + + // Control commands: Commands that actuate the motor + enum class ControlCommands : uint8_t { + TORQUE_CONTROL_CMD = 0xA1, + SPEED_CONTROL_CMD = 0xA2, + ABSOLUTE_POS_CONTROL_CMD = 0xA4, + }; + + // Status commands: Commands sent to request specific status information from the motor + enum class StatusCommands : uint8_t { + READ_PID_CMD = 0x30, + READ_ACCELERATION_CMD = 0x42, + MOTOR_STATUS_1_CMD = 0x9A, + MOTOR_STATUS_2_CMD = 0X9C, + }; + + static constexpr std::array kStatusCommands = { + StatusCommands::READ_PID_CMD, + StatusCommands::READ_ACCELERATION_CMD, + StatusCommands::MOTOR_STATUS_1_CMD, + StatusCommands::MOTOR_STATUS_2_CMD + }; + + // General motor status (can be interpreted by controller) + enum class MotorStatus : uint8_t { - UNDEFINED = 0, - POSITION = 1, - VELOCITY = 2, + UNKNOWN = 0, + IDLE = 1, + ACTIVE = 2, + WARNING = 3, + ERROR = 4, + DISABLED = 5 }; - // Active control mode for each actuator - std::vector control_level_; + inline static const std::vector status_table = { + {RMDMotorStatus::BRAKE_RELEASED, "Brake Released"}, + {RMDMotorStatus::BRAKE_LOCKED, "Brake Locked"}, + {RMDMotorStatus::MOTOR_STALL, "Motor Stall"}, + {RMDMotorStatus::LOW_PRESSURE, "Low Pressure"}, + {RMDMotorStatus::OVERVOLTAGE, "Overvoltage"}, + {RMDMotorStatus::OVERCURRENT, "Overcurrent"}, + {RMDMotorStatus::POWER_OVERRUN, "Power Overrun"}, + {RMDMotorStatus::CAL_PARAM_WRITE_ERROR, "Calibration Param Write Error"}, + {RMDMotorStatus::SPEEDING, "Overspeed"}, + {RMDMotorStatus::OVER_TEMPERATURE, "Over Temperature"}, + {RMDMotorStatus::ENCODER_CALIBRATION_ERROR, "Encoder Calibration Error"} + }; - // CAN Commands - static constexpr uint8_t BRAKE_RELEASE_CMD = 0X77; - static constexpr uint8_t BRAKE_LOCK_CMD = 0x78; - static constexpr uint8_t MOTOR_SHUTDOWN_CMD = 0X80; - static constexpr uint8_t MOTOR_STOP_CMD = 0x81; - static constexpr uint8_t MOTOR_STATUS_2_CMD = 0X9C; - static constexpr uint8_t SPEED_CONTROL_CMD = 0xA2; - static constexpr uint8_t ABSOLUTE_POS_CONTROL_CMD = 0xA4; + 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); -} // namespace rmd_hardware_interface + 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; + } + + inline void pack_decoded_maintenance_frame( + auto & joint, + const DecodedCommand & decoded_maintenance_cmd) + { + joint.decoded_maintenance_frame.clear(); -#endif // RMD_HARDWARE_INTERACE_HPP_ + // command id + joint.decoded_maintenance_frame.push_back(decoded_maintenance_cmd.command_id); + + // u8_data + joint.decoded_maintenance_frame.insert( + joint.decoded_maintenance_frame.end(), + decoded_maintenance_cmd.u8_data.begin(), + decoded_maintenance_cmd.u8_data.end() + ); + + // i16_data → raw bytes + const uint8_t* i16_ptr = reinterpret_cast( + decoded_maintenance_cmd.i16_data.data() + ); + + joint.decoded_maintenance_frame.insert( + joint.decoded_maintenance_frame.end(), + i16_ptr, + i16_ptr + decoded_maintenance_cmd.i16_data.size() * sizeof(int16_t) + ); + + // i32_data → raw bytes + const uint8_t* i32_ptr = reinterpret_cast( + decoded_maintenance_cmd.i32_data.data() + ); + + joint.decoded_maintenance_frame.insert( + joint.decoded_maintenance_frame.end(), + i32_ptr, + i32_ptr + decoded_maintenance_cmd.i32_data.size() * sizeof(int32_t) + ); + } +}; +} // namespace rmd_ros2_control +#endif // RMD_HARDWARE_INTERFACE_HPP_ diff --git a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp index d512409f..de6d4d0b 100644 --- a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp +++ b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp @@ -13,18 +13,18 @@ #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" - -using std::placeholders::_1; - namespace rmd_ros2_control { +// -- HELPER FUNCTIONS -- // + double RMDHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ // Converts from 0.01 deg to deg to radians/s return (motor_position * 0.01 * (M_PI/180.0))/gear_ratio; @@ -57,15 +57,13 @@ void RMDHardwareInterface::send_command(int can_id, int cmd_id){ void RMDHardwareInterface::logger_function(){ // Prevents breaking the logger - if (num_joints == 0) return; + if (RMDJoints_.size() == 0) return; // Building Message std::string log_msg = "\033[2J\033[H \nRMD Logger"; std::string control_mode = ""; - // HWI Specific std::ostringstream oss; - oss << "\n--- HWI Specific ---\n" << "CAN Interface: " << can_interface << " | HWI Update Rate: " << update_rate @@ -73,39 +71,261 @@ void RMDHardwareInterface::logger_function(){ << "Elapsed Time since first update: " << elapsed_time << "\n" << "\n--- Joint Specific ---"; - for (int i = 0; i < num_joints; i++) { - if(static_cast(control_level_[i]) == 1) { + for (auto & joint : RMDJoints_) { + if(static_cast(joint.control_level) == 1) { control_mode = "POSITION"; } - else if(static_cast(control_level_[i]) == 2) { + else if(static_cast(joint.control_level) == 2) { control_mode = "VELOCITY"; } else { control_mode = "UNDEFINED"; } - oss << "\nJOINT: " << info_.joints[i].name << "\n" - << "Parameters: Write CAN ID: 0x" << std::hex << std::uppercase << joint_node_write_ids[i] - << " | Read CAN ID: 0x" << std::hex << std::uppercase << joint_node_read_ids[i] - << " | Gear Ratio: " << joint_gear_ratios[i] - << " | Orientation: " << joint_orientation[i] << "\n" - << "-- Commands --\n" + oss << "\n----- JOINT: " << joint.name << " -----\n" + << "Parameters: Write CAN ID: 0x" << std::hex << std::uppercase << joint.node_write_id + << " | Read CAN ID: 0x" << std::hex << std::uppercase << joint.node_read_id + << " | Gear Ratio: " << joint.gear_ratio + << " | Orientation: " << joint.orientation << "\n" + << "Curr P: " << joint.current_Kp + << " | Curr I: " << joint.current_Ki + << " | Speed P: " << joint.speed_Kp + << " | Speed I: " << joint.speed_Ki + << " | Pos P: " << joint.position_Kp + << " | Pos I: " << joint.position_Ki + << "\n-- Commands --\n" << "Control Mode: " << control_mode << "\n" - << "Motor Position: " << motor_position[i] - << " | Joint Command Position: " << joint_command_position_[i] << "\n" - << "Motor Velocity: " << motor_velocity[i] - << " | Joint Command Velocity: " << joint_command_velocity_[i] << "\n" - << "-- State --\n" - << "Joint Position: " << joint_state_position_[i] - << " | Joint Velocity: " << joint_state_velocity_[i] << "\n" + << "Motor Position: " << joint.motor_position + << " | Joint Command Position: " << joint.joint_command_position << "\n" + << "Motor Velocity: " << joint.motor_velocity + << " | Joint Command Velocity: " << joint.joint_command_velocity << "\n" + << "Motor Status Request: " << joint.motor_status_req + << " | Motor Maintenance Request Rate: " << joint.motor_maintenance_req << "\n" + << " | Motor Maintenance Command High: " << joint.maintenance_frame_high + << " | Motor Maintenance Command Low: " << joint.maintenance_frame_low + << " | Motor Maintenance Command Full: " << joint.maintenance_frame << "\n" + << "Previous: Motor Status Request Rate: " << joint.prev_status_req + << " | Motor Maintenance Request Rate: " << joint.prev_maintenance_req + << " Decoded Maintenance Frame: "; + for (const auto& byte : joint.decoded_maintenance_frame) { + oss << std::hex << std::uppercase << static_cast(byte) << " "; + } + oss << "\n-- State --\n" + << "Joint Position: " << joint.joint_state_position << " rad" + << " | Joint Velocity: " << joint.joint_state_velocity << " rad/s" + << " | Joint Acceleration: " << joint.acceleration << " dps/s\n" << "-- Telemetry --\n" - << "Motor Temperature: " << motor_temperature_[i] << " C" - << " | Torque Current: " << motor_torque_current_[i] << " A\n"; + << "Motor Temperature: " << joint.motor_temperature << " C" + << " | Torque Current: " << joint.motor_torque_current << " A" + << " | Motor Status: : " << joint.motor_status << "\n"; + + log_msg += oss.str(); + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), log_msg.c_str()); } +} + +bool RMDHardwareInterface::process_status(uint16_t status, const rclcpp::Logger & logger) +{ + bool has_warning = false; + bool has_fatal_error = false; + + std::ostringstream error_stream; - log_msg += oss.str(); - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), log_msg.c_str()); + // Most safe state, no errors or warnings + if (status == 0) { + return true; + } + + for (const auto& entry : status_table) { + uint16_t code = static_cast(entry.flag); + + if (code == 0) { + continue; + } + + if (status & code) { + + // warning case + if (code == 0x0001) { + has_warning = true; + continue; + } + + // fatal error case + if (code > 1) { + has_fatal_error = true; + error_stream << entry.name << " (0x" + << std::hex << code << std::dec << "), "; + } + } + } + + // WARN + if (has_warning) { + // RCLCPP_WARN(logger, "Brake Released (warning state)"); + } + + // ERROR + if (has_fatal_error) { + RCLCPP_ERROR(logger, "Motor Errors: %s", error_stream.str().c_str()); + } + + // return system safety state + return !has_fatal_error; } +bool RMDHardwareInterface::format_maintenance_command(CANLib::CanFrame & frame, const DecodedCommand & decoded_cmd) { + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); + frame.data[0] = decoded_cmd.command_id; // Set multiplexor + switch (static_cast(decoded_cmd.command_id)) { + case MaintenanceCommands::WRITE_PID_TO_RAM_CMD: + if(decoded_cmd.u8_data.size() != 6 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + frame.data[2] = decoded_cmd.u8_data[0]; // Current Loop P gain + frame.data[3] = decoded_cmd.u8_data[1]; // Current Loop I gain + frame.data[4] = decoded_cmd.u8_data[2]; // Speed Loop P gain + frame.data[5] = decoded_cmd.u8_data[3]; // Speed Loop I gain + frame.data[6] = decoded_cmd.u8_data[4]; // Position Loop P gain + frame.data[7] = decoded_cmd.u8_data[5]; // Position Loop I gain + return true; + } + case MaintenanceCommands::WRITE_PID_TO_ROM_CMD: + if(decoded_cmd.u8_data.size() != 6 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + frame.data[2] = decoded_cmd.u8_data[0]; // Current Loop P gain + frame.data[3] = decoded_cmd.u8_data[1]; // Current Loop I gain + frame.data[4] = decoded_cmd.u8_data[2]; // Speed Loop P gain + frame.data[5] = decoded_cmd.u8_data[3]; // Speed Loop I gain + frame.data[6] = decoded_cmd.u8_data[4]; // Position Loop P gain + frame.data[7] = decoded_cmd.u8_data[5]; // Position Loop I gain + return true; + } + case MaintenanceCommands::WRITE_ACCELERATION_CMD: + if(decoded_cmd.u8_data.size() != 1 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 1){ + return false; // Invalid data format for this command + } + else{ + frame.data[1] = decoded_cmd.u8_data[0]; // Function Index + frame.data[4] = decoded_cmd.i32_data[0] & 0xFF; // Acceleration low byte + frame.data[5] = (decoded_cmd.i32_data[0] >> 8) & 0xFF; // Acceleration byte 2 + frame.data[6] = (decoded_cmd.i32_data[0] >> 16) & 0xFF; // Acceleration byte 3 + frame.data[7] = (decoded_cmd.i32_data[0] >> 24) & 0xFF; // Acceleration high byte + return true; + } + case MaintenanceCommands::WRITE_ENCODER_MULTI_TURN_ZERO_CMD: + if(decoded_cmd.u8_data.size() != 0 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 1){ + return false; // Invalid data format for this command + } + else{ + frame.data[4] = decoded_cmd.i32_data[0] & 0xFF; // Encoder offset low byte + frame.data[5] = (decoded_cmd.i32_data[0] >> 8) & 0xFF; // Encoder offset byte 2 + frame.data[6] = (decoded_cmd.i32_data[0] >> 16) & 0xFF; // Encoder offset byte 3 + frame.data[7] = (decoded_cmd.i32_data[0] >> 24) & 0xFF; // Encoder offset high byte + return true; + } + case MaintenanceCommands::WRITE_CURRENT_MULTI_TURN_POS_ZERO_CMD: + if(decoded_cmd.u8_data.size() != 0 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + // Empty + return true; + } + case MaintenanceCommands::SYSTEM_RESET_CMD: + if(decoded_cmd.u8_data.size() != 0 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + // Empty + return true; + } + case MaintenanceCommands::BRAKE_RELEASE_CMD: + if(decoded_cmd.u8_data.size() != 0 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + // Empty + return true; + } + case MaintenanceCommands::BRAKE_LOCK_CMD: + if(decoded_cmd.u8_data.size() != 0 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + // Empty + return true; + } + case MaintenanceCommands::MOTOR_SHUTDOWN_CMD: + if(decoded_cmd.u8_data.size() != 0 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + // Empty + return true; + } + case MaintenanceCommands::MOTOR_STOP_CMD: + if(decoded_cmd.u8_data.size() != 0 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + // Empty + return true; + } + default: + return false; + } +} + +void RMDHardwareInterface::format_control_command(CANLib::CanFrame & frame, RMDJoint & joint) { + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); + if (joint.control_level == integration_level_t::POSITION && + std::isfinite(joint.joint_command_position) && + joint.joint_command_position != joint.prev_joint_command_position) + { + int32_t joint_angle = joint.orientation * + calculate_motor_position_from_desired_joint_position( + joint.joint_command_position, joint.gear_ratio); + + frame.data[0] = static_cast(ControlCommands::ABSOLUTE_POS_CONTROL_CMD); + frame.data[1] = 0x00; + frame.data[2] = joint.operating_velocity & 0xFF; + frame.data[3] = (joint.operating_velocity >> 8) & 0xFF; + frame.data[4] = joint_angle & 0xFF; + frame.data[5] = (joint_angle >> 8) & 0xFF; + frame.data[6] = (joint_angle >> 16) & 0xFF; + frame.data[7] = (joint_angle >> 24) & 0xFF; + + joint.prev_joint_command_position = joint.joint_command_position; + } + else if (joint.control_level == integration_level_t::VELOCITY && + std::isfinite(joint.joint_command_velocity) && + joint.joint_command_velocity != joint.prev_joint_command_velocity) + { + int32_t joint_velocity = joint.orientation * + calculate_motor_velocity_from_desired_joint_velocity( + joint.joint_command_velocity, joint.gear_ratio); + + frame.data[0] = static_cast(ControlCommands::SPEED_CONTROL_CMD); + frame.data[1] = 0x00; + frame.data[2] = 0x00; + frame.data[3] = 0x00; + frame.data[4] = joint_velocity & 0xFF; + frame.data[5] = (joint_velocity >> 8) & 0xFF; + frame.data[6] = (joint_velocity >> 16) & 0xFF; + frame.data[7] = (joint_velocity >> 24) & 0xFF; + + joint.prev_joint_command_velocity = joint.joint_command_velocity; + } + else + { + frame.data[0] = static_cast(StatusCommands::MOTOR_STATUS_2_CMD); + } +} + +// -- LIFECYCLE CALLBACKS -- hardware_interface::CallbackReturn RMDHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file @@ -116,46 +336,94 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_init( } // Setting Parameters - for (auto& joint : info_.joints) { - int write_id = std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x141, 0x160); - int gear_ratio = std::abs(std::stoi(joint.parameters.at("gear_ratio"))); - joint_node_write_ids.push_back(write_id); - joint_node_read_ids.push_back(write_id+0x100); - - joint_gear_ratios.push_back(gear_ratio); - joint_orientation.push_back(std::stoi(joint.parameters.at("joint_orientation")) == -1 ? -1 : 1); - operating_velocity = std::clamp( - std::stoi(joint.parameters.at("operating_velocity")), - 0, - static_cast(3.5 * gear_ratio) - ); + RMDJoints_.clear(); - } + num_joints = static_cast(RMDJoints_.size()); - num_joints = static_cast(info_.joints.size()); + // General HWI parameters 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"); - elapsed_update_time = 0.0; - elapsed_time = 0.0; - elapsed_logger_time = 0.0; - - // Initializes command and state interface values - joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); - joint_state_velocity_.assign(num_joints, 0); + // Per Joint Parameters + for (auto& joint : info_.joints) { + // Collect joint specific parameters + int write_id = std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x141, 0x160); + int gear_ratio = std::abs(std::stoi(joint.parameters.at("gear_ratio"))); + int orientation = std::stoi(joint.parameters.at("joint_orientation")) == -1 ? -1 : 1; + int operating_velocity = std::clamp(std::stoi(joint.parameters.at("operating_velocity")), 0, 65*gear_ratio); - joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); - joint_command_velocity_.assign(num_joints, 0); + // Collect state interface names + std::vector state_if_names; + for (const auto &si : joint.state_interfaces) { + state_if_names.push_back(si.name); + } - motor_position.assign(num_joints, 0.0); - motor_velocity.assign(num_joints, 0.0); + // Collect command interface names + std::vector command_if_names; + for (const auto &ci : joint.command_interfaces) { + command_if_names.push_back(ci.name); + } - motor_temperature_.assign(num_joints, 0.0); - motor_torque_current_.assign(num_joints, 0.0); + // Copy parameters into an unordered_map + std::unordered_map params_map; + for (const auto &p : joint.parameters) { + params_map.emplace(p.first, p.second); + } - control_level_.resize(num_joints, integration_level_t::POSITION); + // Populate each RMDJoint object + RMDJoints_.push_back( + RMDJoint{ + .name = joint.name, + .node_write_id = static_cast(write_id), + .node_read_id = static_cast(write_id + 0x100), + .gear_ratio = gear_ratio, + .orientation = orientation, + .operating_velocity = static_cast(operating_velocity), + .control_level = integration_level_t::POSITION, + .joint_state_position = std::numeric_limits::quiet_NaN(), + .joint_state_velocity = 0.0, + .motor_temperature = std::numeric_limits::quiet_NaN(), + .motor_torque_current = std::numeric_limits::quiet_NaN(), + .motor_status = std::numeric_limits::quiet_NaN(), + .current_Kp = std::numeric_limits::quiet_NaN(), + .current_Ki = std::numeric_limits::quiet_NaN(), + .speed_Kp = std::numeric_limits::quiet_NaN(), + .speed_Ki = std::numeric_limits::quiet_NaN(), + .position_Kp = std::numeric_limits::quiet_NaN(), + .position_Ki = std::numeric_limits::quiet_NaN(), + .acceleration = std::numeric_limits::quiet_NaN(), + + .joint_command_position = std::numeric_limits::quiet_NaN(), + .joint_command_velocity = 0.0, + .motor_status_req = 0.0, + .motor_maintenance_req = 0.0, + .maintenance_frame_high = 0.0, + .maintenance_frame_low = 0.0, + .maintenance_frame = 0.0, + .maintenance_data_count = 0.0, + .decoded_maintenance_frame = {}, + + .prev_status_req = 0.0, + .prev_maintenance_req = 0.0, + .elapsed_status_request_time = 0.0, + .elapsed_maintenance_request_time = 0.0, + .motor_velocity = 0.0, + .motor_position = 0.0, + .prev_joint_command_position = std::numeric_limits::quiet_NaN(), + .prev_joint_command_velocity = std::numeric_limits::quiet_NaN(), + .state_interface_names = state_if_names, + .command_interface_names = command_if_names, + .parameters = params_map + } + ); + } + + // Initialize timing tools + elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } @@ -175,21 +443,30 @@ std::vector RMDHardwareInterface::export_sta std::vector state_interfaces; // Each RMD motor corresponds to a different joint. - 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 (published via /dynamic_joint_states by joint_state_broadcaster) - 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])); + for (auto & joint : RMDJoints_) { + for (auto & iface : joint.state_interface_names) { + double * val = nullptr; + if (iface == "position") { + val = &joint.joint_state_position; + } else if (iface == "velocity") { + val = &joint.joint_state_velocity; + } else if (iface == "motor_temperature") { + val = &joint.motor_temperature; + } else if (iface == "torque_current") { + val = &joint.motor_torque_current; + } else if (iface == "status") { + val = &joint.motor_status; + } else { + RCLCPP_WARN( + rclcpp::get_logger("RMDHardwareInterface"), + "Unknown state interface '%s' for joint '%s'", + iface.c_str(), joint.name.c_str()); + continue; + } + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.name, iface, val)); + } } - return state_interfaces; } @@ -199,12 +476,33 @@ RMDHardwareInterface::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])); + for (auto & joint : RMDJoints_) { + for (auto & iface : joint.command_interface_names) { + double * val = nullptr; + if (iface == "position") { + val = &joint.joint_command_position; + } else if (iface == "velocity") { + val = &joint.joint_command_velocity; + } else if (iface == "status_request") { + val = &joint.motor_status_req; + } else if (iface == "maintenance_request") { + val = &joint.motor_maintenance_req; + } else if (iface == "maintenance_frame_high") { + val = &joint.maintenance_frame_high; + } else if (iface == "maintenance_frame_low") { + val = &joint.maintenance_frame_low; + } else if (iface == "maintenance_data_count") { + val = &joint.maintenance_data_count; + } else { + RCLCPP_WARN( + rclcpp::get_logger("RMDHardwareInterface"), + "Unknown command interface '%s' for joint '%s'", + iface.c_str(), joint.name.c_str()); + continue; + } + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, iface, val)); + } } return command_interfaces; @@ -214,7 +512,7 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { if (!canBus.open(can_interface, std::bind(&RMDHardwareInterface::on_can_message, this, std::placeholders::_1))) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Failed to open CAN interface"); + RCLCPP_ERROR(rclcpp::get_logger("RMDHardwareInterface"), "Failed to open CAN interface"); return hardware_interface::CallbackReturn::ERROR; } @@ -228,14 +526,15 @@ void RMDHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { std::vector data(8, 0x00); - for(int i = 0; i < num_joints; i++){ - if(can_rx_frame_.id == joint_node_read_ids[i] && - (can_rx_frame_.data[0] == SPEED_CONTROL_CMD || - can_rx_frame_.data[0] == ABSOLUTE_POS_CONTROL_CMD || - can_rx_frame_.data[0] == MOTOR_STATUS_2_CMD) && - can_rx_frame_.data[1] != 0x00) { - - // DECODING CAN MESSAGE (A2, A4, and 9C all share the same reply) + // Decode CAN Message + for (auto & joint : RMDJoints_) { + if(can_rx_frame_.id == joint.node_read_id && + (can_rx_frame_.data[0] == static_cast(ControlCommands::SPEED_CONTROL_CMD) || + can_rx_frame_.data[0] == static_cast(ControlCommands::ABSOLUTE_POS_CONTROL_CMD) || + can_rx_frame_.data[0] == static_cast(StatusCommands::MOTOR_STATUS_2_CMD))) { + + // Speed Control, Absolute Position Control, and Motor Status 2 all share the same reply + data[0] = can_rx_frame_.data[0]; data[1] = can_rx_frame_.data[1]; // Motor Temperature data[2] = can_rx_frame_.data[2]; // Torque low byte data[3] = can_rx_frame_.data[3]; // Torque high byte @@ -244,19 +543,75 @@ void RMDHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { data[6] = can_rx_frame_.data[6]; // encoder position low byte data[7] = can_rx_frame_.data[7]; // encoder position high byte + // TEMPERATURE (1 byte, degrees C) + joint.motor_temperature = static_cast(data[1]); + + // TORQUE CURRENT (16-bit signed, 0.01A per LSB) + joint.motor_torque_current = static_cast(static_cast((data[3] << 8) | data[2])) * 0.01; + // POSITION // uint16 -> int16 -> double (for calcs) - motor_position[i] = static_cast(static_cast((data[7] << 8) | data[6])); + joint.motor_position = static_cast(static_cast((data[7] << 8) | data[6])); // VELOCITY // uint16 -> int16 -> double (for calcs) - motor_velocity[i] = static_cast(static_cast((data[5] << 8) | data[4])); + joint.motor_velocity = static_cast(static_cast((data[5] << 8) | data[4])); + } else if(can_rx_frame_.id == joint.node_read_id && + can_rx_frame_.data[0] == static_cast(StatusCommands::MOTOR_STATUS_1_CMD)) { - // TEMPERATURE (1 byte, degrees C) - motor_temperature_[i] = static_cast(data[1]); + // DECODING Motor Status 1 Message + data[0] = can_rx_frame_.data[0]; + data[1] = can_rx_frame_.data[1]; // Motor Temperature + data[2] = 0x00; // Null + data[3] = can_rx_frame_.data[3]; // Brake State + data[4] = can_rx_frame_.data[4]; // Voltage low byte + data[5] = can_rx_frame_.data[5]; // Voltage high byte + data[6] = can_rx_frame_.data[6]; // Error state low byte + data[7] = can_rx_frame_.data[7]; // Error state high byte + + // TEMPERATURE (degrees C) + joint.motor_temperature = static_cast(data[1]); + + // Initialize motor status command + joint.motor_status = 0.0; - // TORQUE CURRENT (16-bit signed, 0.01A per LSB) - motor_torque_current_[i] = static_cast(static_cast((data[3] << 8) | data[2])) * 0.01; + // BRAKE STATUS (0: Locked, 1: Released) + joint.motor_status = joint.motor_status + static_cast(data[3]); + + // ERROR STATE + joint.motor_status = joint.motor_status + static_cast(static_cast((data[7] << 8) | data[6])); + } else if(can_rx_frame_.id == joint.node_read_id && + can_rx_frame_.data[0] == static_cast(StatusCommands::READ_PID_CMD)) { + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Received PID Gains from joint: %s", joint.name.c_str()); + data[1] = 0x00; // Null + data[2] = can_rx_frame_.data[2]; // Current Loop P gain + data[3] = can_rx_frame_.data[3]; // Current Loop I gain + data[4] = can_rx_frame_.data[4]; // Speed Loop P gain + data[5] = can_rx_frame_.data[5]; // Speed Loop I gain + data[6] = can_rx_frame_.data[6]; // Position Loop P gain + data[7] = can_rx_frame_.data[7]; // Position Loop I gain + + // Joint PI Gains + joint.current_Kp = static_cast(data[2]); + joint.current_Ki = static_cast(data[3]); + joint.speed_Kp = static_cast(data[4]); + joint.speed_Ki = static_cast(data[5]); + joint.position_Kp = static_cast(data[6]); + joint.position_Ki = static_cast(data[7]); + } + else if(can_rx_frame_.id == joint.node_read_id && + can_rx_frame_.data[0] == static_cast(StatusCommands::READ_ACCELERATION_CMD)) { + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Received Acceleration from joint: %s", joint.name.c_str()); + data[1] = 0x00; // Null + data[2] = 0x00; // Null + data[3] = 0x00; // Null + data[4] = can_rx_frame_.data[4]; // Acceleration low byte + data[5] = can_rx_frame_.data[5]; // Acceleration byte 2 + data[6] = can_rx_frame_.data[6]; // Acceleration byte 3 + data[7] = can_rx_frame_.data[7]; // Acceleration high byte + + // Joint acceleration in dps/s + joint.acceleration = static_cast((data[7] << 24) | (data[6] << 16) | (data[5] << 8) | data[4]); } else { if(logger_state == 1) { @@ -270,11 +625,9 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_cleanup( const rclcpp_lifecycle::State & /*previous_state*/) { // If cleanup occurs before shutdown, this is the last opportunity to shutdown motor since pointers must be deleted here - std::vector data(8, 0x00); - - for(int i = 0; i < num_joints; i++){ - send_command(joint_node_write_ids[i], MOTOR_SHUTDOWN_CMD); // Motor Shutdown Command - send_command(joint_node_write_ids[i], BRAKE_LOCK_CMD); // Brake Lock Command (don't think this works) + for (const auto & joint : RMDJoints_) { + send_command(joint.node_write_id, static_cast(MaintenanceCommands::MOTOR_SHUTDOWN_CMD)); // Motor Shutdown Command + send_command(joint.node_write_id, static_cast(MaintenanceCommands::BRAKE_LOCK_CMD)); // Brake Lock Command (don't think this works) } canBus.close(); @@ -287,13 +640,15 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_activate( { RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Activating ...please wait..."); - for(int i = 0; i < num_joints; i++){ + for (const auto & joint : RMDJoints_) { // Brake Release command (pretty sure brakes don't work) - send_command(joint_node_write_ids[i], BRAKE_RELEASE_CMD); + send_command(joint.node_write_id, static_cast(MaintenanceCommands::BRAKE_RELEASE_CMD)); } // Sets initial command to joint state - joint_command_position_ = joint_state_position_; + for (auto & joint : RMDJoints_) { + joint.joint_command_position = joint.joint_state_position; + } RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Successfully activated!"); @@ -306,9 +661,9 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_deactivate( { RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Deactivating ...please wait..."); - for(int i = 0; i < num_joints; i++){ - send_command(joint_node_write_ids[i], MOTOR_STOP_CMD); // Motor Stop Command - send_command(joint_node_write_ids[i], BRAKE_LOCK_CMD); // Brake Lock Command (don't think this works) + for (const auto & joint : RMDJoints_) { + send_command(joint.node_write_id, static_cast(MaintenanceCommands::MOTOR_STOP_CMD)); // Motor Stop Command + send_command(joint.node_write_id, static_cast(MaintenanceCommands::BRAKE_LOCK_CMD)); // Brake Lock Command (don't think this works) } RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Successfully deactivated all RMD motors!"); @@ -321,9 +676,14 @@ hardware_interface::return_type RMDHardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // CALCULATING JOINT STATE - for(int i = 0; i < num_joints; i++) { - joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); - joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); + for (auto & joint : RMDJoints_) { + joint.joint_state_velocity = calculate_joint_velocity_from_motor_velocity(joint.motor_velocity, joint.gear_ratio); + joint.joint_state_position = calculate_joint_position_from_motor_position(joint.motor_position, joint.gear_ratio); + + // Process status logs warning and errors and returns false if fatal error occurs + if (!process_status(joint.motor_status, rclcpp::get_logger("RMDHardwareInterface"))) { + return hardware_interface::return_type::ERROR; + } } return hardware_interface::return_type::OK; @@ -333,15 +693,8 @@ hardware_interface::return_type RMDHardwareInterface::read( hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - elapsed_update_time+=period.seconds(); - double update_period = 1.0/update_rate; - int data[8] = {0x00}; - int32_t joint_angle = 0; - int32_t joint_velocity = 0; - - elapsed_time+=period.seconds(); - // Logger update + elapsed_time+=period.seconds(); elapsed_logger_time+=period.seconds(); double logging_period = 1.0/logger_rate; if(elapsed_logger_time > logging_period){ @@ -351,58 +704,88 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( } } + // Status request handling + for (auto & joint : RMDJoints_) { + double curr_status_req = joint.motor_status_req; + if (curr_status_req < 0 && joint.prev_status_req >= 0) // Send one shot status request + { + for (auto & status_cmd : kStatusCommands) { + send_command(joint.node_write_id, static_cast(status_cmd)); + } + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "One-shot status request sent for joint '%s'.", joint.name.c_str()); + } + else if (curr_status_req > 0) // Send status request at specified frequency (Hz) + { + joint.elapsed_status_request_time += period.seconds(); + double status_request_period = 1.0 / curr_status_req; + if(joint.elapsed_status_request_time > status_request_period){ + joint.elapsed_status_request_time = 0.0; + for (auto & status_cmd : kStatusCommands) { + send_command(joint.node_write_id, static_cast(status_cmd)); + } + } + } + joint.prev_status_req = curr_status_req; + } + + // Maintenance request handling + for (auto & joint : RMDJoints_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.node_write_id; + can_tx_frame_.dlc = 8; + + auto doubles_to_payload = [](double high, double low) -> int64_t + { + uint64_t h = static_cast(high); + uint64_t l = static_cast(low); + return static_cast((h << 32) | l); + }; + + // Call it and store the result + joint.maintenance_frame = doubles_to_payload(joint.maintenance_frame_high, joint.maintenance_frame_low); + + // Decode the maintenance command interfaces + DecodedCommand decoded_maintenance_cmd = unpack_command_full(static_cast(joint.maintenance_data_count), static_cast(joint.maintenance_frame)); + + pack_decoded_maintenance_frame(joint, decoded_maintenance_cmd); // Store decoded maintenance command in joint struct for logging and validation + + if(!format_maintenance_command(can_tx_frame_, decoded_maintenance_cmd)){ // Validate maintenance command ID before sending + // RCLCPP_WARN(rclcpp::get_logger("RMDHardwareInterface"), "Invalid maintenance command for joint '%s'.", joint.name.c_str()); + continue; + } + + double curr_maintenance_req = joint.motor_maintenance_req; + + if (curr_maintenance_req < 0 && joint.prev_maintenance_req >= 0) // Send one shot maintenance request + { + canBus.send(can_tx_frame_); + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "One-shot maintenance request sent for joint '%s'.", joint.name.c_str()); + } + else if(curr_maintenance_req > 0) // Send maintenance request at specified frequency (Hz) + { + joint.elapsed_maintenance_request_time += period.seconds(); + double maintenance_request_period = 1.0 / joint.motor_maintenance_req; + if(joint.elapsed_maintenance_request_time > maintenance_request_period){ + joint.elapsed_maintenance_request_time = 0.0; + canBus.send(can_tx_frame_); + } + } + joint.prev_maintenance_req = curr_maintenance_req; + } // HWI can only go as fast as the controller manager. To limit frequency of bus messages, // keep track of time passed over iterations of this function and if it exceeds the // desired frequency of the HWI, skip message + elapsed_update_time+=period.seconds(); + double update_period = 1.0/update_rate; if(elapsed_update_time > update_period){ elapsed_update_time = 0.0; - for(int i = 0; i < num_joints; i++) { + for(auto & joint : RMDJoints_) { can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated - can_tx_frame_.id = joint_node_write_ids[i]; + can_tx_frame_.id = joint.node_write_id; can_tx_frame_.dlc = 8; - if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { - - // CALCULATE DESIRED JOINT ANGLE - joint_angle = joint_orientation[i]*calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); - - // ENCODING CAN MESSAGE - data[0] = ABSOLUTE_POS_CONTROL_CMD; - data[1] = 0x00; - data[2] = operating_velocity & 0xFF; - data[3] = (operating_velocity >> 8) & 0xFF; - data[4] = joint_angle & 0xFF; - data[5] = (joint_angle >> 8) & 0xFF; - data[6] = (joint_angle >> 16) & 0xFF; - data[7] = (joint_angle >> 24) & 0xFF; - } - else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { - - // CALCULATE DESIRED JOINT VELOCITY - joint_velocity = joint_orientation[i]*calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); - - // ENCODING CAN MESSAGE - data[0] = SPEED_CONTROL_CMD; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = joint_velocity & 0xFF; - data[5] = (joint_velocity >> 8) & 0xFF; - data[6] = (joint_velocity >> 16) & 0xFF; - data[7] = (joint_velocity >> 24) & 0xFF; - } - else{ - // RCLCPP_WARN(rclcpp::get_logger("RMDHardwareInterface"), "Joint command value not found or undefined command state. Sending Motor Status 3 commands for now."); - // ENCODING CAN MESSAGE - data[0] = MOTOR_STATUS_2_CMD; - } - - // Cast data to uint8_t - for(int j = 0; j < 8; j++){ - data[j] = static_cast(data[j]); - can_tx_frame_.data[j] = data[j]; - } + format_control_command(can_tx_frame_, joint); canBus.send(can_tx_frame_); } } @@ -415,24 +798,25 @@ hardware_interface::return_type RMDHardwareInterface::perform_command_mode_switc const std::vector& stop_interfaces) { // Debug: print incoming requests - std::ostringstream ss; - ss << "perform_command_mode_switch called. start_interfaces: ["; - for (auto &s : start_interfaces) ss << s << ","; - ss << "] stop_interfaces: ["; - for (auto &s : stop_interfaces) ss << s << ","; - ss << "]"; - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), ss.str().c_str()); + // std::ostringstream ss; + // ss << "perform_command_mode_switch called. start_interfaces: ["; + // for (auto &s : start_interfaces) ss << s << ","; + // ss << "] stop_interfaces: ["; + // for (auto &s : stop_interfaces) ss << s << ","; + // ss << "]"; + // RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), ss.str().c_str()); // For each joint, decide its new control mode based on start/stop interfaces. // We allow partial starts/stops: only affected joints are switched. - std::vector requested_modes(num_joints, integration_level_t::UNDEFINED); + std::vector requested_modes(RMDJoints_.size(), integration_level_t::UNDEFINED); // Process stop interfaces first: mark those joints as UNDEFINED for (const auto &ifname : stop_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 || ifname == vel_if || ifname.find(info_.joints[i].name) != std::string::npos) { + for (size_t i = 0; i < RMDJoints_.size(); ++i){ + auto & joint = RMDJoints_[i]; + const std::string pos_if = joint.name + "/" + std::string(hardware_interface::HW_IF_POSITION); + const std::string vel_if = joint.name + "/" + std::string(hardware_interface::HW_IF_VELOCITY); + if (ifname == pos_if || ifname == vel_if || ifname.find(joint.name) != std::string::npos) { requested_modes[i] = integration_level_t::UNDEFINED; } } @@ -440,9 +824,10 @@ hardware_interface::return_type RMDHardwareInterface::perform_command_mode_switc // Process start interfaces: set POSITION or VELOCITY per joint 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); + for (size_t i = 0; i < RMDJoints_.size(); ++i) { + auto & joint = RMDJoints_[i]; + const std::string pos_if = joint.name + "/" + std::string(hardware_interface::HW_IF_POSITION); + const std::string vel_if = joint.name + "/" + std::string(hardware_interface::HW_IF_VELOCITY); if (ifname == pos_if) { requested_modes[i] = integration_level_t::POSITION; } else if (ifname == vel_if) { @@ -451,43 +836,43 @@ hardware_interface::return_type RMDHardwareInterface::perform_command_mode_switc } } - // Now apply the requested_modes to control_level_. + // Now apply the requested_modes to each joint. // For any joint with UNDEFINED in requested_modes, we only change it if it was explicitly stopped. - for (int i = 0; i < num_joints; ++i) { + for (size_t i = 0; i < RMDJoints_.size(); ++i) { + auto & joint = RMDJoints_[i]; if (requested_modes[i] == integration_level_t::UNDEFINED) { // if stop requested, set to UNDEFINED; otherwise leave existing mode // (we only set to UNDEFINED if this joint was mentioned in stop_interfaces) bool was_stopped = false; for (const auto &ifname : stop_interfaces) { - if (ifname.find(info_.joints[i].name) != std::string::npos) { + if (ifname.find(joint.name) != std::string::npos) { was_stopped = true; break; } } if (was_stopped) { - control_level_[i] = integration_level_t::UNDEFINED; - joint_command_velocity_[i] = 0; + joint.control_level = integration_level_t::UNDEFINED; + joint.joint_command_velocity = 0; // optional: reset position cmd to current state to be safe - joint_command_position_[i] = joint_state_position_[i]; + joint.joint_command_position = joint.joint_state_position; RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), - "Joint %s: stopped -> set UNDEFINED", info_.joints[i].name.c_str()); + "Joint %s: stopped -> set UNDEFINED", joint.name.c_str()); } - // else, leave control_level_ as-is + // else, leave the existing control mode as-is } else { // Set the mode requested - control_level_[i] = requested_modes[i]; + joint.control_level = requested_modes[i]; // If switching to velocity, optionally set command velocity to current state to avoid jumps if (requested_modes[i] == integration_level_t::VELOCITY) { - // joint_command_velocity_[i] = joint_state_velocity_[i]; - joint_command_velocity_[i] = 0; + joint.joint_command_velocity = 0; RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Joint %s: switched to VELOCITY (cmd vel initialized from state: %f)", - info_.joints[i].name.c_str(), joint_command_velocity_[i]); + joint.name.c_str(), joint.joint_command_velocity); } else if (requested_modes[i] == integration_level_t::POSITION) { - joint_command_position_[i] = joint_state_position_[i]; + joint.joint_command_position = joint.joint_state_position; RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Joint %s: switched to POSITION (cmd pos initialized from state: %f)", - info_.joints[i].name.c_str(), joint_command_position_[i]); + joint.name.c_str(), joint.joint_command_position); } } } @@ -501,4 +886,4 @@ hardware_interface::return_type RMDHardwareInterface::perform_command_mode_switc #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - rmd_ros2_control::RMDHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file + rmd_ros2_control::RMDHardwareInterface, hardware_interface::SystemInterface) diff --git a/src/hardware_interfaces/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp b/src/hardware_interfaces/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp index d19a19c7..1cea87a2 100644 --- a/src/hardware_interfaces/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp +++ b/src/hardware_interfaces/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp @@ -8,8 +8,6 @@ #include "rclcpp/rclcpp.hpp" #include "socket_can.hpp" -#define DEBUG_MODE 0 - namespace odrive_ros2_control { class Axis; @@ -37,6 +35,7 @@ class ODriveHardwareInterface final : public hardware_interface::SystemInterface return_type write(const rclcpp::Time&, const rclcpp::Duration&) override; private: + void logger_function(); void on_can_msg(const can_frame& frame); void set_axis_command_mode(const Axis& axis); @@ -46,6 +45,14 @@ class ODriveHardwareInterface final : public hardware_interface::SystemInterface std::string can_intf_name_; SocketCanIntf can_intf_; rclcpp::Time timestamp_; + + // Hardware Interface Parameters + int update_rate; + double elapsed_update_time; // Time since last hardware interface update + double elapsed_time; // Time since first hardware interface update + double elapsed_logger_time; // Time since last logger update + int logger_rate; // Logger update rate + int logger_state; // Logger on/off state }; struct Axis { @@ -67,22 +74,22 @@ struct Axis { // State (ODrives => ros2_control) // rclcpp::Time encoder_estimates_timestamp_; - // uint32_t axis_error_ = 0; - // uint8_t axis_state_ = 0; - // uint8_t procedure_result_ = 0; - // uint8_t trajectory_done_flag_ = 0; + uint32_t axis_error_ = 0; + uint8_t axis_state_ = 0; + uint8_t procedure_result_ = 0; + uint8_t trajectory_done_flag_ = 0; double pos_estimate_ = 0; // [rad] ** CHANGED ** double vel_estimate_ = 0; // [rad/s] ** CHANGED ** - // double iq_setpoint_ = NAN; - // double iq_measured_ = NAN; + double iq_setpoint_ = NAN; + double iq_measured_ = NAN; double torque_target_ = NAN; // [Nm] double torque_estimate_ = NAN; // [Nm] - // uint32_t active_errors_ = 0; - // uint32_t disarm_reason_ = 0; - // double fet_temperature_ = NAN; - // double motor_temperature_ = NAN; - // double bus_voltage_ = NAN; - // double bus_current_ = NAN; + uint32_t active_errors_ = 0; + uint32_t disarm_reason_ = 0; + double fet_temperature_ = NAN; + double motor_temperature_ = NAN; + double bus_voltage_ = NAN; + double bus_current_ = NAN; // Indicates which controller inputs are enabled. This is configured by the // controller that sits on top of this hardware interface. Multiple inputs @@ -111,12 +118,83 @@ using namespace odrive_ros2_control; using hardware_interface::CallbackReturn; using hardware_interface::return_type; +void ODriveHardwareInterface::logger_function(){ + int num_joints = static_cast(info_.joints.size()); + + // Prevents breaking the logger + if (num_joints == 0) return; + + // Building Message + std::string log_msg = "\033[2J\033[H \nODrive Logger"; + + // HWI Specific + std::ostringstream oss; + + oss << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_intf_name_ + << " | HWI Update Rate: " << 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) { + Axis& axis = axes_[i]; + + std::string control_mode; + if (axis.pos_input_enabled_) control_mode = "POSITION"; + else if (axis.vel_input_enabled_) control_mode = "VELOCITY"; + else if (axis.torque_input_enabled_) control_mode = "TORQUE"; + else control_mode = "UNDEFINED"; + + // Print parameters, commanded setpoints, enabled inputs, state estimates, and telemetry + oss << "\nJOINT: " << info_.joints[i].name << "\n" + << "Parameters: Node ID: 0x" << std::hex << axis.node_id_ << std::dec + << " | Gear Ratio: " << axis.gear_ratio_ << "\n" + << "-- Commands (requested) --\n" + << "Control Mode: " << control_mode << "\n" + << "Position Setpoint (rad): " << axis.pos_setpoint_ + << " | Motor Input Position (rev): " << (axis.pos_setpoint_ * axis.gear_ratio_) / (2 * M_PI) << "\n" + << "Velocity Setpoint (rad/s): " << axis.vel_setpoint_ + << " | Motor Input Velocity (rev/s): " << (axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI) << "\n" + << "Torque Setpoint (Nm): " << axis.torque_setpoint_ + << " | Motor Input Torque (Nm at motor): " << (axis.torque_setpoint_ / std::max(1, axis.gear_ratio_)) << "\n" + << "Inputs Enabled: pos=" << axis.pos_input_enabled_ + << " vel=" << axis.vel_input_enabled_ + << " torque=" << axis.torque_input_enabled_ << "\n" + << "-- State (estimates) --\n" + << "Joint Position (est): " << axis.pos_estimate_ + << " | Joint Velocity (est): " << axis.vel_estimate_ + << " | Joint Torque (est): " << axis.torque_estimate_ << "\n" + << "-- Telemetry --\n" + << "FET Temperature: " << axis.fet_temperature_ << " C" + << " | Motor Temperature: " << axis.motor_temperature_ << " C" + << " | Bus Voltage: " << axis.bus_voltage_ << " V" + << " | Bus Current: " << axis.bus_current_ << " A\n" + << "IQ Setpoint: " << axis.iq_setpoint_ << " | IQ Measured: " << axis.iq_measured_ << "\n" + << "Active Errors: 0x" << std::hex << axis.active_errors_ << std::dec + << " | Disarm Reason: " << axis.disarm_reason_ << "\n" + << "Axis Error: " << axis.axis_error_ << " | Axis State: " << static_cast(axis.axis_state_) + << " | Procedure Result: " << static_cast(axis.procedure_result_) + << " | Trajectory Done Flag: " << static_cast(axis.trajectory_done_flag_) << "\n"; + } + + log_msg += oss.str(); + RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "%s", log_msg.c_str()); +} + CallbackReturn ODriveHardwareInterface::on_init(const hardware_interface::HardwareInfo& info) { if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } - can_intf_name_ = "can1"; + 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_intf_name_ = info_.hardware_parameters["can"]; + + elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; for (auto& joint : info_.joints) { axes_.emplace_back(&can_intf_, std::stoi(joint.parameters.at("node_id")), std::stoi(joint.parameters.at("gear_ratio"))); @@ -187,6 +265,17 @@ std::vector ODriveHardwareInterface::export_ hardware_interface::HW_IF_POSITION, &axes_[i].pos_estimate_ )); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, + "motor_temperature", + &axes_[i].motor_temperature_ + )); + + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, + "torque_current", + &axes_[i].bus_current_ + )); } return state_interfaces; @@ -262,47 +351,58 @@ return_type ODriveHardwareInterface::read(const rclcpp::Time& timestamp, const r // repeat until CAN interface has no more messages } - // for (size_t i = 0; i < info_.joints.size(); i++) { - // if(DEBUG_MODE == 1) { - // RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Position Estimate: %f", &axes_[i].pos_estimate_); - // } - // } - return return_type::OK; } -return_type ODriveHardwareInterface::write(const rclcpp::Time&, const rclcpp::Duration&) { - for (auto& axis : axes_) { - // Send the CAN message that fits the set of enabled setpoints - // Since we are implementing a gearbox, what goes to the motors is different than what we want on the arms - // If I want my arm to move 30 degrees, the motor must move 30 * gear_ratio - // If I want my arm to have 10 Nm of torque, the motor must use 10 / gear_ratio - // Very important that this is taken care of here - if (axis.pos_input_enabled_) { - Set_Input_Pos_msg_t msg; - msg.Input_Pos = (axis.pos_setpoint_ * axis.gear_ratio_) / (2 * M_PI); - msg.Vel_FF = axis.vel_input_enabled_ ? ((axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI)) : 0.0f; - msg.Torque_FF = axis.torque_input_enabled_ ? (axis.torque_setpoint_ / axis.gear_ratio_) : 0.0f; - //RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Writing positions for ODrive %d Setpoint: %f, Joint angle of motor (rev): %f", axis.node_id_, axis.pos_setpoint_, (axis.pos_setpoint_ * axis.gear_ratio_) / (2 * M_PI)); - - axis.send(msg); - } else if (axis.vel_input_enabled_) { - Set_Input_Vel_msg_t msg; - msg.Input_Vel = (axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI); - msg.Input_Torque_FF = axis.torque_input_enabled_ ? (axis.torque_setpoint_ / axis.gear_ratio_): 0.0f; - RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Writing velocities for ODrive %d Joint velocity of motor (rev/s): %f", axis.node_id_, (axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI)); - - axis.send(msg); - } else if (axis.torque_input_enabled_) { - Set_Input_Torque_msg_t msg; - msg.Input_Torque = axis.torque_setpoint_ / axis.gear_ratio_; - axis.send(msg); - } else { - // no control enabled - don't send any setpoint +return_type ODriveHardwareInterface::write(const rclcpp::Time&, const rclcpp::Duration& period) { + + double update_period = 1.0/update_rate; + elapsed_update_time+=period.seconds(); + elapsed_time+=period.seconds(); + + // 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(); } } - // RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "\n"); + // HWI can only go as fast as the controller manager. To limit frequency of bus messages, + // keep track of time passed over iterations of this function and if it exceeds the + // desired frequency of the HWI, skip message + if(elapsed_update_time > update_period){ + elapsed_update_time = 0.0; + for (auto& axis : axes_) { + // Send the CAN message that fits the set of enabled setpoints + // If I want my arm to have 10 Nm of torque, the motor must use 10 / gear_ratio + // Very important that this is taken care of here + if (axis.pos_input_enabled_) { + Set_Input_Pos_msg_t msg; + msg.Input_Pos = (axis.pos_setpoint_ * axis.gear_ratio_) / (2 * M_PI); + msg.Vel_FF = axis.vel_input_enabled_ ? ((axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI)) : 0.0f; + msg.Torque_FF = axis.torque_input_enabled_ ? (axis.torque_setpoint_ / axis.gear_ratio_) : 0.0f; + // RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Writing positions for ODrive %d Setpoint: %f, Joint angle of motor (rev): %f", axis.node_id_, axis.pos_setpoint_, (axis.pos_setpoint_ * axis.gear_ratio_) / (2 * M_PI)); + + axis.send(msg); + } else if (axis.vel_input_enabled_) { + Set_Input_Vel_msg_t msg; + msg.Input_Vel = (axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI); + msg.Input_Torque_FF = axis.torque_input_enabled_ ? (axis.torque_setpoint_ / axis.gear_ratio_): 0.0f; + // RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Writing velocities for ODrive %d Joint velocity of motor (rev/s): %f", axis.node_id_, (axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI)); + + axis.send(msg); + } else if (axis.torque_input_enabled_) { + Set_Input_Torque_msg_t msg; + msg.Input_Torque = axis.torque_setpoint_ / axis.gear_ratio_; + axis.send(msg); + } else { + // no control enabled - don't send any setpoint + } + } + } return return_type::OK; } @@ -376,6 +476,38 @@ void Axis::on_can_msg(const rclcpp::Time&, const can_frame& frame) { torque_target_ = msg.Torque_Target; torque_estimate_ = msg.Torque_Estimate; } + } break; + case Get_Temperature_msg_t::cmd_id: { + if(Get_Temperature_msg_t msg; try_decode(msg)) { + fet_temperature_ = msg.FET_Temperature; + motor_temperature_ = msg.Motor_Temperature; + } + } break; + case Get_Bus_Voltage_Current_msg_t::cmd_id: { + if(Get_Bus_Voltage_Current_msg_t msg; try_decode(msg)) { + bus_voltage_= msg.Bus_Voltage; + bus_current_ = msg.Bus_Current; + } + } break; + case Get_Error_msg_t::cmd_id: { + if(Get_Error_msg_t msg; try_decode(msg)) { + active_errors_= msg.Active_Errors; + disarm_reason_ = msg.Disarm_Reason; + } + } break; + case Get_Iq_msg_t::cmd_id: { + if(Get_Iq_msg_t msg; try_decode(msg)) { + iq_setpoint_= msg.Iq_Setpoint; + iq_measured_ = msg.Iq_Measured; + } + } break; + case Heartbeat_msg_t::cmd_id: { + if(Heartbeat_msg_t msg; try_decode(msg)) { + axis_error_ = msg.Axis_Error; + axis_state_ = msg.Axis_State; + procedure_result_ = msg.Procedure_Result; + trajectory_done_flag_ = msg.Trajectory_Done_Flag; + } } break; // silently ignore unimplemented command IDs } diff --git a/src/hardware_interfaces/rotary_encoder_ros2_control/CMakeLists.txt b/src/hardware_interfaces/rotary_encoder_ros2_control/CMakeLists.txt new file mode 100644 index 00000000..33322f45 --- /dev/null +++ b/src/hardware_interfaces/rotary_encoder_ros2_control/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.8) +project(rotary_encoder_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) +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() + +add_library( + rotary_encoder_ros2_control + SHARED + src/rotary_encoder_hardware_interface.cpp +) + +target_compile_features(rotary_encoder_ros2_control PUBLIC cxx_std_20) +target_include_directories(rotary_encoder_ros2_control PUBLIC + $ + $ +) + +ament_target_dependencies(rotary_encoder_ros2_control PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(rotary_encoder_ros2_control PUBLIC umdloop_can_library::umdloop_can_library) + +pluginlib_export_plugin_description_file(hardware_interface rotary_encoder_hardware_interface.xml) + +install( + DIRECTORY include/ + DESTINATION include/rotary_encoder_ros2_control +) + +install(TARGETS rotary_encoder_ros2_control + EXPORT export_rotary_encoder_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_rotary_encoder_ros2_control) +ament_export_include_directories(include) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/hardware_interfaces/rotary_encoder_ros2_control/include/rotary_encoder_ros2_control/rotary_encoder_hardware_interface.hpp b/src/hardware_interfaces/rotary_encoder_ros2_control/include/rotary_encoder_ros2_control/rotary_encoder_hardware_interface.hpp new file mode 100644 index 00000000..b5d55f5d --- /dev/null +++ b/src/hardware_interfaces/rotary_encoder_ros2_control/include/rotary_encoder_ros2_control/rotary_encoder_hardware_interface.hpp @@ -0,0 +1,99 @@ +#ifndef ROTARY_ENCODER_ROS2_CONTROL__ROTARY_ENCODER_HARDWARE_INTERFACE_HPP_ +#define ROTARY_ENCODER_ROS2_CONTROL__ROTARY_ENCODER_HARDWARE_INTERFACE_HPP_ + +#include +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "umdloop_can_library/CanFrame.hpp" +#include "umdloop_can_library/SocketCanBus.hpp" + +namespace CANLib +{ +struct CanFrame; +} + +namespace rotary_encoder_ros2_control +{ + +class RotaryEncoderHardwareInterface : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(RotaryEncoderHardwareInterface) + + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + void on_can_message(const CANLib::CanFrame & frame); + void logger_function(); + +private: + struct RotaryEncoderJoint + { + std::string name; + uint32_t node_id; + uint8_t rotary_encoder_id; + double joint_state_position; + double joint_state_velocity; + double state_request; + double prev_state_request; + double elapsed_state_request_time; + + std::vector state_interface_names; + std::vector command_interface_names; + std::unordered_map parameters; + }; + + void send_state_request(const RotaryEncoderJoint & joint); + static double position_from_raw(int16_t raw_position); + static double velocity_from_raw(int16_t raw_velocity); + + std::string can_interface_; + CANLib::SocketCanBus canBus; + CANLib::CanFrame can_rx_frame_; + + uint32_t can_write_id_ = 0; + uint32_t can_read_id_ = 0; + int update_rate_ = 0; + int logger_rate_ = 0; + int logger_state_ = 0; + double elapsed_time_ = 0.0; + double elapsed_logger_time_ = 0.0; + + std::vector rotary_encoder_joints_; + + static constexpr uint8_t ROTARY_ENCODER_STATE_CMD = 0x20; +}; + +} // namespace rotary_encoder_ros2_control + +#endif // ROTARY_ENCODER_ROS2_CONTROL__ROTARY_ENCODER_HARDWARE_INTERFACE_HPP_ diff --git a/src/hardware_interfaces/rotary_encoder_ros2_control/package.xml b/src/hardware_interfaces/rotary_encoder_ros2_control/package.xml new file mode 100644 index 00000000..df0c603d --- /dev/null +++ b/src/hardware_interfaces/rotary_encoder_ros2_control/package.xml @@ -0,0 +1,29 @@ + + + + rotary_encoder_ros2_control + 0.1.0 + Rotary encoder hardware interface for ros2_control + Ishan Dutta + Apache-2.0 + + ament_cmake + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + umdloop_can_library + umdloop_can_library + + ament_lint_auto + ament_lint_common + + ament_cmake + + + ament_cmake + + diff --git a/src/hardware_interfaces/rotary_encoder_ros2_control/rotary_encoder_hardware_interface.xml b/src/hardware_interfaces/rotary_encoder_ros2_control/rotary_encoder_hardware_interface.xml new file mode 100644 index 00000000..5a16271d --- /dev/null +++ b/src/hardware_interfaces/rotary_encoder_ros2_control/rotary_encoder_hardware_interface.xml @@ -0,0 +1,9 @@ + + + + Rotary encoder hardware interface via ros2_control joint state interfaces. + + + diff --git a/src/hardware_interfaces/rotary_encoder_ros2_control/src/rotary_encoder_hardware_interface.cpp b/src/hardware_interfaces/rotary_encoder_ros2_control/src/rotary_encoder_hardware_interface.cpp new file mode 100644 index 00000000..99351be9 --- /dev/null +++ b/src/hardware_interfaces/rotary_encoder_ros2_control/src/rotary_encoder_hardware_interface.cpp @@ -0,0 +1,295 @@ +#include "rotary_encoder_ros2_control/rotary_encoder_hardware_interface.hpp" + +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace rotary_encoder_ros2_control +{ + +double RotaryEncoderHardwareInterface::position_from_raw(int16_t raw_position) +{ + return static_cast(raw_position) * 0.1 * (M_PI / 180.0); +} + +double RotaryEncoderHardwareInterface::velocity_from_raw(int16_t raw_velocity) +{ + return static_cast(raw_velocity) * 0.01 * (M_PI / 180.0); +} + +void RotaryEncoderHardwareInterface::send_state_request(const RotaryEncoderJoint & joint) +{ + CANLib::CanFrame frame; + frame.id = can_write_id_; + frame.dlc = 8; + frame.data.fill(0x00); + frame.data[0] = static_cast( + ROTARY_ENCODER_STATE_CMD + static_cast(joint.node_id & 0xFF)); + frame.data[1] = joint.rotary_encoder_id; + canBus.send(frame); +} + +void RotaryEncoderHardwareInterface::logger_function() +{ + if (rotary_encoder_joints_.empty()) { + return; + } + + std::ostringstream oss; + oss << "\033[2J\033[H \nRotary Encoder Logger" + << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface_ + << " | Write CAN ID: 0x" << std::hex << std::uppercase << can_write_id_ + << " | Read CAN ID: 0x" << can_read_id_ << std::dec + << " | HWI Update Rate: " << update_rate_ + << " | Logger Update Rate: " << logger_rate_ << "\n" + << "Elapsed Time since first update: " << elapsed_time_ + << "\n--- Joint Specific ---"; + + for (const auto & joint : rotary_encoder_joints_) { + oss << "\nJOINT: " << joint.name + << " | Node ID: 0x" << std::hex << std::uppercase << joint.node_id + << " | Rotary Encoder ID: 0x" << static_cast(joint.rotary_encoder_id) + << std::dec + << "\n-- Commands --\n" + << "State Request: " << joint.state_request + << "\n-- State --\n" + << "Position: " << joint.joint_state_position + << " | Velocity: " << joint.joint_state_velocity << "\n"; + } + + RCLCPP_INFO(rclcpp::get_logger("RotaryEncoderHardwareInterface"), "%s", oss.str().c_str()); +} + +hardware_interface::CallbackReturn RotaryEncoderHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + can_interface_ = info_.hardware_parameters.count("can_interface") ? + info_.hardware_parameters.at("can_interface") : "can0"; + update_rate_ = info_.hardware_parameters.count("update_rate") ? + std::stoi(info_.hardware_parameters.at("update_rate")) : 10; + logger_rate_ = info_.hardware_parameters.count("logger_rate") ? + std::stoi(info_.hardware_parameters.at("logger_rate")) : 0; + logger_state_ = info_.hardware_parameters.count("logger_state") ? + std::stoi(info_.hardware_parameters.at("logger_state")) : 0; + + if (!info_.hardware_parameters.count("can_id")) { + RCLCPP_ERROR( + rclcpp::get_logger("RotaryEncoderHardwareInterface"), + "CAN ID parameter 'can_id' is missing in hardware configuration."); + return hardware_interface::CallbackReturn::ERROR; + } + + try { + can_write_id_ = static_cast( + std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); + } catch (const std::exception & e) { + RCLCPP_ERROR( + rclcpp::get_logger("RotaryEncoderHardwareInterface"), + "Failed to parse 'can_id': %s", e.what()); + return hardware_interface::CallbackReturn::ERROR; + } + can_read_id_ = can_write_id_ + 1; + + rotary_encoder_joints_.clear(); + for (const auto & joint : info_.joints) { + std::vector state_if_names; + for (const auto & si : joint.state_interfaces) { + state_if_names.push_back(si.name); + } + + std::vector command_if_names; + for (const auto & ci : joint.command_interfaces) { + command_if_names.push_back(ci.name); + } + + std::unordered_map params_map; + for (const auto & p : joint.parameters) { + params_map.emplace(p.first, p.second); + } + + const uint32_t node_id = static_cast( + std::stoul(joint.parameters.at("node_id"), nullptr, 0)); + const uint8_t rotary_encoder_id = static_cast( + std::stoul(joint.parameters.at("rotary_encoder_id"), nullptr, 0) & 0xFF); + + rotary_encoder_joints_.push_back(RotaryEncoderJoint{ + joint.name, + node_id, + rotary_encoder_id, + std::numeric_limits::quiet_NaN(), + 0.0, + 0.0, + 0.0, + 0.0, + state_if_names, + command_if_names, + params_map}); + } + + elapsed_time_ = 0.0; + elapsed_logger_time_ = 0.0; + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RotaryEncoderHardwareInterface::on_configure( + const rclcpp_lifecycle::State &) +{ + if (!canBus.open( + can_interface_, + std::bind(&RotaryEncoderHardwareInterface::on_can_message, this, std::placeholders::_1))) + { + RCLCPP_ERROR(rclcpp::get_logger("RotaryEncoderHardwareInterface"), "Failed to open CAN interface"); + return hardware_interface::CallbackReturn::ERROR; + } + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector +RotaryEncoderHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + for (auto & joint : rotary_encoder_joints_) { + for (const auto & iface : joint.state_interface_names) { + double * value = nullptr; + if (iface == hardware_interface::HW_IF_POSITION) { + value = &joint.joint_state_position; + } else if (iface == hardware_interface::HW_IF_VELOCITY) { + value = &joint.joint_state_velocity; + } else { + RCLCPP_WARN( + rclcpp::get_logger("RotaryEncoderHardwareInterface"), + "Unknown state interface '%s' for joint '%s'", + iface.c_str(), joint.name.c_str()); + continue; + } + state_interfaces.emplace_back(joint.name, iface, value); + } + } + return state_interfaces; +} + +std::vector +RotaryEncoderHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + for (auto & joint : rotary_encoder_joints_) { + for (const auto & iface : joint.command_interface_names) { + if (iface == "state_request") { + command_interfaces.emplace_back(joint.name, iface, &joint.state_request); + } else { + RCLCPP_WARN( + rclcpp::get_logger("RotaryEncoderHardwareInterface"), + "Unknown command interface '%s' for joint '%s'", + iface.c_str(), joint.name.c_str()); + } + } + } + return command_interfaces; +} + +hardware_interface::CallbackReturn RotaryEncoderHardwareInterface::on_activate( + const rclcpp_lifecycle::State &) +{ + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RotaryEncoderHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State &) +{ + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RotaryEncoderHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State &) +{ + canBus.close(); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RotaryEncoderHardwareInterface::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + return on_cleanup(previous_state); +} + +void RotaryEncoderHardwareInterface::on_can_message(const CANLib::CanFrame & frame) +{ + can_rx_frame_ = frame; + if (can_rx_frame_.id != can_read_id_) { + return; + } + + for (auto & joint : rotary_encoder_joints_) { + const uint8_t expected_command = static_cast( + ROTARY_ENCODER_STATE_CMD + static_cast(joint.node_id & 0xFF)); + if ( + can_rx_frame_.data[0] != expected_command || + can_rx_frame_.data[1] != joint.rotary_encoder_id) + { + continue; + } + + const auto raw_position = static_cast( + (static_cast(can_rx_frame_.data[3]) << 8) | + static_cast(can_rx_frame_.data[2])); + const auto raw_velocity = static_cast( + (static_cast(can_rx_frame_.data[5]) << 8) | + static_cast(can_rx_frame_.data[4])); + + joint.joint_state_position = position_from_raw(raw_position); + joint.joint_state_velocity = velocity_from_raw(raw_velocity); + return; + } +} + +hardware_interface::return_type RotaryEncoderHardwareInterface::read( + const rclcpp::Time &, const rclcpp::Duration &) +{ + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type RotaryEncoderHardwareInterface::write( + const rclcpp::Time &, const rclcpp::Duration & period) +{ + 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(); + } + } + + for (auto & joint : rotary_encoder_joints_) { + const double curr_state_req = joint.state_request; + if (curr_state_req < 0.0 && joint.prev_state_request >= 0.0) { + send_state_request(joint); + } else if (curr_state_req > 0.0) { + joint.elapsed_state_request_time += period.seconds(); + if (joint.elapsed_state_request_time > (1.0 / curr_state_req)) { + joint.elapsed_state_request_time = 0.0; + send_state_request(joint); + } + } + joint.prev_state_request = curr_state_req; + } + + return hardware_interface::return_type::OK; +} + +} // namespace rotary_encoder_ros2_control + +PLUGINLIB_EXPORT_CLASS( + rotary_encoder_ros2_control::RotaryEncoderHardwareInterface, + hardware_interface::SystemInterface) diff --git a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp index 177235a5..aa0a1c05 100644 --- a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp +++ b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp @@ -2,11 +2,13 @@ #define SERVO_HARDWARE_INTERACE_HPP_ #include + +#include +#include #include #include +#include #include -#include - #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -16,163 +18,255 @@ #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" +#include "umdloop_can_library/SocketCanBus.hpp" + +namespace CANLib +{ +struct CanFrame; +} namespace servo_ros2_control { -class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inheriting from System Interface + +class SERVOHardwareInterface : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(SERVOHardwareInterface) - // Initialization, so reading parameters, initializing variables, checking if all the joint state and command interfaces are correct hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - // Exports/exposes Interfaces that are available so that the controllers - // know what to read and write to std::vector export_state_interfaces() override; std::vector export_command_interfaces() override; - // Lifecycle 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; + 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; + enum class integration_level_t : std::uint8_t + { + UNDEFINED = 0, + POSITION = 1, + VELOCITY = 2, + }; + + enum class servo_type_t : std::uint8_t + { + STANDARD = 0, + CONTINUOUS = 1, + }; + + enum class joint_type_t : std::uint8_t + { + REVOLUTE = 0, + PRISMATIC = 1, + }; + + struct ServoJoint + { + std::string name; + uint8_t node_id; + int gear_ratio; + double rated_max; + double meters_per_deg; + servo_type_t servo_type; + joint_type_t joint_type; + integration_level_t control_level; + + double joint_state_position; + double joint_state_velocity; + double motor_position; + double motor_velocity; + double motor_status; + + double joint_command_position; + double joint_command_velocity; + double motor_status_req; + double motor_maintenance_req; + double maintenance_frame_high; + double maintenance_frame_low; + double maintenance_frame; + double maintenance_data_count; + std::vector decoded_maintenance_frame; + + double prev_status_req; + double prev_maintenance_req; + double elapsed_status_request_time; + double elapsed_maintenance_request_time; + double prev_joint_command_position; + double prev_joint_command_velocity; + + std::vector state_interface_names; + std::vector command_interface_names; + std::unordered_map parameters; + }; + + struct DecodedCommand + { + uint8_t command_id; + std::vector u8_data; + std::vector i16_data; + std::vector i32_data; + }; - // -- Helper Functions -- - void on_can_message(const CANLib::CanFrame& frame); + void on_can_message(const CANLib::CanFrame & frame); void logger_function(); double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); - double calculate_joint_displacement_from_motor_position(double motor_position, int gear_ratio, double meters_per_deg); + double calculate_joint_displacement_from_motor_position( + double motor_position, int gear_ratio, double meters_per_deg); double calculate_joint_angular_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); - double calculate_joint_linear_velocity_from_motor_velocity(double motor_velocity, int gear_ratio, double meters_per_deg); + double calculate_joint_linear_velocity_from_motor_velocity( + double motor_velocity, int gear_ratio, double meters_per_deg); 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 meters_per_deg); - 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 meters_per_deg); + int16_t calculate_motor_position_from_desired_joint_displacement( + double joint_position, int gear_ratio, double meters_per_deg); + 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 meters_per_deg); + + void format_control_command(CANLib::CanFrame & frame, ServoJoint & joint); + bool format_status_command(CANLib::CanFrame & frame, uint8_t command_id, uint8_t node_id); + bool format_maintenance_command( + CANLib::CanFrame & frame, uint8_t node_id, const DecodedCommand & decoded_cmd); private: - // Hardware Interface Parameters int update_rate; - double elapsed_update_time; // Time since last hardware interface update - double elapsed_time; // Time since first hardware interface update - double elapsed_logger_time; // Time since last logger update - int logger_rate; // Logger update rate - int logger_state; // Logger on/off state + double elapsed_update_time; + double elapsed_time; + double elapsed_logger_time; + int logger_rate; + int logger_state; int write_count; - - // Keeps track of amount of joints int num_joints; - - // Stores Arbitration IDs + int can_command_id; uint32_t can_response_id; - - // Store the state for the simulated robot - std::vector joint_state_position_; - std::vector joint_state_velocity_; - - // Store previous state for simulated robot - std::vector prev_joint_state_position_; - std::vector prev_joint_state_velocity_; - - // Store the command for the simulated robot - std::vector joint_command_position_; - std::vector joint_command_velocity_; - - // Store the command for the simulated robot - std::vector prev_joint_command_position_; - std::vector prev_joint_command_velocity_; - - // Place holders for data from the canBus, will be accessed in read() - std::vector motor_velocity; - std::vector motor_position; - std::vector rated_max; - std::vector meters_per_deg; - std::vector device_status; - - // CAN Library Setup CANLib::SocketCanBus canBus; CANLib::CanFrame can_tx_frame_; CANLib::CanFrame can_rx_frame_; std::string can_interface; - // Joint specific parameters - std::vector joint_node_ids; - std::vector joint_gear_ratios; + std::vector SERVOJoints_; - // Modes for control mode - enum class integration_level_t : std::uint8_t + enum class ControlCommands : uint8_t { - UNDEFINED = 0, - POSITION = 1, - VELOCITY = 2, + ABSOLUTE_POS_CONTROL_CMD = 0x20, + VELOCITY_CONTROL_CMD = 0x30, + }; + + enum class MaintenanceCommands : uint8_t + { + PCB_HEARTBEAT_CMD = 0x10, + MAINTENANCE_CMD = 0x60, + SERVO_SPECS_CMD = 0x70, }; - // Active control mode for each actuator - std::vector control_level_; + enum class MaintenanceCommandOptions : uint8_t + { + SET_CURRENT_MULTI_TURN_POS_ZERO_TO_ROM_CMD = 0x00, + MOTOR_STOP_CMD = 0x01, + MOTOR_SHUTDOWN_CMD = 0x02, + CLEAR_ERRORS_CMD = 0x03, + }; - enum class servo_type_t : std::uint8_t + enum class StatusCommands : uint8_t { - STANDARD = 0, - CONTINUOUS = 1, + MOTOR_STATE = 0x40, + MOTOR_STATUS = 0x50, }; - // Type of servo for each actuator - std::vector servo_type_; + enum class MotorStatus : uint8_t + { + UNDEFINED = 0, + IDLE = 1, + STARTUP_SEQUENCE = 2, + + ERROR_INVALID_REQUEST = 3, + ERROR_SERVO_DISARMED = 4, + ERROR_SERVO_FAILED = 5, + ERROR_CONTROLLER_FAILED = 6, + ERROR_ESTOP_REQUESTED = 7, + ERROR_UNKNOWN_POSITION = 8, + + POSITION_CONTROL = 9, + VELOCITY_CONTROL = 10, + SERVO_STOPPED = 11 + }; - // Types of joints - enum class joint_type_t : std::uint8_t + enum class ValidateRequest : uint8_t { - REVOLUTE = 0, - PRISMATIC = 1, + INVALID = 0, + VALID = 1, }; - // Type of joint for each actuator - std::vector joint_type_; - - // CAN Commands - 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 = 0x4; - static constexpr uint8_t MOTOR_STATUS_CMD = 0x5; - static constexpr uint8_t MAINTENANCE_CMD = 0x6; - static constexpr uint8_t SERVO_SPECS_CMD = 0x7; + static constexpr std::array kStatusCommands = { + StatusCommands::MOTOR_STATE, + StatusCommands::MOTOR_STATUS, + }; + + 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; + } + + template + inline void pack_decoded_maintenance_frame( + JointT & joint, const DecodedCommand & decoded_maintenance_cmd) + { + joint.decoded_maintenance_frame.clear(); + joint.decoded_maintenance_frame.push_back(decoded_maintenance_cmd.command_id); + joint.decoded_maintenance_frame.insert( + joint.decoded_maintenance_frame.end(), + decoded_maintenance_cmd.u8_data.begin(), + decoded_maintenance_cmd.u8_data.end()); + } }; -} // namespace servo_hardware_interface +} // namespace servo_ros2_control #endif // SERVO_HARDWARE_INTERACE_HPP_ diff --git a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp index d7807759..61c70128 100644 --- a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp +++ b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp @@ -1,603 +1,665 @@ #include "servo_ros2_control/servo_hardware_interface.hpp" -#include -#include -#include +#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 "pluginlib/class_list_macros.hpp" #include "rclcpp/rclcpp.hpp" -using std::placeholders::_1; - namespace servo_ros2_control { -double SERVOHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ - // Converts from deg to radians with gear ratio - return (motor_position * (M_PI/180.0))/gear_ratio; +double SERVOHardwareInterface::calculate_joint_position_from_motor_position( + double motor_position, int gear_ratio) +{ + return (motor_position * (M_PI / 180.0)) / gear_ratio; } -double SERVOHardwareInterface::calculate_joint_displacement_from_motor_position(double motor_position, int gear_ratio, double meters_per_deg){ - // Converts from deg to meters with gear ratio - return (motor_position * meters_per_deg)/gear_ratio; +double SERVOHardwareInterface::calculate_joint_displacement_from_motor_position( + double motor_position, int gear_ratio, double meters_per_deg) +{ + return (motor_position * meters_per_deg) / gear_ratio; } -double SERVOHardwareInterface::calculate_joint_angular_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ - // Converts from dps to radians/s with gear ratio - return (motor_velocity * (M_PI/180.0))/gear_ratio; +double SERVOHardwareInterface::calculate_joint_angular_velocity_from_motor_velocity( + double motor_velocity, int gear_ratio) +{ + return (motor_velocity * (M_PI / 180.0)) / gear_ratio; } -double SERVOHardwareInterface::calculate_joint_linear_velocity_from_motor_velocity(double motor_velocity, int gear_ratio, double meters_per_deg){ - // Converts from dps to meters/s with gear ratio - return (motor_velocity * meters_per_deg)/gear_ratio; +double SERVOHardwareInterface::calculate_joint_linear_velocity_from_motor_velocity( + double motor_velocity, int gear_ratio, double meters_per_deg) +{ + return (motor_velocity * meters_per_deg) / gear_ratio; } -int16_t SERVOHardwareInterface::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/M_PI))*gear_ratio)); +int16_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_position( + double joint_position, int gear_ratio) +{ + return static_cast(std::round((joint_position * (180.0 / M_PI)) * gear_ratio)); } -int16_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_displacement(double joint_position, int gear_ratio, double meters_per_deg){ - // m -> deg with gear ratio - return static_cast(std::round((joint_position*meters_per_deg*gear_ratio))); +int16_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_displacement( + double joint_position, int gear_ratio, double meters_per_deg) +{ + return static_cast(std::round((joint_position / meters_per_deg) * gear_ratio)); } -int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_angular_velocity(double joint_velocity, int gear_ratio){ - // radians/s -> deg/s with gear ratio - return static_cast(std::round((joint_velocity*(180/M_PI))*gear_ratio)); +int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_angular_velocity( + double joint_velocity, int gear_ratio) +{ + return static_cast(std::round((joint_velocity * (180.0 / M_PI)) * gear_ratio)); } -int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_linear_velocity(double joint_velocity, int gear_ratio, double meters_per_deg){ - // m/s -> deg/s with gear ratio - return static_cast(std::round((joint_velocity*meters_per_deg*gear_ratio))); +int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_linear_velocity( + double joint_velocity, int gear_ratio, double meters_per_deg) +{ + return static_cast(std::round((joint_velocity / meters_per_deg) * gear_ratio)); } -void SERVOHardwareInterface::logger_function(){ - - // Prevents breaking the logger - if (num_joints == 0) return; +void SERVOHardwareInterface::logger_function() +{ + if (num_joints == 0) { + return; + } - // Building Message - std::string log_msg = "\033[2J\033[H \nSERVO Logger"; std::ostringstream oss; - std::string status; - - // HWI Specific - oss << "\n--- HWI Specific ---\n" + oss << "\033[2J\033[H \nSERVO Logger" + << "\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 + << " | Response CAN ID: 0x" << can_response_id << std::dec << " | HWI Update Rate: " << 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 (device_status[i]) { - case 0x00: - status = "IDLE (ready)"; - break; - case 0x01: - status = "ACTIVE (busy)"; - break; - case 0x02: - status = "DOES NOT EXIST"; - break; - case 0x03: - status = "ERROR"; - break; - default: - status = "UNDEFINED"; - break; + for (const auto & joint : SERVOJoints_) { + std::string control_mode = "UNDEFINED"; + if (joint.control_level == integration_level_t::POSITION) { + control_mode = "POSITION"; + } else if (joint.control_level == integration_level_t::VELOCITY) { + control_mode = "VELOCITY"; } - oss << "\nJOINT: " << info_.joints[i].name << "\n" - << "Parameters: Node ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] - << " | Gear Ratio: " << joint_gear_ratios[i] - << " | Device Status: " << std::hex << std::uppercase << device_status[i] - << " | Control Mode: " << static_cast(control_level_[i]) - << " - " << status << "\n" + oss << "\nJOINT: " << joint.name << "\n" + << "Parameters: Node ID: 0x" << std::hex << std::uppercase + << static_cast(joint.node_id) << std::dec + << " | Gear Ratio: " << joint.gear_ratio + << " | Control Mode: " << control_mode << "\n" << "-- Commands --\n" - << "Motor Position: " << motor_position[i] - << " | Joint Command Position: " << joint_command_position_[i] << "\n" - << "Motor Velocity: " << motor_velocity[i] - << " | Joint Command Velocity: " << joint_command_velocity_[i] << "\n" + << "Motor Position: " << joint.motor_position + << " | Joint Command Position: " << joint.joint_command_position << "\n" + << "Motor Velocity: " << joint.motor_velocity + << " | Joint Command Velocity: " << joint.joint_command_velocity << "\n" + << "Status Request: " << joint.motor_status_req + << " | Maintenance Request: " << joint.motor_maintenance_req << "\n" << "-- State --\n" - << "Joint Position: " << joint_state_position_[i] - << " | Joint Velocity: " << joint_state_velocity_[i] << "\n"; + << "Joint Position: " << joint.joint_state_position + << " | Joint Velocity: " << joint.joint_state_velocity + << " | Status: " << joint.motor_status << "\n"; } - log_msg += oss.str(); - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), log_msg.c_str()); + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "%s", oss.str().c_str()); } - -hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( - const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file +void SERVOHardwareInterface::format_control_command(CANLib::CanFrame & frame, ServoJoint & joint) { - if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS){ - return hardware_interface::CallbackReturn::ERROR; - } - - // -- Parameters -- - for (auto& joint : info_.joints) { - joint_node_ids.push_back(std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x0, 0xF)); - joint_gear_ratios.push_back(std::abs(std::stoi(joint.parameters.at("gear_ratio")))); - - std::string servo_type = joint.parameters.at("servo_type"); - std::string joint_type = joint.parameters.at("joint_type"); - - // Standard or Continuous - if (servo_type == "standard" && joint.parameters.count("max_pos")) { - servo_type_.push_back(servo_type_t::STANDARD); - } else if (servo_type == "continuous" && joint.parameters.count("max_vel")) { - servo_type_.push_back(servo_type_t::CONTINUOUS); + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); + + if ( + joint.control_level == integration_level_t::POSITION && + joint.servo_type == servo_type_t::STANDARD && + std::isfinite(joint.joint_command_position) && + joint.joint_command_position != joint.prev_joint_command_position) + { + joint.prev_joint_command_position = joint.joint_command_position; + joint.joint_command_position = std::clamp(joint.joint_command_position, 0.0, joint.rated_max); + int16_t joint_angle = 0; + if (joint.joint_type == joint_type_t::REVOLUTE) { + joint_angle = calculate_motor_position_from_desired_joint_position( + joint.joint_command_position, joint.gear_ratio); } else { - RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid servo_type parameter for joint %s. Must be 'standard' with 'max_pos' or 'continuous' with 'max_vel'.", joint.name.c_str()); - return hardware_interface::CallbackReturn::ERROR; + joint_angle = calculate_motor_position_from_desired_joint_displacement( + joint.joint_command_position, joint.gear_ratio, joint.meters_per_deg); } - // Revolute or Prismatic - if (joint_type == "revolute") { - joint_type_.push_back(joint_type_t::REVOLUTE); - meters_per_deg.push_back(std::nan("")); // Not used for revolute joints - } else if (joint_type == "prismatic" && joint.parameters.count("meters_per_deg")) { - joint_type_.push_back(joint_type_t::PRISMATIC); - meters_per_deg.push_back(std::abs(std::stod(joint.parameters.at("meters_per_deg")))); + frame.dlc = 3; + frame.data[0] = static_cast( + static_cast(ControlCommands::ABSOLUTE_POS_CONTROL_CMD) + joint.node_id); + frame.data[1] = static_cast(joint_angle & 0xFF); + frame.data[2] = static_cast((joint_angle >> 8) & 0xFF); + return; + } + else if ( + joint.control_level == integration_level_t::VELOCITY && + joint.servo_type == servo_type_t::CONTINUOUS && + std::isfinite(joint.joint_command_velocity) && + joint.joint_command_velocity != joint.prev_joint_command_velocity) + { + joint.prev_joint_command_velocity = joint.joint_command_velocity; + joint.joint_command_velocity = std::clamp( + joint.joint_command_velocity, -joint.rated_max, joint.rated_max); + int16_t joint_velocity = 0; + if (joint.joint_type == joint_type_t::REVOLUTE) { + joint_velocity = calculate_motor_velocity_from_desired_joint_angular_velocity( + joint.joint_command_velocity, joint.gear_ratio); } else { - RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid joint_type parameter for joint %s. Must be 'revolute' or 'prismatic'. If prismatic, it must have a 'meters_per_deg' parameter.", joint.name.c_str()); - return hardware_interface::CallbackReturn::ERROR; + joint_velocity = calculate_motor_velocity_from_desired_joint_linear_velocity( + joint.joint_command_velocity, joint.gear_ratio, joint.meters_per_deg); } - // Determining rated max based on joint and servo type - if (joint_type_.back() == joint_type_t::REVOLUTE && servo_type_.back() == servo_type_t::STANDARD) { - rated_max.push_back(std::abs(std::stod(joint.parameters.at("max_pos")))*(M_PI/180.0)); // Convert from degrees to radians - } else if(joint_type_.back() == joint_type_t::REVOLUTE && servo_type_.back() == servo_type_t::CONTINUOUS){ - rated_max.push_back(std::abs(std::stod(joint.parameters.at("max_vel")))*(M_PI/180.0)); // Convert from dps to radians/s - } else if(joint_type_.back() == joint_type_t::PRISMATIC && servo_type_.back() == servo_type_t::STANDARD){ - rated_max.push_back(std::abs(std::stod(joint.parameters.at("max_pos")) * std::stod(joint.parameters.at("meters_per_deg")))); // Convert from degrees to meters - } else if(joint_type_.back() == joint_type_t::PRISMATIC && servo_type_.back() == servo_type_t::CONTINUOUS){ - rated_max.push_back(std::abs(std::stod(joint.parameters.at("max_vel")) * std::stod(joint.parameters.at("meters_per_deg")))); // Convert from dps to meters/s - } - else { - RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid combination of joint_type and servo_type for joint %s.", joint.name.c_str()); - return hardware_interface::CallbackReturn::ERROR; - } + frame.dlc = 3; + frame.data[0] = static_cast( + static_cast(ControlCommands::VELOCITY_CONTROL_CMD) + joint.node_id); + frame.data[1] = static_cast(joint_velocity & 0xFF); + frame.data[2] = static_cast((joint_velocity >> 8) & 0xFF); + return; + } + else { + frame.dlc = 2; + frame.data[0] = static_cast( + static_cast(StatusCommands::MOTOR_STATE) + joint.node_id); + frame.data[1] = static_cast(ValidateRequest::VALID); + } +} + +bool SERVOHardwareInterface::format_status_command( + CANLib::CanFrame & frame, uint8_t command_id, uint8_t node_id) +{ + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); + frame.dlc = 2; + switch (static_cast(command_id)) { + case StatusCommands::MOTOR_STATE: + frame.data[0] = static_cast(static_cast(StatusCommands::MOTOR_STATE) + node_id); + frame.data[1] = static_cast(ValidateRequest::VALID); + return true; + case StatusCommands::MOTOR_STATUS: + frame.data[0] = static_cast(static_cast(StatusCommands::MOTOR_STATUS) + node_id); + frame.data[1] = static_cast(ValidateRequest::VALID); + return true; + default: + return false; + } +} + +bool SERVOHardwareInterface::format_maintenance_command( + CANLib::CanFrame & frame, uint8_t node_id, const DecodedCommand & decoded_cmd) +{ + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); + frame.dlc = 2; + frame.data[0] = static_cast( + static_cast(MaintenanceCommands::MAINTENANCE_CMD) + node_id); + frame.data[1] = decoded_cmd.command_id; + + switch (static_cast(decoded_cmd.command_id)) { + case MaintenanceCommands::PCB_HEARTBEAT_CMD: + if(decoded_cmd.u8_data.size() != 1 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else { + frame.data[0] = static_cast(static_cast(MaintenanceCommands::PCB_HEARTBEAT_CMD)); + frame.data[1] = decoded_cmd.u8_data[0]; // Heartbeat value + return true; + } + case MaintenanceCommands::MAINTENANCE_CMD: + if(decoded_cmd.u8_data.size() != 1 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else { + frame.data[0] = static_cast(static_cast(MaintenanceCommands::MAINTENANCE_CMD) + node_id); + frame.data[1] = decoded_cmd.u8_data[0]; // Maintenance Command Value + return true; + } + case MaintenanceCommands::SERVO_SPECS_CMD: + if(decoded_cmd.u8_data.size() != 1 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else { + frame.data[0] = static_cast(static_cast(MaintenanceCommands::SERVO_SPECS_CMD) + node_id); + frame.data[1] = static_cast(ValidateRequest::VALID); // Invalid data format for this command + return true; + } + default: + return false; + } +} +hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; } - num_joints = static_cast(info_.joints.size()); + SERVOJoints_.clear(); 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; + can_response_id = static_cast(can_command_id + 0x01); + + for (const auto & joint : info_.joints) { + const std::string servo_type_param = joint.parameters.at("servo_type"); + const std::string joint_type_param = joint.parameters.at("joint_type"); + + servo_type_t servo_type = servo_type_t::STANDARD; + if (servo_type_param == "continuous") { + servo_type = servo_type_t::CONTINUOUS; + } + joint_type_t joint_type = joint_type_t::REVOLUTE; + if (joint_type_param == "prismatic") { + joint_type = joint_type_t::PRISMATIC; + } + + const double meters_per_deg = joint.parameters.count("meters_per_deg") ? + std::abs(std::stod(joint.parameters.at("meters_per_deg"))) : std::numeric_limits::quiet_NaN(); + + double rated_max = 0.0; + if (servo_type == servo_type_t::STANDARD) { + rated_max = std::abs(std::stod(joint.parameters.at("max_pos"))); + rated_max = joint_type == joint_type_t::REVOLUTE ? + rated_max * (M_PI / 180.0) : rated_max * meters_per_deg; + } else { + rated_max = std::abs(std::stod(joint.parameters.at("max_vel"))); + rated_max = joint_type == joint_type_t::REVOLUTE ? + rated_max * (M_PI / 180.0) : rated_max * meters_per_deg; + } + + std::vector state_if_names; + for (const auto & si : joint.state_interfaces) { + state_if_names.push_back(si.name); + } + + std::vector command_if_names; + for (const auto & ci : joint.command_interfaces) { + command_if_names.push_back(ci.name); + } + + std::unordered_map params_map; + for (const auto & p : joint.parameters) { + params_map.emplace(p.first, p.second); + } + + const integration_level_t initial_mode = + servo_type == servo_type_t::CONTINUOUS ? integration_level_t::VELOCITY : integration_level_t::POSITION; + + SERVOJoints_.push_back(ServoJoint{ + joint.name, + static_cast(std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0, 15)), + std::abs(std::stoi(joint.parameters.at("gear_ratio"))), + rated_max, + meters_per_deg, + servo_type, + joint_type, + initial_mode, + std::numeric_limits::quiet_NaN(), + 0.0, + 0.0, + 0.0, + 0.0, + std::numeric_limits::quiet_NaN(), + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + {}, + 0.0, + 0.0, + 0.0, + 0.0, + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + state_if_names, + command_if_names, + params_map + }); + } + + num_joints = static_cast(SERVOJoints_.size()); 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); - prev_joint_state_velocity_.assign(num_joints, 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); - prev_joint_command_velocity_.assign(num_joints, 0); - - motor_position.assign(num_joints, 0.0); - motor_velocity.assign(num_joints, 0.0); - device_status.assign(num_joints, 0); - - // Set initial control type based on servo type - control_level_.resize(num_joints, integration_level_t::POSITION); - for (int i = 0; i < num_joints; i++) { - if(servo_type_[i] == servo_type_t::STANDARD){ - control_level_[i] = integration_level_t::POSITION; - } else if(servo_type_[i] == servo_type_t::CONTINUOUS){ - control_level_[i] = integration_level_t::VELOCITY; - } else{ - control_level_[i] = integration_level_t::UNDEFINED; - } - } - return hardware_interface::CallbackReturn::SUCCESS; } - hardware_interface::CallbackReturn SERVOHardwareInterface::on_shutdown( const rclcpp_lifecycle::State & previous_state) { - // Reuse cleanup logic which shuts down the motor and then deinitializes shared pointers. - // Need this in case on_cleanup never gets called - return this->on_cleanup(previous_state); + return on_cleanup(previous_state); } - std::vector SERVOHardwareInterface::export_state_interfaces() { std::vector state_interfaces; + for (auto & joint : SERVOJoints_) { + for (const auto & iface : joint.state_interface_names) { + double * value = nullptr; + if (iface == hardware_interface::HW_IF_POSITION) { + value = &joint.joint_state_position; + } else if (iface == hardware_interface::HW_IF_VELOCITY) { + value = &joint.joint_state_velocity; + } else if (iface == "status") { + value = &joint.motor_status; + } - // Each SERVO motor corresponds to a different joint. - 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])); + if (value == nullptr) { + continue; + } + state_interfaces.emplace_back(joint.name, iface, value); + } } - return state_interfaces; } - -std::vector -SERVOHardwareInterface::export_command_interfaces() +std::vector SERVOHardwareInterface::export_command_interfaces() { std::vector command_interfaces; + for (auto & joint : SERVOJoints_) { + for (const auto & iface : joint.command_interface_names) { + double * value = nullptr; + if (iface == hardware_interface::HW_IF_POSITION) { + value = &joint.joint_command_position; + } else if (iface == hardware_interface::HW_IF_VELOCITY) { + value = &joint.joint_command_velocity; + } else if (iface == "status_request") { + value = &joint.motor_status_req; + } else if (iface == "maintenance_request") { + value = &joint.motor_maintenance_req; + } else if (iface == "maintenance_frame_high") { + value = &joint.maintenance_frame_high; + } else if (iface == "maintenance_frame_low") { + value = &joint.maintenance_frame_low; + } else if (iface == "maintenance_data_count") { + value = &joint.maintenance_data_count; + } - 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])); + if (value == nullptr) { + continue; + } + command_interfaces.emplace_back(joint.name, iface, value); + } } - return command_interfaces; } hardware_interface::CallbackReturn SERVOHardwareInterface::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - if (!canBus.open(can_interface, std::bind(&SERVOHardwareInterface::on_can_message, this, std::placeholders::_1))) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Failed to open CAN interface"); + if (!canBus.open( + can_interface, + std::bind(&SERVOHardwareInterface::on_can_message, this, std::placeholders::_1))) + { + RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Failed to open CAN interface"); return hardware_interface::CallbackReturn::ERROR; } - return hardware_interface::CallbackReturn::SUCCESS; } -void SERVOHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { - can_rx_frame_ = frame; // Store the received frame for processing in read() - - std::string result; - - int data[7] = {0x00}; - int8_t command_nibble = ((can_rx_frame_.data[0]>>4) & 0x0F); - int8_t device_id_nibble = (can_rx_frame_.data[0] & 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 && command_nibble == MOTOR_STATE_CMD && device_id_nibble == joint_node_ids[i]){ - - // DECODING CAN MESSAGE FOR VELOCITY - 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 - // uint32 -> int16 -> double (for calcs) - raw_motor_position = static_cast(static_cast((data[2] << 8) | data[1])); - - // VELOCITY - // uint16 -> int16 -> double (for calcs) - raw_motor_velocity = static_cast(static_cast((data[4] << 8) | data[3])); - - // CALCULATING JOINT STATE - if(joint_type_[i] == joint_type_t::REVOLUTE){ - motor_position[i] = calculate_joint_position_from_motor_position(raw_motor_position, joint_gear_ratios[i]); // radians - motor_velocity[i] = calculate_joint_angular_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i]); // radians/s - } - else if(joint_type_[i] == joint_type_t::PRISMATIC){ - motor_position[i] = calculate_joint_displacement_from_motor_position(raw_motor_position, joint_gear_ratios[i], meters_per_deg[i]); // meters - motor_velocity[i] = calculate_joint_linear_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i], meters_per_deg[i]); // meters/s - } - else{ - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); +void SERVOHardwareInterface::on_can_message(const CANLib::CanFrame & frame) +{ + can_rx_frame_ = frame; + if (frame.id != can_response_id) { + return; + } + + const uint8_t command_nibble = static_cast((frame.data[0] >> 4) & 0x0F); + const uint8_t device_id_nibble = static_cast(frame.data[0] & 0x0F); + + for (auto & joint : SERVOJoints_) { + if (joint.node_id != device_id_nibble) { + continue; + } + + if (command_nibble == (static_cast(StatusCommands::MOTOR_STATE) >> 4) || + command_nibble == (static_cast(ControlCommands::ABSOLUTE_POS_CONTROL_CMD) >> 4) || + command_nibble == (static_cast(ControlCommands::VELOCITY_CONTROL_CMD) >> 4)) { + const double raw_position = static_cast( + static_cast((frame.data[2] << 8) | frame.data[1])); + const double raw_velocity = static_cast( + static_cast((frame.data[4] << 8) | frame.data[3])); + + if (joint.joint_type == joint_type_t::REVOLUTE) { + joint.motor_position = calculate_joint_position_from_motor_position( + raw_position, joint.gear_ratio); + joint.motor_velocity = calculate_joint_angular_velocity_from_motor_velocity( + raw_velocity, joint.gear_ratio); + } else { + joint.motor_position = calculate_joint_displacement_from_motor_position( + raw_position, joint.gear_ratio, joint.meters_per_deg); + joint.motor_velocity = calculate_joint_linear_velocity_from_motor_velocity( + raw_velocity, joint.gear_ratio, joint.meters_per_deg); } + return; } - else if(can_rx_frame_.id == can_response_id && command_nibble == MOTOR_STATUS_CMD && device_id_nibble == joint_node_ids[i]){ - - // Populate device status - device_status[i] = can_rx_frame_.data[1]; + + if (command_nibble == (static_cast(StatusCommands::MOTOR_STATUS) >> 4)) { + joint.motor_status = static_cast(frame.data[1]); + return; } - else{ - if(logger_state == 1) { - // RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Reply not heard."); - } + + if (command_nibble == (static_cast(MaintenanceCommands::MAINTENANCE_CMD) >> 4) && frame.data[0] == 1) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Successfully sent maintenance command"); + return; } - } + if (command_nibble == (static_cast(MaintenanceCommands::SERVO_SPECS_CMD) >> 4)) { + const uint8_t command_id = frame.data[0]; + const uint8_t servo_type = frame.data[1]; + + const uint16_t max_servo_position = + static_cast(frame.data[2]) | + (static_cast(frame.data[3]) << 8); + + const uint16_t max_servo_velocity = + static_cast(frame.data[4]) | + (static_cast(frame.data[5]) << 8); + + RCLCPP_INFO( + rclcpp::get_logger("SERVOHardwareInterface"), + "Servo Config Reply | " + "command_id: 0x%02X (%u) | " + "servo_type: %u | " + "max_servo_position: %u | " + "max_servo_velocity: %u", + command_id, + command_id, + servo_type, + max_servo_position, + max_servo_velocity + ); + return; + } + } } hardware_interface::CallbackReturn SERVOHardwareInterface::on_cleanup( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Cleaning up ...please wait..."); - - // If cleanup occurs before shutdown, this is the last opportunity to shutdown motor since pointers must be deleted here - int8_t command_nibble = MAINTENANCE_CMD; - int8_t maintenance_command = 2; // Motor Shutdown Command - int8_t device_id_nibble; - 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; - - device_id_nibble = joint_node_ids[i] & 0x0F; - can_tx_frame_.data[0] = static_cast((command_nibble << 4) | device_id_nibble); - can_tx_frame_.data[1] = maintenance_command; - canBus.send(can_tx_frame_); + for (const auto & joint : SERVOJoints_) { + CANLib::CanFrame frame; + frame.id = can_command_id; + if (format_maintenance_command( + frame, joint.node_id, + DecodedCommand{ + static_cast(MaintenanceCommands::MAINTENANCE_CMD), + {static_cast(MaintenanceCommandOptions::MOTOR_SHUTDOWN_CMD)}, + {}, + {} + } + ) + ) + { + canBus.send(frame); + } } - - // Close CAN bus canBus.close(); - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Cleaning up successful!"); - return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn SERVOHardwareInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Activating ...please wait..."); - - // Sets initial command to joint state - joint_command_position_ = joint_state_position_; - - for (size_t i = 0; i < joint_command_position_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %zu command position initialized to: %f", i, joint_command_position_[i]); + for (auto & joint : SERVOJoints_) { + joint.joint_command_position = joint.joint_state_position; } - - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Successfully activated!"); - return hardware_interface::CallbackReturn::SUCCESS; } - hardware_interface::CallbackReturn SERVOHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Deactivating ...please wait..."); - int8_t command_nibble = MAINTENANCE_CMD; - int8_t maintenance_command = 1; // Motor Stop Command - int8_t device_id_nibble; - - 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; - - // Motor Stop Command - device_id_nibble = joint_node_ids[i] & 0x0F; - can_tx_frame_.data[0] = static_cast((command_nibble << 4) | device_id_nibble); - can_tx_frame_.data[1] = maintenance_command; - canBus.send(can_tx_frame_); + for (const auto & joint : SERVOJoints_) { + CANLib::CanFrame frame; + frame.id = can_command_id; + if (format_maintenance_command( + frame, joint.node_id, + DecodedCommand{ + static_cast(MaintenanceCommands::MAINTENANCE_CMD), + {static_cast(MaintenanceCommandOptions::MOTOR_STOP_CMD)}, + {}, + {} + } + ) + ) + { + canBus.send(frame); + } } - - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Successfully deactivated all SERVO motors!"); - return hardware_interface::CallbackReturn::SUCCESS; } - hardware_interface::return_type SERVOHardwareInterface::read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time &, const rclcpp::Duration &) { - 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]; - } - - // ACCOUNTING FOR DEVICE STATUS - if(device_status[i] != 0x00 && device_status[i] != 0x01 && device_status[i] != -1){ - return hardware_interface::return_type::ERROR; - } + for (auto & joint : SERVOJoints_) { + joint.joint_state_position = joint.motor_position; + joint.joint_state_velocity = joint.motor_velocity; } - return hardware_interface::return_type::OK; } - -hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +hardware_interface::return_type SERVOHardwareInterface::write( + const rclcpp::Time &, const rclcpp::Duration & period) { - elapsed_update_time+=period.seconds(); - double update_period = 1.0/update_rate; - int data[3] = {0x00}; - int8_t command_nibble; - int8_t device_id_nibble; - int16_t joint_angle = 0; - int16_t joint_velocity = 0; - - elapsed_time+=period.seconds(); - - // Logger update - elapsed_logger_time+=period.seconds(); - double logging_period = 1.0/logger_rate; - if(elapsed_logger_time > logging_period){ + 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(); } } - // HWI can only go as fast as the controller manager. To limit frequency of bus messages, - // keep track of time passed over iterations of this function and if it exceeds the - // desired frequency of the HWI, skip message - if(elapsed_update_time > update_period){ - elapsed_update_time = 0.0; - - // Motor Command for each joint at given frequency - // for(int i = 0; i < num_joints; i++) { // TESTING - int i = write_count % num_joints; // TESTING - write_count+=1; - - // Motor Command for each joint at given frequency - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_command_id; - can_tx_frame_.dlc = 3; - - if( - control_level_[i] == integration_level_t::POSITION && - servo_type_[i] == servo_type_t::STANDARD && - std::isfinite(joint_command_position_[i]) && - joint_command_position_[i] != prev_joint_command_position_[i] - ) { - // CALCULATE DESIRED JOINT ANGLE - joint_command_position_[i] = std::clamp(joint_command_position_[i], 0.0, rated_max[i]); // Input must be within bounds of rated max (positive and either rad or m) - if(joint_type_[i] == joint_type_t::REVOLUTE){ - joint_angle = calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); - } - else if(joint_type_[i] == joint_type_t::PRISMATIC){ - joint_angle = calculate_motor_position_from_desired_joint_displacement(joint_command_position_[i], joint_gear_ratios[i], meters_per_deg[i]); - } - else{ - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); - } - - // RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), - // "Joint %s: cmd_pos=%.4f, joint_angle=%d \n", - // info_.joints[i].name.c_str(), joint_command_position_[i], (int)joint_angle); - - // ENCODING CAN MESSAGE - data[0] = ABSOLUTE_POS_CONTROL_CMD + joint_node_ids[i]; - data[1] = joint_angle & 0xFF; - data[2] = (joint_angle >> 8) & 0xFF; - - // Cast data to uint8_t - for(int j = 0; j < 3; j++){ - data[j] = static_cast(data[j]); - can_tx_frame_.data[j] = data[j]; - } - - // Save previous joint command position - prev_joint_command_position_[i] = joint_command_position_[i]; - } - else if( - control_level_[i] == integration_level_t::VELOCITY && - servo_type_[i] == servo_type_t::CONTINUOUS && - std::isfinite(joint_command_velocity_[i]) && - joint_command_velocity_[i] != prev_joint_command_velocity_[i] - ) { - // CALCULATE DESIRED JOINT VELOCITY - joint_command_velocity_[i] = std::clamp(joint_command_velocity_[i], -rated_max[i], rated_max[i]); // Input must be within bounds of rated max (all real values and either rad or m) - joint_velocity = joint_command_velocity_[i]; - if(joint_type_[i] == joint_type_t::REVOLUTE){ - joint_velocity = calculate_motor_velocity_from_desired_joint_angular_velocity(joint_velocity, joint_gear_ratios[i]); - } - else if(joint_type_[i] == joint_type_t::PRISMATIC){ - joint_velocity = calculate_motor_velocity_from_desired_joint_linear_velocity(joint_velocity, joint_gear_ratios[i], meters_per_deg[i]); - } - else{ - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); - } - - // ENCODING CAN MESSAGE - data[0] = VELOCITY_CONTROL_CMD + joint_node_ids[i]; - data[1] = joint_velocity & 0xFF; - data[2] = (joint_velocity >> 8) & 0xFF; - - // Cast data to uint8_t - for(int j = 0; j < 3; j++){ - data[j] = static_cast(data[j]); - can_tx_frame_.data[j] = data[j]; - } - - // Save previous joint command velocity - prev_joint_command_velocity_[i] = joint_command_velocity_[i]; + for (auto & joint : SERVOJoints_) { + const double curr_status_req = joint.motor_status_req; + if (curr_status_req < 0.0 && joint.prev_status_req >= 0.0) { + for (auto status_cmd : kStatusCommands) { + CANLib::CanFrame frame; + frame.id = can_command_id; + if (format_status_command(frame, static_cast(status_cmd), joint.node_id)) { + canBus.send(frame); + } } - else - { - command_nibble = MOTOR_STATE_CMD; // Motor State Command for each joint at given frequency - can_tx_frame_.dlc = 1; - device_id_nibble = joint_node_ids[i] & 0x0F; - can_tx_frame_.data = { static_cast((command_nibble << 4) | device_id_nibble) }; - - if (logger_state == 0) { - if (control_level_[i] == integration_level_t::POSITION && servo_type_[i] == servo_type_t::CONTINUOUS) { - RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is continuous type and cannot be position controlled.", info_.joints[i].name.c_str()); - } - if (control_level_[i] == integration_level_t::VELOCITY && servo_type_[i] == servo_type_t::STANDARD) { - RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is standard type and cannot be velocity controlled.", info_.joints[i].name.c_str()); - } - if (control_level_[i] == integration_level_t::UNDEFINED) { - RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s has undefined control level. Please ensure it is properly configured.", info_.joints[i].name.c_str()); - } - if(std::isfinite(joint_command_velocity_[i]) == false && control_level_[i] == integration_level_t::VELOCITY){ - RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s has non-finite velocity command.", info_.joints[i].name.c_str()); - } - if(std::isfinite(joint_command_position_[i]) == false && control_level_[i] == integration_level_t::POSITION){ - RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s has non-finite position command.", info_.joints[i].name.c_str()); + } 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; + for (auto status_cmd : kStatusCommands) { + CANLib::CanFrame frame; + frame.id = can_command_id; + if (format_status_command(frame, static_cast(status_cmd), joint.node_id)) { + canBus.send(frame); } } } - - canBus.send(can_tx_frame_); - // } + } + joint.prev_status_req = curr_status_req; + } + + for (auto & joint : SERVOJoints_) { + 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)); + pack_decoded_maintenance_frame(joint, decoded_maintenance_cmd); + + CANLib::CanFrame frame; + frame.id = can_command_id; + if (!format_maintenance_command(frame, joint.node_id, decoded_maintenance_cmd)) { + joint.prev_maintenance_req = joint.motor_maintenance_req; + continue; + } + + const double curr_maintenance_req = joint.motor_maintenance_req; + if (curr_maintenance_req < 0.0 && joint.prev_maintenance_req >= 0.0) { + canBus.send(frame); + } else if (curr_maintenance_req > 0.0) { + joint.elapsed_maintenance_request_time += period.seconds(); + if (joint.elapsed_maintenance_request_time > (1.0 / curr_maintenance_req)) { + joint.elapsed_maintenance_request_time = 0.0; + canBus.send(frame); + } + } + joint.prev_maintenance_req = curr_maintenance_req; + } + + elapsed_update_time += period.seconds(); + if (update_rate > 0 && elapsed_update_time > (1.0 / static_cast(update_rate))) { + elapsed_update_time = 0.0; + if (!SERVOJoints_.empty()) { + auto & joint = SERVOJoints_[static_cast(write_count % num_joints)]; + ++write_count; + CANLib::CanFrame frame; + frame.id = can_command_id; + format_control_command(frame, joint); + canBus.send(frame); + } } return hardware_interface::return_type::OK; } hardware_interface::return_type SERVOHardwareInterface::perform_command_mode_switch( - const std::vector& start_interfaces, - const std::vector& stop_interfaces) + const std::vector & start_interfaces, + const std::vector & stop_interfaces) { - // Debug: print incoming requests - std::ostringstream ss; - ss << "perform_command_mode_switch called. start_interfaces: ["; - for (auto &s : start_interfaces) ss << s << ","; - ss << "] stop_interfaces: ["; - for (auto &s : stop_interfaces) ss << s << ","; - ss << "]"; - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), ss.str().c_str()); - - // For each joint, decide its new control mode based on start/stop interfaces. - // We allow partial starts/stops: only affected joints are switched. - std::vector requested_modes(num_joints, integration_level_t::UNDEFINED); - - // Process stop interfaces first: mark those joints as UNDEFINED - for (const auto &ifname : stop_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 || ifname == vel_if || ifname.find(info_.joints[i].name) != std::string::npos) { + std::vector requested_modes( + static_cast(num_joints), integration_level_t::UNDEFINED); + + for (const auto & ifname : stop_interfaces) { + for (size_t i = 0; i < SERVOJoints_.size(); ++i) { + const auto & joint = SERVOJoints_[i]; + const std::string pos_if = joint.name + "/" + hardware_interface::HW_IF_POSITION; + const std::string vel_if = joint.name + "/" + hardware_interface::HW_IF_VELOCITY; + if (ifname == pos_if || ifname == vel_if || ifname.find(joint.name) != std::string::npos) { requested_modes[i] = integration_level_t::UNDEFINED; } } } - // Process start interfaces: set POSITION or VELOCITY per joint - 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); + for (const auto & ifname : start_interfaces) { + for (size_t i = 0; i < SERVOJoints_.size(); ++i) { + const auto & joint = SERVOJoints_[i]; + const std::string pos_if = joint.name + "/" + hardware_interface::HW_IF_POSITION; + const std::string vel_if = joint.name + "/" + hardware_interface::HW_IF_VELOCITY; if (ifname == pos_if) { requested_modes[i] = integration_level_t::POSITION; } else if (ifname == vel_if) { @@ -606,54 +668,36 @@ hardware_interface::return_type SERVOHardwareInterface::perform_command_mode_swi } } - // Now apply the requested_modes to control_level_. - // For any joint with UNDEFINED in requested_modes, we only change it if it was explicitly stopped. - for (int i = 0; i < num_joints; ++i) { + for (size_t i = 0; i < SERVOJoints_.size(); ++i) { + auto & joint = SERVOJoints_[i]; if (requested_modes[i] == integration_level_t::UNDEFINED) { - // if stop requested, set to UNDEFINED; otherwise leave existing mode - // (we only set to UNDEFINED if this joint was mentioned in stop_interfaces) bool was_stopped = false; - for (const auto &ifname : stop_interfaces) { - if (ifname.find(info_.joints[i].name) != std::string::npos) { + for (const auto & ifname : stop_interfaces) { + if (ifname.find(joint.name) != std::string::npos) { was_stopped = true; break; } } if (was_stopped) { - control_level_[i] = integration_level_t::UNDEFINED; - joint_command_velocity_[i] = 0; - // optional: reset position cmd to current state to be safe - joint_command_position_[i] = joint_state_position_[i]; - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), - "Joint %s: stopped -> set UNDEFINED", info_.joints[i].name.c_str()); + joint.control_level = integration_level_t::UNDEFINED; + joint.joint_command_velocity = 0.0; + joint.joint_command_position = joint.joint_state_position; } - // else, leave control_level_ as-is + continue; + } + + joint.control_level = requested_modes[i]; + if (joint.control_level == integration_level_t::VELOCITY) { + joint.joint_command_velocity = 0.0; } else { - // Set the mode requested - control_level_[i] = requested_modes[i]; - // If switching to velocity, optionally set command velocity to current state to avoid jumps - if (requested_modes[i] == integration_level_t::VELOCITY) { - // joint_command_velocity_[i] = joint_state_velocity_[i]; - joint_command_velocity_[i] = 0; - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), - "Joint %s: switched to VELOCITY (cmd vel initialized from state: %f)", - info_.joints[i].name.c_str(), joint_command_velocity_[i]); - } else if (requested_modes[i] == integration_level_t::POSITION) { - joint_command_position_[i] = joint_state_position_[i]; - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), - "Joint %s: switched to POSITION (cmd pos initialized from state: %f)", - info_.joints[i].name.c_str(), joint_command_position_[i]); - } + joint.joint_command_position = joint.joint_state_position; } } return hardware_interface::return_type::OK; } - -} // namespace servo_hardware_interface - -#include "pluginlib/class_list_macros.hpp" +} // namespace servo_ros2_control PLUGINLIB_EXPORT_CLASS( - servo_ros2_control::SERVOHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file + servo_ros2_control::SERVOHardwareInterface, hardware_interface::SystemInterface) diff --git a/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp b/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp index 275d3ed4..4b485a55 100644 --- a/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp +++ b/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp @@ -2,11 +2,14 @@ #define SMC_HARDWARE_INTERACE_HPP_ #include + +#include +#include +#include #include #include +#include #include -#include - #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -16,31 +19,29 @@ #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" +#include "umdloop_can_library/SocketCanBus.hpp" + +namespace CANLib +{ +struct CanFrame; +} namespace smc_ros2_control { -class SMCHardwareInterface : public hardware_interface::SystemInterface // Inheriting from System Interface + +class SMCHardwareInterface : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(SMCHardwareInterface) - // Initialization, so reading parameters, initializing variables, checking if all the joint state and command interfaces are correct + // -- Lifecycle Functions -- hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - // Exports/exposes Interfaces that are available so that the controllers - // know what to read and write to std::vector export_state_interfaces() override; std::vector export_command_interfaces() override; - - // -- Lifecycle Functions -- + hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; @@ -57,25 +58,102 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher 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; + 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; + enum class integration_level_t : std::uint8_t + { + UNDEFINED = 0, + POSITION = 1, + VELOCITY = 2, + }; + + enum class SMCMotorStatus : std::uint8_t + { + OK = 0x00, + LOW_VOLTAGE = 0x01, + OVER_TEMPERATURE = 0x08, + }; + + struct SMCJoint + { + std::string name; + uint32_t node_id; + int gear_ratio; + int orientation; + uint16_t operating_velocity; + integration_level_t control_level; + + double joint_state_position; + double joint_state_velocity; + double motor_temperature; + double motor_torque_current; + double motor_status; + double current_Kp; + double current_Ki; + double current_Kd; + double speed_Kp; + double speed_Ki; + double speed_Kd; + double position_Kp; + double position_Ki; + double position_Kd; + double acceleration; + double max_torque; + double max_speed; + double max_angle; + double current_ramp; + double speed_ramp; + + double joint_command_position; + double joint_command_velocity; + double motor_status_req; + double motor_maintenance_req; + double maintenance_frame_high; + double maintenance_frame_low; + double maintenance_frame; + double maintenance_data_count; + std::vector decoded_maintenance_frame; + + double prev_status_req; + double prev_maintenance_req; + double elapsed_status_request_time; + double elapsed_maintenance_request_time; + double motor_velocity; + double motor_position; + double encoder_position; + double prev_joint_command_position; + double prev_joint_command_velocity; + + std::vector state_interface_names; + std::vector command_interface_names; + std::unordered_map parameters; + }; + + struct DecodedCommand + { + uint8_t command_id; + std::vector u8_data; + std::vector i16_data; + std::vector i32_data; + }; - // -- Helper Functions -- void send_command(int can_id, int cmd_id); - void on_can_message(const CANLib::CanFrame& frame); + void on_can_message(const CANLib::CanFrame & frame); void logger_function(); double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); double calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); int32_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); int32_t calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio); + bool interpret_settings_parameters(SMCJoint & joint, const std::array & data); + void format_control_command(CANLib::CanFrame & frame, SMCJoint & joint); + bool format_status_command(CANLib::CanFrame & frame, uint8_t command_id); + bool format_maintenance_command(CANLib::CanFrame & frame, const DecodedCommand & decoded_cmd); private: // Hardware Interface Parameters @@ -86,61 +164,119 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher int logger_rate; // Logger update rate int logger_state; // Logger on/off state - // Keeps track of amount of joints int num_joints; + int state_iterator; - // Store the state for the simulated robot - std::vector joint_state_position_; - std::vector joint_state_velocity_; - - // Store the command for the simulated robot - std::vector joint_command_position_; - std::vector joint_command_velocity_; - - // Place holders for data from the canBus, will be accessed in read() - std::vector encoder_position; - std::vector motor_velocity; - std::vector motor_position; - - // Telemetry data from CAN responses - std::vector motor_temperature_; // Motor temperature in °C - std::vector motor_torque_current_; // Motor torque current in A - - // Velocity at which **joint** rotates to reach position in 1 dps - uint16_t operating_velocity; - - // CAN Library Setup CANLib::SocketCanBus canBus; CANLib::CanFrame can_tx_frame_; CANLib::CanFrame can_rx_frame_; std::string can_interface; - // Joint specific parameters - std::vector joint_node_ids; - std::vector joint_gear_ratios; - std::vector joint_orientation; - std::vector joint_initialization_; - - // Modes for control mode - enum integration_level_t : std::uint8_t + std::vector SMCJoints_; + + enum class MaintenanceCommands : uint8_t { - UNDEFINED = 0, - POSITION = 1, - VELOCITY = 2, + WRITE_ACCELERATION_CMD = 0x34, + READ_SETTINGS_CMD = 0x40, + WRITE_SETTINGS_TO_RAM_CMD = 0x42, + WRITE_SETTINGS_TO_ROM_CMD = 0x44, + MOTOR_SHUTDOWN_CMD = 0x80, + MOTOR_STOP_CMD = 0x81, + MOTOR_RUNNING_CMD = 0x88, + CLEAR_MOTOR_ANGLE_CMD = 0x95, + CLEAR_ERROR_CMD = 0x9B, }; - // Active control mode for each actuator - std::vector control_level_; + enum class ControlCommands : uint8_t + { + TORQUE_CONTROL_CMD = 0xA1, + SPEED_CONTROL_CMD = 0xA2, + ABSOLUTE_POS_WITH_VEL_CONTROL_CMD = 0xA4, + }; - // CAN Commands - static constexpr uint8_t MOTOR_SHUTDOWN_CMD = 0X80; - static constexpr uint8_t MOTOR_STOP_CMD = 0x81; - static constexpr uint8_t MOTOR_RUNNING_CMD = 0x88; - static constexpr uint8_t MOTOR_STATUS_2_CMD = 0X9C; - static constexpr uint8_t SPEED_CONTROL_CMD = 0xA2; - static constexpr uint8_t ABSOLUTE_POS_CONTROL_CMD = 0xA4; -}; + enum class StatusCommands : uint8_t + { + READ_ACCELERATION_CMD = 0x33, + READ_ENCODER_CMD = 0x90, + READ_ABS_ANGLE_CMD = 0x92, + MOTOR_STATUS_1_CMD = 0x9A, + MOTOR_STATUS_2_CMD = 0x9C, + }; + + enum class ParamID : uint8_t + { + ANGLE_PID = 0x96, + SPEED_PID = 0x97, + CURRENT_PID = 0x98, + MAX_TORQUE = 0x99, + MAX_SPEED = 0x9A, + MAX_ANGLE_A = 0x9B, + MAX_ANGLE_B = 0x9C, + CURRENT_RAMP = 0x9D, + SPEED_RAMP = 0x9E, + }; + + static constexpr std::array kStatusCommands = { + StatusCommands::READ_ACCELERATION_CMD, + StatusCommands::READ_ENCODER_CMD, + StatusCommands::READ_ABS_ANGLE_CMD, + StatusCommands::MOTOR_STATUS_1_CMD, + StatusCommands::MOTOR_STATUS_2_CMD, + }; + + 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); -} // namespace smc_hardware_interface + 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; + }; -#endif // SMC_HARDWARE_INTERACE_HPP_ \ No newline at end of file + 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; + } + + template + inline void pack_decoded_maintenance_frame( + JointT & joint, const DecodedCommand & decoded_maintenance_cmd) + { + joint.decoded_maintenance_frame.clear(); + joint.decoded_maintenance_frame.push_back(decoded_maintenance_cmd.command_id); + joint.decoded_maintenance_frame.insert( + joint.decoded_maintenance_frame.end(), + decoded_maintenance_cmd.u8_data.begin(), + decoded_maintenance_cmd.u8_data.end()); + + const auto * i16_ptr = reinterpret_cast(decoded_maintenance_cmd.i16_data.data()); + joint.decoded_maintenance_frame.insert( + joint.decoded_maintenance_frame.end(), + i16_ptr, + i16_ptr + decoded_maintenance_cmd.i16_data.size() * sizeof(int16_t)); + + const auto * i32_ptr = reinterpret_cast(decoded_maintenance_cmd.i32_data.data()); + joint.decoded_maintenance_frame.insert( + joint.decoded_maintenance_frame.end(), + i32_ptr, + i32_ptr + decoded_maintenance_cmd.i32_data.size() * sizeof(int32_t)); + } +}; +} // namespace smc_ros2_control +#endif // SMC_HARDWARE_INTERACE_HPP_ diff --git a/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp b/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp index 7664791c..a0ed83d8 100644 --- a/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp +++ b/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp @@ -1,68 +1,65 @@ #include "smc_ros2_control/smc_hardware_interface.hpp" -#include -#include -#include +#include +#include #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 "pluginlib/class_list_macros.hpp" #include "rclcpp/rclcpp.hpp" namespace smc_ros2_control { -double SMCHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ - // Converts from 0.01 deg to deg to radians/s - return (motor_position * 0.01 * (M_PI/180.0))/gear_ratio; +// -- HELPER FUNCTIONS -- // + +double SMCHardwareInterface::calculate_joint_position_from_motor_position( + double motor_position, int gear_ratio) +{ + return (motor_position * 0.01 * (M_PI / 180.0)) / gear_ratio; } -double SMCHardwareInterface::calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ - // Converts from dps to radians/s - return (motor_velocity * (M_PI/180.0))/gear_ratio; +double SMCHardwareInterface::calculate_joint_velocity_from_motor_velocity( + double motor_velocity, int gear_ratio) +{ + return (motor_velocity * (M_PI / 180.0)) / gear_ratio; } -int32_t SMCHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ - // radians -> deg -> 0.01 deg - return static_cast(std::round((joint_position*(180/M_PI)*100)*gear_ratio)); +int32_t SMCHardwareInterface::calculate_motor_position_from_desired_joint_position( + double joint_position, int gear_ratio) +{ + return static_cast(std::round((joint_position * (180.0 / M_PI) * 100.0) * gear_ratio)); } -int32_t SMCHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio){ - // radians/s -> deg/s -> 0.01 deg/s - return static_cast(std::round((joint_velocity*(180/M_PI)*100)*gear_ratio)); +int32_t SMCHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity( + double joint_velocity, int gear_ratio) +{ + return static_cast(std::round((joint_velocity * (180.0 / M_PI) * 100.0) * gear_ratio)); } -void SMCHardwareInterface::send_command(int can_id, int cmd_id){ +void SMCHardwareInterface::send_command(int can_id, int cmd_id) +{ CANLib::CanFrame frame; - frame.id = can_id; + frame.id = can_id; frame.dlc = 8; - frame.data.fill(0); - frame.data[0] = cmd_id; + frame.data.fill(0x00); + frame.data[0] = static_cast(cmd_id); canBus.send(frame); } -void SMCHardwareInterface::logger_function(){ - - // Prevents breaking the logger - if (num_joints == 0) return; +void SMCHardwareInterface::logger_function() +{ + // Prevent breaking the logger + if (SMCJoints_.empty()) { + return; + } - // Building Message std::string log_msg = "\033[2J\033[H \nSMC Logger"; std::string control_mode = ""; - - // HWI Specific - std::ostringstream oss; + std::ostringstream oss; oss << "\n--- HWI Specific ---\n" << "CAN Interface: " << can_interface << " | HWI Update Rate: " << update_rate @@ -70,390 +67,708 @@ void SMCHardwareInterface::logger_function(){ << "Elapsed Time since first update: " << elapsed_time << "\n" << "\n--- Joint Specific ---"; - for (int i = 0; i < num_joints; i++) { - if(static_cast(control_level_[i]) == 1) { + for (auto & joint : SMCJoints_) { + + if (joint.control_level == integration_level_t::POSITION) { control_mode = "POSITION"; - } - else if(static_cast(control_level_[i]) == 2) { + } else if (joint.control_level == integration_level_t::VELOCITY) { control_mode = "VELOCITY"; - } - else { + } else { control_mode = "UNDEFINED"; } - oss << "\nJOINT: " << info_.joints[i].name << "\n" - << "Parameters: CAN ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] - << " | Gear Ratio: " << joint_gear_ratios[i] << "\n" - << " | Orientation: " << joint_orientation[i] << "\n" - << "-- Commands --\n" + + oss << "\n----- JOINT: " << joint.name << " -----\n" + << "Parameters: CAN ID: 0x" + << std::hex << std::uppercase << joint.node_id << std::dec + << " | Gear Ratio: " << joint.gear_ratio + << " | Orientation: " << joint.orientation + << " | Operating Velocity: " << joint.operating_velocity << "\n" + + << "Curr P: " << joint.current_Kp + << " | Curr I: " << joint.current_Ki + << " | Curr D: " << joint.current_Kd << "\n" + + << "Speed P: " << joint.speed_Kp + << " | Speed I: " << joint.speed_Ki + << " | Speed D: " << joint.speed_Kd << "\n" + + << "Pos P: " << joint.position_Kp + << " | Pos I: " << joint.position_Ki + << " | Pos D: " << joint.position_Kd << "\n" + + << "Max Torque: " << joint.max_torque + << " | Max Speed: " << joint.max_speed + << " | Max Angle: " << joint.max_angle << "\n" + + << "Current Ramp: " << joint.current_ramp + << " | Speed Ramp: " << joint.speed_ramp << "\n" + + << "\n-- Commands --\n" << "Control Mode: " << control_mode << "\n" - << "Motor Position: " << motor_position[i] - << " | Joint Command Position: " << joint_command_position_[i] << "\n" - << "Motor Velocity: " << motor_velocity[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" - << "Motor Temperature: " << motor_temperature_[i] << " C" - << " | Torque Current: " << motor_torque_current_[i] << " A\n"; + << "Motor Position: " << joint.motor_position + << " | Joint Command Position: " << joint.joint_command_position << "\n" + + << "Motor Velocity: " << joint.motor_velocity + << " | Joint Command Velocity: " << joint.joint_command_velocity << "\n" + + << "Motor Status Request: " << joint.motor_status_req + << " | Motor Maintenance Request: " << joint.motor_maintenance_req << "\n" + + << "Maintenance Command High: " << joint.maintenance_frame_high + << " | Low: " << joint.maintenance_frame_low + << " | Full: " << joint.maintenance_frame << "\n" + + << "Maintenance Data Count: " << joint.maintenance_data_count << "\n" + + << "Previous Status Req: " << joint.prev_status_req + << " | Previous Maintenance Req: " << joint.prev_maintenance_req << "\n" + + << "Elapsed Status Req Time: " << joint.elapsed_status_request_time + << " | Elapsed Maintenance Req Time: " + << joint.elapsed_maintenance_request_time << "\n" + + << "Decoded Maintenance Frame: "; + + for (const auto & byte : joint.decoded_maintenance_frame) { + oss << std::hex << std::uppercase + << static_cast(byte) << " "; + } + + oss << std::dec + << "\n-- State --\n" + << "Joint Position: " << joint.joint_state_position << " rad" + << " | Joint Velocity: " << joint.joint_state_velocity << " rad/s" + << " | Acceleration: " << joint.acceleration << "\n" + + << "Encoder Position: " << joint.encoder_position << "\n" + + << "\n-- Telemetry --\n" + << "Motor Temperature: " << joint.motor_temperature << " C" + << " | Torque Current: " << joint.motor_torque_current << " A" + << " | Motor Status: " << joint.motor_status << "\n" + + << "\n-- Previous Commands --\n" + << "Previous Joint Command Position: " + << joint.prev_joint_command_position + << " | Previous Joint Command Velocity: " + << joint.prev_joint_command_velocity << "\n"; + + log_msg += oss.str(); + RCLCPP_INFO( + rclcpp::get_logger("SMCHardwareInterface"), + log_msg.c_str()); + + oss.str(""); + oss.clear(); } +} - log_msg += oss.str(); - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), log_msg.c_str()); +bool SMCHardwareInterface::interpret_settings_parameters( + SMCJoint & joint, const std::array & data) +{ + const auto param = static_cast(data[1]); + switch (param) { + case ParamID::ANGLE_PID: + joint.position_Kp = static_cast(static_cast((data[3] << 8) | data[2])); + joint.position_Ki = static_cast(static_cast((data[5] << 8) | data[4])); + joint.position_Kd = static_cast(static_cast((data[7] << 8) | data[6])); + return true; + case ParamID::SPEED_PID: + joint.speed_Kp = static_cast(static_cast((data[3] << 8) | data[2])); + joint.speed_Ki = static_cast(static_cast((data[5] << 8) | data[4])); + joint.speed_Kd = static_cast(static_cast((data[7] << 8) | data[6])); + return true; + case ParamID::CURRENT_PID: + joint.current_Kp = static_cast(static_cast((data[3] << 8) | data[2])); + joint.current_Ki = static_cast(static_cast((data[5] << 8) | data[4])); + joint.current_Kd = static_cast(static_cast((data[7] << 8) | data[6])); + return true; + case ParamID::MAX_TORQUE: + joint.max_torque = static_cast(static_cast((data[3] << 8) | data[2])); + return true; + case ParamID::MAX_SPEED: + joint.max_speed = static_cast(static_cast((data[5] << 24) | (data[4] << 16) | (data[3] << 8) | data[2])); + return true; + case ParamID::MAX_ANGLE_A: + case ParamID::MAX_ANGLE_B: + joint.max_angle = static_cast(static_cast((data[5] << 24) | (data[4] << 16) | (data[3] << 8) | data[2])) * 0.01 * (M_PI / 180.0); + return true; + case ParamID::CURRENT_RAMP: + joint.current_ramp = static_cast(static_cast((data[3] << 8) | data[2])); + return true; + case ParamID::SPEED_RAMP: + joint.speed_ramp = static_cast(static_cast((data[5] << 24) | (data[4] << 16) | (data[3] << 8) | data[2])); + return true; + default: + return false; + } } +void SMCHardwareInterface::format_control_command(CANLib::CanFrame & frame, SMCJoint & joint) +{ + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); + + if ( + joint.control_level == integration_level_t::POSITION && + std::isfinite(joint.joint_command_position) && + joint.joint_command_position != joint.prev_joint_command_position) + { + const int32_t joint_angle = joint.orientation * + calculate_motor_position_from_desired_joint_position( + joint.joint_command_position, joint.gear_ratio); + + frame.data[0] = static_cast(ControlCommands::ABSOLUTE_POS_WITH_VEL_CONTROL_CMD); + frame.data[2] = static_cast(joint.operating_velocity & 0xFF); + frame.data[3] = static_cast((joint.operating_velocity >> 8) & 0xFF); + frame.data[4] = static_cast(joint_angle & 0xFF); + frame.data[5] = static_cast((joint_angle >> 8) & 0xFF); + frame.data[6] = static_cast((joint_angle >> 16) & 0xFF); + frame.data[7] = static_cast((joint_angle >> 24) & 0xFF); + joint.prev_joint_command_position = joint.joint_command_position; + return; + } + else if ( + joint.control_level == integration_level_t::VELOCITY && + std::isfinite(joint.joint_command_velocity) && + joint.joint_command_velocity != joint.prev_joint_command_velocity) + { + const int32_t joint_velocity = joint.orientation * + calculate_motor_velocity_from_desired_joint_velocity( + joint.joint_command_velocity, joint.gear_ratio); + + frame.data[0] = static_cast(ControlCommands::SPEED_CONTROL_CMD); + frame.data[4] = static_cast(joint_velocity & 0xFF); + frame.data[5] = static_cast((joint_velocity >> 8) & 0xFF); + frame.data[6] = static_cast((joint_velocity >> 16) & 0xFF); + frame.data[7] = static_cast((joint_velocity >> 24) & 0xFF); + joint.prev_joint_command_velocity = joint.joint_command_velocity; + return; + } + else{ + state_iterator = state_iterator + 1; + (state_iterator%2 == 0) ? frame.data[0] = static_cast(StatusCommands::MOTOR_STATUS_2_CMD) : + frame.data[0] = static_cast(StatusCommands::READ_ABS_ANGLE_CMD); + return; + } +} + +bool SMCHardwareInterface::format_maintenance_command( + CANLib::CanFrame & frame, const DecodedCommand & decoded_cmd) +{ + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); + frame.data[0] = decoded_cmd.command_id; // Set multiplexor + switch (static_cast(decoded_cmd.command_id)) { + case MaintenanceCommands::WRITE_ACCELERATION_CMD: + if(decoded_cmd.u8_data.size() != 0 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 1){ + return false; // Invalid data format for this command + } + else{ + frame.data[4] = decoded_cmd.i32_data[0] & 0xFF; // Acceleration low byte + frame.data[5] = (decoded_cmd.i32_data[0] >> 8) & 0xFF; // Acceleration byte 2 + frame.data[6] = (decoded_cmd.i32_data[0] >> 16) & 0xFF; // Acceleration byte 3 + frame.data[7] = (decoded_cmd.i32_data[0] >> 24) & 0xFF; // Acceleration high byte + return true; + } + case MaintenanceCommands::READ_SETTINGS_CMD: + if(decoded_cmd.u8_data.size() != 1 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + frame.data[1] = decoded_cmd.u8_data[0]; // Parameter ID + return true; + } + case MaintenanceCommands::WRITE_SETTINGS_TO_RAM_CMD: + if(decoded_cmd.u8_data.size() != 1 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + frame.data[1] = decoded_cmd.u8_data[0]; // Parameter ID + return true; + } + case MaintenanceCommands::WRITE_SETTINGS_TO_ROM_CMD: + if(decoded_cmd.u8_data.size() != 1 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + frame.data[1] = decoded_cmd.u8_data[0]; // Parameter ID + return true; + } + case MaintenanceCommands::MOTOR_SHUTDOWN_CMD: + if(decoded_cmd.u8_data.size() != 0 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + // Empty + return true; + } + case MaintenanceCommands::MOTOR_STOP_CMD: + if(decoded_cmd.u8_data.size() != 0 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + // Empty + return true; + } + case MaintenanceCommands::MOTOR_RUNNING_CMD: + if(decoded_cmd.u8_data.size() != 0 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + // Empty + return true; + } + case MaintenanceCommands::CLEAR_MOTOR_ANGLE_CMD: + if(decoded_cmd.u8_data.size() != 0 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + // Empty + return true; + } + case MaintenanceCommands::CLEAR_ERROR_CMD: + if(decoded_cmd.u8_data.size() != 0 || decoded_cmd.i16_data.size() != 0 || decoded_cmd.i32_data.size() != 0){ + return false; // Invalid data format for this command + } + else{ + // Empty + return true; + } + default: + return false; + } + + size_t index = 1; + for (uint8_t value : decoded_cmd.u8_data) { + if (index >= frame.data.size()) { + return false; + } + frame.data[index++] = value; + } + for (int16_t value : decoded_cmd.i16_data) { + if (index + 1 >= frame.data.size()) { + return false; + } + frame.data[index++] = static_cast(value & 0xFF); + frame.data[index++] = static_cast((value >> 8) & 0xFF); + } + for (int32_t value : decoded_cmd.i32_data) { + if (index + 3 >= frame.data.size()) { + return false; + } + frame.data[index++] = static_cast(value & 0xFF); + frame.data[index++] = static_cast((value >> 8) & 0xFF); + frame.data[index++] = static_cast((value >> 16) & 0xFF); + frame.data[index++] = static_cast((value >> 24) & 0xFF); + } + return true; +} hardware_interface::CallbackReturn SMCHardwareInterface::on_init( - const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file + const hardware_interface::HardwareInfo & info) { if (hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) { + hardware_interface::CallbackReturn::SUCCESS) + { return hardware_interface::CallbackReturn::ERROR; } - - // Setting Parameters - for (auto& joint : info_.joints) { - int gear_ratio = std::abs(std::stoi(joint.parameters.at("gear_ratio"))); - joint_node_ids.push_back(std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x141, 0x160)); - joint_gear_ratios.push_back(gear_ratio); - joint_orientation.push_back(std::stoi(joint.parameters.at("joint_orientation")) == -1 ? -1 : 1); - operating_velocity = std::clamp(std::stoi(joint.parameters.at("operating_velocity")), 0, 65*gear_ratio); - } - num_joints = static_cast(info_.joints.size()); + SMCJoints_.clear(); 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"); + // Per Joint Parameters + for (const auto & joint : info_.joints) { + // Collect joint specific parameters + const int node_id = std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x141, 0x160); + const int gear_ratio = std::abs(std::stoi(joint.parameters.at("gear_ratio"))); + const int orientation = joint.parameters.count("joint_orientation") && + std::stoi(joint.parameters.at("joint_orientation")) == -1 ? -1 : 1; + const int operating_velocity = std::clamp( + std::stoi(joint.parameters.at("operating_velocity")), 0, 65 * gear_ratio); + + // Collect state interface names + std::vector state_if_names; + for (const auto & si : joint.state_interfaces) { + state_if_names.push_back(si.name); + } + + // Collect command interface names + std::vector command_if_names; + for (const auto & ci : joint.command_interfaces) { + command_if_names.push_back(ci.name); + } + + // Copy parameters into an unordered_map + std::unordered_map params_map; + for (const auto & p : joint.parameters) { + params_map.emplace(p.first, p.second); + } + + // Populate each SMCJoint object + SMCJoint smc_joint{}; + SMCJoints_.push_back( + SMCJoint{ + .name = joint.name, + .node_id = static_cast(node_id), + .gear_ratio = gear_ratio, + .orientation = orientation, + .operating_velocity = static_cast(operating_velocity), + .control_level = integration_level_t::POSITION, + .joint_state_position = std::numeric_limits::quiet_NaN(), + .joint_state_velocity = 0.0, + .motor_temperature = std::numeric_limits::quiet_NaN(), + .motor_torque_current = std::numeric_limits::quiet_NaN(), + .motor_status = 0.0, + .current_Kp = std::numeric_limits::quiet_NaN(), + .current_Ki = std::numeric_limits::quiet_NaN(), + .current_Kd = std::numeric_limits::quiet_NaN(), + .speed_Kp = std::numeric_limits::quiet_NaN(), + .speed_Ki = std::numeric_limits::quiet_NaN(), + .speed_Kd = std::numeric_limits::quiet_NaN(), + .position_Kp = std::numeric_limits::quiet_NaN(), + .position_Ki = std::numeric_limits::quiet_NaN(), + .position_Kd = std::numeric_limits::quiet_NaN(), + .acceleration = std::numeric_limits::quiet_NaN(), + .max_torque = std::numeric_limits::quiet_NaN(), + .max_speed = std::numeric_limits::quiet_NaN(), + .max_angle = std::numeric_limits::quiet_NaN(), + .current_ramp = std::numeric_limits::quiet_NaN(), + .speed_ramp = std::numeric_limits::quiet_NaN(), + + .joint_command_position = std::numeric_limits::quiet_NaN(), + .joint_command_velocity = 0.0, + .motor_status_req = 0.0, + .motor_maintenance_req = 0.0, + .maintenance_frame_high = 0.0, + .maintenance_frame_low = 0.0, + .maintenance_frame = 0.0, + .maintenance_data_count = 0.0, + .decoded_maintenance_frame = {}, + + .prev_status_req = 0.0, + .prev_maintenance_req = 0.0, + .elapsed_status_request_time = 0.0, + .elapsed_maintenance_request_time = 0.0, + .motor_velocity = 0.0, + .motor_position = 0.0, + .encoder_position = 0.0, + .prev_joint_command_position = std::numeric_limits::quiet_NaN(), + .prev_joint_command_velocity = std::numeric_limits::quiet_NaN(), + .state_interface_names = state_if_names, + .command_interface_names = command_if_names, + .parameters = params_map + } + ); + } + + num_joints = static_cast(SMCJoints_.size()); + state_iterator = 0; elapsed_update_time = 0.0; elapsed_time = 0.0; elapsed_logger_time = 0.0; - - // Initializes command and state interface values - joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); - joint_state_velocity_.assign(num_joints, 0); - - joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); - joint_command_velocity_.assign(num_joints, 0); - - encoder_position.assign(num_joints, 0.0); - motor_position.assign(num_joints, 0.0); - motor_velocity.assign(num_joints, 0.0); - - motor_temperature_.assign(num_joints, 0.0); - motor_torque_current_.assign(num_joints, 0.0); - - control_level_.resize(num_joints, integration_level_t::POSITION); return hardware_interface::CallbackReturn::SUCCESS; } - hardware_interface::CallbackReturn SMCHardwareInterface::on_shutdown( const rclcpp_lifecycle::State & previous_state) { - // Reuse cleanup logic which shuts down the motor and then deinitializes shared pointers. - // Need this in case on_cleanup never gets called - return this->on_cleanup(previous_state); + return on_cleanup(previous_state); } - std::vector SMCHardwareInterface::export_state_interfaces() { std::vector state_interfaces; - - // Each SMC motor corresponds to a different joint. - 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 (published via /dynamic_joint_states by joint_state_broadcaster) - 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])); + for (auto & joint : SMCJoints_) { + for (const auto & iface : joint.state_interface_names) { + double * value = nullptr; + if (iface == hardware_interface::HW_IF_POSITION) { + value = &joint.joint_state_position; + } else if (iface == hardware_interface::HW_IF_VELOCITY) { + value = &joint.joint_state_velocity; + } else if (iface == "motor_temperature") { + value = &joint.motor_temperature; + } else if (iface == "torque_current") { + value = &joint.motor_torque_current; + } else if (iface == "status") { + value = &joint.motor_status; + } else { + RCLCPP_WARN( + rclcpp::get_logger("SMCHardwareInterface"), + "Unknown state interface '%s' for joint '%s'", + iface.c_str(), joint.name.c_str()); + continue; + } + state_interfaces.emplace_back(joint.name, iface, value); + } } - return state_interfaces; } - -std::vector -SMCHardwareInterface::export_command_interfaces() +std::vector SMCHardwareInterface::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])); + for (auto & joint : SMCJoints_) { + for (const auto & iface : joint.command_interface_names) { + double * value = nullptr; + if (iface == hardware_interface::HW_IF_POSITION) { + value = &joint.joint_command_position; + } else if (iface == hardware_interface::HW_IF_VELOCITY) { + value = &joint.joint_command_velocity; + } else if (iface == "status_request") { + value = &joint.motor_status_req; + } else if (iface == "maintenance_request") { + value = &joint.motor_maintenance_req; + } else if (iface == "maintenance_frame_high") { + value = &joint.maintenance_frame_high; + } else if (iface == "maintenance_frame_low") { + value = &joint.maintenance_frame_low; + } else if (iface == "maintenance_data_count") { + value = &joint.maintenance_data_count; + } else { + RCLCPP_WARN( + rclcpp::get_logger("SMCHardwareInterface"), + "Unknown command interface '%s' for joint '%s'", + iface.c_str(), joint.name.c_str()); + continue; + } + command_interfaces.emplace_back(joint.name, iface, value); + } } - return command_interfaces; } hardware_interface::CallbackReturn SMCHardwareInterface::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - if (!canBus.open(can_interface, std::bind(&SMCHardwareInterface::on_can_message, this, std::placeholders::_1))) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Failed to open CAN interface"); + if (!canBus.open( + can_interface, + std::bind(&SMCHardwareInterface::on_can_message, this, std::placeholders::_1))) + { + RCLCPP_ERROR(rclcpp::get_logger("SMCHardwareInterface"), "Failed to open CAN interface"); return hardware_interface::CallbackReturn::ERROR; } - return hardware_interface::CallbackReturn::SUCCESS; } -void SMCHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { - can_rx_frame_ = frame; // Store the received frame for processing in read() - - std::string result; - - int data[8] = {0x00}; - - for(int i = 0; i < num_joints; i++){ - if(can_rx_frame_.id == static_cast(joint_node_ids[i]) && - (can_rx_frame_.data[0] == SPEED_CONTROL_CMD || - can_rx_frame_.data[0] == ABSOLUTE_POS_CONTROL_CMD || - can_rx_frame_.data[0] == MOTOR_STATUS_2_CMD) && - can_rx_frame_.data[1] != 0x00){ - - // DECODING CAN MESSAGE FOR VELOCITY - data[1] = can_rx_frame_.data[1]; // Motor Temperature - data[2] = can_rx_frame_.data[2]; // Torque low byte - data[3] = can_rx_frame_.data[3]; // Torque high byte - data[4] = can_rx_frame_.data[4]; // speed low byte - data[5] = can_rx_frame_.data[5]; // speed high byte - data[6] = can_rx_frame_.data[6]; // encoder position low byte - data[7] = can_rx_frame_.data[7]; // encoder position high byte - - // ENCODER POSITION - // uint16 -> int16 -> double (for calcs) - encoder_position[i] = static_cast(static_cast((data[7] << 8) | data[6])); +void SMCHardwareInterface::on_can_message(const CANLib::CanFrame & frame) +{ + can_rx_frame_ = frame; + for (auto & joint : SMCJoints_) { + if (frame.id != joint.node_id) { + continue; + } - // SPEED - // uint16 -> int16 -> double (for calcs) - motor_velocity[i] = static_cast(static_cast((data[5] << 8) | data[4])); + if ( + frame.data[0] == static_cast(ControlCommands::SPEED_CONTROL_CMD) || + frame.data[0] == static_cast(ControlCommands::ABSOLUTE_POS_WITH_VEL_CONTROL_CMD) || + frame.data[0] == static_cast(StatusCommands::MOTOR_STATUS_2_CMD)) + { + joint.encoder_position = static_cast( + static_cast((frame.data[7] << 8) | frame.data[6])); + joint.motor_velocity = static_cast( + static_cast((frame.data[5] << 8) | frame.data[4])); + joint.motor_temperature = static_cast(frame.data[1]); + joint.motor_torque_current = static_cast( + static_cast((frame.data[3] << 8) | frame.data[2])) * 0.01; + continue; + } - // TEMPERATURE (1 byte, degrees C) - motor_temperature_[i] = static_cast(data[1]); + if (frame.data[0] == static_cast(StatusCommands::READ_ABS_ANGLE_CMD)) { + const uint64_t raw = + (static_cast(frame.data[7]) << 48) | + (static_cast(frame.data[6]) << 40) | + (static_cast(frame.data[5]) << 32) | + (static_cast(frame.data[4]) << 24) | + (static_cast(frame.data[3]) << 16) | + (static_cast(frame.data[2]) << 8) | + static_cast(frame.data[1]); + joint.motor_position = static_cast((static_cast(raw) << 8) >> 8); + continue; + } - // TORQUE CURRENT (16-bit signed, 0.01A per LSB) - motor_torque_current_[i] = static_cast(static_cast((data[3] << 8) | data[2])) * 0.01; + if (frame.data[0] == static_cast(StatusCommands::MOTOR_STATUS_1_CMD)) { + joint.motor_temperature = static_cast(frame.data[1]); + joint.motor_status = static_cast(frame.data[7]); + continue; } - else if(can_rx_frame_.id == static_cast(joint_node_ids[i]) && can_rx_frame_.data[0] == 0x92){ - data[0] = 0x92; - data[1] = can_rx_frame_.data[1]; // Multi-turn low byte - data[2] = can_rx_frame_.data[2]; - data[3] = can_rx_frame_.data[3]; - data[4] = can_rx_frame_.data[4]; - data[5] = can_rx_frame_.data[5]; - data[6] = can_rx_frame_.data[6]; - data[7] = can_rx_frame_.data[7]; // Multi-turn high byte - - // POSITION - // In this case, we have 56 bits to store in 64, so we must have a sign extension - // uint64 -> int64 (to do sign extension) -> sign extension (left shift 8, then arithmetic right shift 8) --> double (for calcs) - uint64_t motor_position_raw = (static_cast(data[7]) << 48) | - (static_cast(data[6]) << 40) | - (static_cast(data[5]) << 32) | - (static_cast(data[4]) << 24) | - (static_cast(data[3]) << 16) | - (static_cast(data[2]) << 8) | - static_cast(data[1]); - - motor_position[i] = static_cast((static_cast(motor_position_raw) << 8) >> 8); + + if (frame.data[0] == static_cast(StatusCommands::READ_ACCELERATION_CMD)) { + joint.acceleration = static_cast( + (frame.data[7] << 24) | (frame.data[6] << 16) | (frame.data[5] << 8) | frame.data[4]); } - else{ - if(logger_state == 1) { - // RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Reply not heard."); + + if(frame.data[0] == static_cast(MaintenanceCommands::READ_SETTINGS_CMD)) { + if(!interpret_settings_parameters(joint, frame.data)) { + RCLCPP_ERROR(rclcpp::get_logger("SMCHardwareInterface"), "Failed to interpret settings parameters for joint '%s'.", joint.name.c_str()); } - } + } } - } hardware_interface::CallbackReturn SMCHardwareInterface::on_cleanup( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - // If cleanup occurs before shutdown, this is the last opportunity to shutdown motor since pointers must be deleted here - for(int i = 0; i < num_joints; i++){ - // Motor Off (Shutdown) Command - send_command(joint_node_ids[i], MOTOR_SHUTDOWN_CMD); + for (const auto & joint : SMCJoints_) { + send_command(joint.node_id, static_cast(MaintenanceCommands::MOTOR_SHUTDOWN_CMD)); } - canBus.close(); - return hardware_interface::CallbackReturn::SUCCESS; } - hardware_interface::CallbackReturn SMCHardwareInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Activating ...please wait..."); - - for(int i = 0; i < num_joints; i++){ - send_command(joint_node_ids[i], MOTOR_RUNNING_CMD); + for (const auto & joint : SMCJoints_) { + send_command(joint.node_id, static_cast(MaintenanceCommands::MOTOR_RUNNING_CMD)); + } + for (auto & joint : SMCJoints_) { + joint.joint_command_position = joint.joint_state_position; } - - // Sets initial command to joint state - joint_command_position_ = joint_state_position_; - - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Successfully activated!"); - return hardware_interface::CallbackReturn::SUCCESS; } - hardware_interface::CallbackReturn SMCHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Deactivating ...please wait..."); - - for(int i = 0; i < num_joints; i++){ - send_command(joint_node_ids[i], MOTOR_STOP_CMD); + for (const auto & joint : SMCJoints_) { + send_command(joint.node_id, static_cast(MaintenanceCommands::MOTOR_STOP_CMD)); } - - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Successfully deactivated all SMC motors!"); - return hardware_interface::CallbackReturn::SUCCESS; } - hardware_interface::return_type SMCHardwareInterface::read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time &, const rclcpp::Duration &) { - //CALCULATING JOINT STATE - for(int i = 0; i < num_joints; i++){ - joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); - joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); + for (auto & joint : SMCJoints_) { + joint.joint_state_velocity = calculate_joint_velocity_from_motor_velocity( + joint.motor_velocity, joint.gear_ratio); + joint.joint_state_position = calculate_joint_position_from_motor_position( + joint.motor_position, joint.gear_ratio); } - return hardware_interface::return_type::OK; } - -hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +hardware_interface::return_type SMCHardwareInterface::write( + const rclcpp::Time &, const rclcpp::Duration & period) { - elapsed_update_time+=period.seconds(); - double update_period = 1.0/update_rate; - int data[8] = {0x00}; - int32_t joint_angle = 0; - int16_t operating_velocity = 200; - int32_t joint_velocity = 0; - - elapsed_time+=period.seconds(); - // Logger update - elapsed_logger_time+=period.seconds(); - double logging_period = 1.0/logger_rate; - if(elapsed_logger_time > logging_period){ + 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(); } } + // Status request handling + for (auto & joint : SMCJoints_) { + const double curr_status_req = joint.motor_status_req; + if (curr_status_req < 0.0 && joint.prev_status_req >= 0.0) + { + for (auto status_cmd : kStatusCommands) { + send_command(joint.node_id, static_cast(status_cmd)); + } + RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "One-shot status request sent for joint '%s'.", joint.name.c_str()); + } + else if (curr_status_req > 0.0) + { + joint.elapsed_status_request_time += period.seconds(); + double status_request_period = 1.0 / curr_status_req; + if (joint.elapsed_status_request_time > status_request_period) { + joint.elapsed_status_request_time = 0.0; + for (auto status_cmd : kStatusCommands) { + send_command(joint.node_id, static_cast(status_cmd)); + } + } + } + joint.prev_status_req = curr_status_req; + } + + // Maintenance request handling + for (auto & joint : SMCJoints_) { + auto doubles_to_payload = [](double high, double low) -> int64_t { + const uint64_t h = static_cast(high); + const uint64_t l = static_cast(low); + return static_cast((h << 32) | l); + }; + + 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)); + pack_decoded_maintenance_frame(joint, decoded_maintenance_cmd); + + CANLib::CanFrame frame; + frame.id = joint.node_id; + frame.dlc = 8; + if (!format_maintenance_command(frame, decoded_maintenance_cmd)) { + // RCLCPP_WARN(rclcpp::get_logger("SMCHardwareInterface"), "Invalid maintenance command for joint '%s'.", joint.name.c_str()); + continue; + } + + + const double curr_maintenance_req = joint.motor_maintenance_req; + if (curr_maintenance_req < 0.0 && joint.prev_maintenance_req >= 0.0) { // One-shot maintenance command + canBus.send(frame); + RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "One-shot maintenance request sent for joint '%s'.", joint.name.c_str()); + } else if (curr_maintenance_req > 0.0) { + joint.elapsed_maintenance_request_time += period.seconds(); + double maintenance_request_period = 1.0 / curr_maintenance_req; + if (joint.elapsed_maintenance_request_time > maintenance_request_period) { + joint.elapsed_maintenance_request_time = 0.0; + canBus.send(frame); + RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Periodic maintenance request sent for joint '%s'.", joint.name.c_str()); + } + } + joint.prev_maintenance_req = curr_maintenance_req; + } // HWI can only go as fast as the controller manager. To limit frequency of bus messages, // keep track of time passed over iterations of this function and if it exceeds the // desired frequency of the HWI, skip message - if(elapsed_update_time > update_period){ + elapsed_update_time += period.seconds(); + if (update_rate > 0 && elapsed_update_time > (1.0 / static_cast(update_rate))) { elapsed_update_time = 0.0; - for(int i = 0; i < num_joints; i++) { - can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated - can_tx_frame_.id = joint_node_ids[i]; - can_tx_frame_.dlc = 8; - - if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { - - // CALCULATE DESIRED JOINT ANGLE - joint_angle = joint_orientation[i]*calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); - - // ENCODING CAN MESSAGE - data[0] = ABSOLUTE_POS_CONTROL_CMD; - data[1] = 0x00; - data[2] = operating_velocity & 0xFF; - data[3] = (operating_velocity >> 8) & 0xFF; - data[4] = joint_angle & 0xFF; - data[5] = (joint_angle >> 8) & 0xFF; - data[6] = (joint_angle >> 16) & 0xFF; - data[7] = (joint_angle >> 24) & 0xFF; - } - else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { - - // CALCULATE DESIRED JOINT VELOCITY - joint_velocity = joint_orientation[i]*calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); - - // ENCODING CAN MESSAGE - data[0] = SPEED_CONTROL_CMD; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = joint_velocity & 0xFF; - data[5] = (joint_velocity >> 8) & 0xFF; - data[6] = (joint_velocity >> 16) & 0xFF; - data[7] = (joint_velocity >> 24) & 0xFF; - } - else{ - // RCLCPP_WARN(rclcpp::get_logger("SMCHardwareInterface"), "Joint command value not found or undefined command state. Sending Motor Status 3 commands for now."); - // ENCODING CAN MESSAGE - data[0] = MOTOR_STATUS_2_CMD; - } - - // Cast data to uint8_t - for(int j = 0; j < 8; j++){ - data[j] = static_cast(data[j]); - can_tx_frame_.data[j] = data[j]; - } - canBus.send(can_tx_frame_); + for (auto & joint : SMCJoints_) { + CANLib::CanFrame frame; + frame.id = joint.node_id; + frame.dlc = 8; + format_control_command(frame, joint); + canBus.send(frame); } } - + return hardware_interface::return_type::OK; } hardware_interface::return_type SMCHardwareInterface::perform_command_mode_switch( - const std::vector& start_interfaces, - const std::vector& stop_interfaces) + const std::vector & start_interfaces, + const std::vector & stop_interfaces) { - // Debug: print incoming requests - std::ostringstream ss; - ss << "perform_command_mode_switch called. start_interfaces: ["; - for (auto &s : start_interfaces) ss << s << ","; - ss << "] stop_interfaces: ["; - for (auto &s : stop_interfaces) ss << s << ","; - ss << "]"; - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), ss.str().c_str()); - - // For each joint, decide its new control mode based on start/stop interfaces. - // We allow partial starts/stops: only affected joints are switched. - std::vector requested_modes(num_joints, integration_level_t::UNDEFINED); - - // Process stop interfaces first: mark those joints as UNDEFINED - for (const auto &ifname : stop_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 || ifname == vel_if || ifname.find(info_.joints[i].name) != std::string::npos) { + std::vector requested_modes( + static_cast(num_joints), integration_level_t::UNDEFINED); + + for (const auto & ifname : stop_interfaces) { + for (size_t i = 0; i < SMCJoints_.size(); ++i) { + const auto & joint = SMCJoints_[i]; + const std::string pos_if = joint.name + "/" + hardware_interface::HW_IF_POSITION; + const std::string vel_if = joint.name + "/" + hardware_interface::HW_IF_VELOCITY; + if (ifname == pos_if || ifname == vel_if || ifname.find(joint.name) != std::string::npos) { requested_modes[i] = integration_level_t::UNDEFINED; } } } - // Process start interfaces: set POSITION or VELOCITY per joint - 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); + for (const auto & ifname : start_interfaces) { + for (size_t i = 0; i < SMCJoints_.size(); ++i) { + const auto & joint = SMCJoints_[i]; + const std::string pos_if = joint.name + "/" + hardware_interface::HW_IF_POSITION; + const std::string vel_if = joint.name + "/" + hardware_interface::HW_IF_VELOCITY; if (ifname == pos_if) { requested_modes[i] = integration_level_t::POSITION; } else if (ifname == vel_if) { @@ -462,53 +777,36 @@ hardware_interface::return_type SMCHardwareInterface::perform_command_mode_switc } } - // Now apply the requested_modes to control_level_. - // For any joint with UNDEFINED in requested_modes, we only change it if it was explicitly stopped. - for (int i = 0; i < num_joints; ++i) { + for (size_t i = 0; i < SMCJoints_.size(); ++i) { + auto & joint = SMCJoints_[i]; if (requested_modes[i] == integration_level_t::UNDEFINED) { - // if stop requested, set to UNDEFINED; otherwise leave existing mode - // (we only set to UNDEFINED if this joint was mentioned in stop_interfaces) bool was_stopped = false; - for (const auto &ifname : stop_interfaces) { - if (ifname.find(info_.joints[i].name) != std::string::npos) { + for (const auto & ifname : stop_interfaces) { + if (ifname.find(joint.name) != std::string::npos) { was_stopped = true; break; } } if (was_stopped) { - control_level_[i] = integration_level_t::UNDEFINED; - joint_command_velocity_[i] = 0; - // optional: reset position cmd to current state to be safe - joint_command_position_[i] = joint_state_position_[i]; - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), - "Joint %s: stopped -> set UNDEFINED", info_.joints[i].name.c_str()); - } - // else, leave control_level_ as-is - } else { - // Set the mode requested - control_level_[i] = requested_modes[i]; - // If switching to velocity, optionally set command velocity to current state to avoid jumps - if (requested_modes[i] == integration_level_t::VELOCITY) { - // joint_command_velocity_[i] = joint_state_velocity_[i]; - joint_command_velocity_[i] = 0; - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), - "Joint %s: switched to VELOCITY (cmd vel initialized from state: %f)", - info_.joints[i].name.c_str(), joint_command_velocity_[i]); - } else if (requested_modes[i] == integration_level_t::POSITION) { - joint_command_position_[i] = joint_state_position_[i]; - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), - "Joint %s: switched to POSITION (cmd pos initialized from state: %f)", - info_.joints[i].name.c_str(), joint_command_position_[i]); + joint.control_level = integration_level_t::UNDEFINED; + joint.joint_command_velocity = 0.0; + joint.joint_command_position = joint.joint_state_position; } + continue; + } + + joint.control_level = requested_modes[i]; + if (joint.control_level == integration_level_t::VELOCITY) { + joint.joint_command_velocity = 0.0; + } else if (joint.control_level == integration_level_t::POSITION) { + joint.joint_command_position = joint.joint_state_position; } } return hardware_interface::return_type::OK; } -} // namespace smc_hardware_interface - -#include "pluginlib/class_list_macros.hpp" +} // namespace smc_ros2_control PLUGINLIB_EXPORT_CLASS( - smc_ros2_control::SMCHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file + smc_ros2_control::SMCHardwareInterface, hardware_interface::SystemInterface) diff --git a/src/hardware_interfaces/stepper_ros2_control/CMakeLists.txt b/src/hardware_interfaces/stepper_ros2_control/CMakeLists.txt index 0bb0b393..75482a92 100644 --- a/src/hardware_interfaces/stepper_ros2_control/CMakeLists.txt +++ b/src/hardware_interfaces/stepper_ros2_control/CMakeLists.txt @@ -11,7 +11,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle realtime_tools - msgs umdloop_can_library ) @@ -26,7 +25,6 @@ endforeach() # umdloop_can_library is found through the dependency loop above ament_auto_find_build_dependencies() -ament_export_dependencies(rosidl_default_runtime) include_directories(include) @@ -52,7 +50,6 @@ ament_target_dependencies( rclcpp rclcpp_lifecycle realtime_tools - msgs ) # Export hardware plugins diff --git a/src/hardware_interfaces/stepper_ros2_control/include/stepper_ros2_control/stepper_hardware_interface.hpp b/src/hardware_interfaces/stepper_ros2_control/include/stepper_ros2_control/stepper_hardware_interface.hpp index d23870ea..43bc9ddd 100644 --- a/src/hardware_interfaces/stepper_ros2_control/include/stepper_ros2_control/stepper_hardware_interface.hpp +++ b/src/hardware_interfaces/stepper_ros2_control/include/stepper_ros2_control/stepper_hardware_interface.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. @@ -11,21 +11,22 @@ // 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. - -// -// Authors: Denis Stogl // #ifndef STEPPER_HARDWARE_INTERFACE_HPP_ #define STEPPER_HARDWARE_INTERFACE_HPP_ #include + +#include +#include +#include #include #include +#include #include #include - #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" @@ -34,114 +35,232 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "umdloop_can_library/CanFrame.hpp" +#include "umdloop_can_library/SocketCanBus.hpp" -#include -#include -#include -#include "msgs/msg/cana.hpp" +namespace CANLib +{ +struct CanFrame; +} namespace stepper_ros2_control { -class STEPPERHardwareInterface : public hardware_interface::SystemInterface // Inheriting from System Interface +class STEPPERHardwareInterface : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(STEPPERHardwareInterface) - // Initialization, so reading parameters, initializing variables, checking if all the joint state and command interfaces are correct hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - // Exports/exposes Interfaces that are available so that the controllers - // know what to read and write to std::vector export_state_interfaces() override; - std::vector export_command_interfaces() override; - // Lifecycle 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; + 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 - double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); - double calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); - - int32_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); - int32_t calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio); - -private: + enum class integration_level_t : std::uint8_t + { + UNDEFINED = 0, + POSITION = 1, + VELOCITY = 2, + }; - int num_joints; + struct StepperJoint + { + std::string name; + uint8_t node_id; + int gear_ratio; + int orientation; + integration_level_t control_level; + + double joint_state_position; + double joint_state_velocity; + double motor_position; + double motor_velocity; + double motor_status; + double motor_temperature; + double motor_torque_current; + + double joint_command_position; + double joint_command_velocity; + double motor_status_req; + double motor_maintenance_req; + double maintenance_frame_high; + double maintenance_frame_low; + double maintenance_frame; + double maintenance_data_count; + std::vector decoded_maintenance_frame; + + double prev_status_req; + double prev_maintenance_req; + double elapsed_status_request_time; + double elapsed_maintenance_request_time; + double prev_joint_command_position; + double prev_joint_command_velocity; + + std::vector state_interface_names; + std::vector command_interface_names; + std::unordered_map parameters; + }; + struct DecodedCommand + { + uint8_t command_id; + std::vector u8_data; + std::vector i16_data; + std::vector i32_data; + }; - // EXPERIMENTING - std::vector initial_position_; + void on_can_message(const CANLib::CanFrame & frame); + void logger_function(); - // Store the state for the simulated robot - std::vector joint_state_position_; - std::vector joint_state_velocity_; - - // Store the command for the simulated robot - std::vector joint_command_position_; - std::vector joint_command_velocity_; + double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); + double calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); + int16_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); + int16_t calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio); - // Telemetry data from CAN responses - std::vector motor_temperature_; // Motor temperature in °C - std::vector motor_torque_current_; // Motor torque current in A + void format_control_command(CANLib::CanFrame & frame, StepperJoint & joint); + bool format_status_command(CANLib::CanFrame & frame, uint8_t command_id, uint8_t node_id); + bool format_maintenance_command( + CANLib::CanFrame & frame, uint8_t node_id, const DecodedCommand & decoded_cmd); - double encoder_position; - double motor_velocity; - double motor_position; +private: + 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; - std::vector joint_initialization_; + int can_command_id; + uint32_t can_response_id; + CANLib::SocketCanBus canBus; + CANLib::CanFrame can_tx_frame_; + CANLib::CanFrame can_rx_frame_; + std::string can_interface; - rclcpp::Publisher::SharedPtr science_can_publisher_; - rclcpp::Subscription::SharedPtr science_can_subscriber_; - rclcpp::Node::SharedPtr node_; - uint16_t current_iteration; + std::vector STEPPERJoints_; + enum class ControlCommands : uint8_t + { + RELATIVE_POS_CONTROL_CMD = 0x20, + VELOCITY_CONTROL_CMD = 0x30, + }; - msgs::msg::CANA received_joint_data_; + enum class MaintenanceCommands : uint8_t + { + PCB_HEARTBEAT_CMD = 0x10, + MAINTENANCE_CMD = 0x60, + STEPPER_SPECS_CMD = 0x70, + }; - std::vector joint_gear_ratios; - std::vector joint_orientation; + enum class MaintenanceCommandOptions : uint8_t + { + SET_CURRENT_MULTI_TURN_POS_ZERO_TO_ROM_CMD = 0x00, + MOTOR_STOP_CMD = 0x01, + MOTOR_SHUTDOWN_CMD = 0x02, + CLEAR_ERRORS_CMD = 0x03, + }; + enum class StatusCommands : uint8_t + { + MOTOR_STATE = 0x40, + MOTOR_STATUS = 0x50, + }; + enum class MotorStatus : uint8_t + { + UNDEFINED = 0, + IDLE = 1, + STARTUP_SEQUENCE = 2, + + ERROR_INVALID_REQUEST = 3, + ERROR_STEPPER_DISARMED = 4, + ERROR_STEPPER_FAILED = 5, + ERROR_CONTROLLER_FAILED = 6, + ERROR_ESTOP_REQUESTED = 7, + ERROR_UNKNOWN_POSITION = 8, + + POSITION_CONTROL = 9, + VELOCITY_CONTROL = 10, + STEPPER_STOPPED = 11 + }; - enum integration_level_t : std::uint8_t + enum class ValidateRequest : uint8_t { - UNDEFINED = 0, - POSITION = 1, - VELOCITY = 2, + INVALID = 0, + VALID = 1, }; - // Active control mode for each actuator - std::vector control_level_; + static constexpr std::array kStatusCommands = { + StatusCommands::MOTOR_STATE, + StatusCommands::MOTOR_STATUS, + }; + 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; + } + + template + inline void pack_decoded_maintenance_frame( + JointT & joint, const DecodedCommand & decoded_maintenance_cmd) + { + joint.decoded_maintenance_frame.clear(); + joint.decoded_maintenance_frame.push_back(decoded_maintenance_cmd.command_id); + joint.decoded_maintenance_frame.insert( + joint.decoded_maintenance_frame.end(), + decoded_maintenance_cmd.u8_data.begin(), + decoded_maintenance_cmd.u8_data.end()); + } }; } // namespace stepper_ros2_control -#endif // STEPPER_HARDWARE_INTERFACE_HPP_ \ No newline at end of file +#endif // STEPPER_HARDWARE_INTERFACE_HPP_ diff --git a/src/hardware_interfaces/stepper_ros2_control/package.xml b/src/hardware_interfaces/stepper_ros2_control/package.xml index c1a4f127..b09db3b4 100644 --- a/src/hardware_interfaces/stepper_ros2_control/package.xml +++ b/src/hardware_interfaces/stepper_ros2_control/package.xml @@ -15,7 +15,6 @@ rclcpp rclcpp_lifecycle realtime_tools - msgs umdloop_can_library @@ -25,9 +24,6 @@ ament_lint_common ament_cmake - rosidl_default_runtime - - rosidl_interface_packages ament_cmake diff --git a/src/hardware_interfaces/stepper_ros2_control/src/stepper_hardware_interface.cpp b/src/hardware_interfaces/stepper_ros2_control/src/stepper_hardware_interface.cpp index d12c3d15..c4427caf 100644 --- a/src/hardware_interfaces/stepper_ros2_control/src/stepper_hardware_interface.cpp +++ b/src/hardware_interfaces/stepper_ros2_control/src/stepper_hardware_interface.cpp @@ -1,567 +1,604 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. - -// -// Authors: Denis Stogl -// - #include "stepper_ros2_control/stepper_hardware_interface.hpp" -#include -#include -#include +#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 "pluginlib/class_list_macros.hpp" #include "rclcpp/rclcpp.hpp" -#define DEBUG_MODE 0 // 0 for off 1 for on +namespace stepper_ros2_control +{ -using std::placeholders::_1; +double STEPPERHardwareInterface::calculate_joint_position_from_motor_position( + double motor_position, int gear_ratio) +{ + return (motor_position * (M_PI / 180.0)) / gear_ratio; +} -namespace stepper_ros2_control +double STEPPERHardwareInterface::calculate_joint_velocity_from_motor_velocity( + double motor_velocity, int gear_ratio) { -hardware_interface::CallbackReturn STEPPERHardwareInterface::on_init( - const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file + return (motor_velocity * (M_PI / 180.0)) / gear_ratio; +} + +int16_t STEPPERHardwareInterface::calculate_motor_position_from_desired_joint_position( + double joint_position, int gear_ratio) { - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { - return hardware_interface::CallbackReturn::ERROR; + return static_cast(std::round((joint_position * (180.0 / M_PI)) * gear_ratio)); +} + +int16_t STEPPERHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity( + double joint_velocity, int gear_ratio) +{ + return static_cast(std::round((joint_velocity * (180.0 / M_PI)) * gear_ratio)); +} + +void STEPPERHardwareInterface::logger_function() +{ + if (num_joints == 0) { + return; } - - /* - IF YOU WANT TO USE PARAMETERS FROM ROS2_CONTROL XACRO, DO THAT HERE!!! - */ - - // This stores the can ids for each joint aka motor - for (auto& joint : info_.joints) { - joint_gear_ratios.push_back(std::stoi(joint.parameters.at("gear_ratio"))); - initial_position_.push_back(std::stof(joint.parameters.at("initial_position"))); - - joint_orientation.push_back(std::stoi(joint.parameters.at("joint_orientation"))); + + std::ostringstream oss; + oss << "\033[2J\033[H \nSTEPPER Logger" + << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface + << " | Command CAN ID: 0x" << std::hex << std::uppercase << can_command_id + << " | Response CAN ID: 0x" << can_response_id << std::dec + << " | HWI Update Rate: " << update_rate + << " | Logger Update Rate: " << logger_rate << "\n" + << "Elapsed Time since first update: " << elapsed_time << "\n" + << "\n--- Joint Specific ---"; + + for (const auto & joint : STEPPERJoints_) { + std::string control_mode = "UNDEFINED"; + if (joint.control_level == integration_level_t::POSITION) { + control_mode = "POSITION"; + } else if (joint.control_level == integration_level_t::VELOCITY) { + control_mode = "VELOCITY"; + } + + oss << "\nJOINT: " << joint.name << "\n" + << "Parameters: Node ID: 0x" << std::hex << std::uppercase + << static_cast(joint.node_id) << std::dec + << " | Gear Ratio: " << joint.gear_ratio + << " | Orientation: " << joint.orientation + << " | Control Mode: " << control_mode << "\n" + << "-- Commands --\n" + << "Motor Position: " << joint.motor_position + << " | Joint Command Position: " << joint.joint_command_position << "\n" + << "Motor Velocity: " << joint.motor_velocity + << " | Joint Command Velocity: " << joint.joint_command_velocity << "\n" + << "Status Request: " << joint.motor_status_req + << " | Maintenance Request: " << joint.motor_maintenance_req << "\n" + << "-- State --\n" + << "Joint Position: " << joint.joint_state_position + << " | Joint Velocity: " << joint.joint_state_velocity + << " | Status: " << joint.motor_status << "\n"; } - num_joints = static_cast(info_.joints.size()); + RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "%s", oss.str().c_str()); +} + +void STEPPERHardwareInterface::format_control_command(CANLib::CanFrame & frame, StepperJoint & joint) +{ + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); - for (size_t i = 0; i < initial_position_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Joint %zu initial position in on_init: %f", i, initial_position_[i]); + if ( + joint.control_level == integration_level_t::POSITION && + std::isfinite(joint.joint_command_position) && + joint.joint_command_position != joint.prev_joint_command_position) + { + joint.prev_joint_command_position = joint.joint_command_position; + const int16_t joint_position = static_cast( + joint.orientation * + calculate_motor_position_from_desired_joint_position( + joint.joint_command_position, joint.gear_ratio)); + + frame.dlc = 3; + frame.data[0] = static_cast( + static_cast(ControlCommands::RELATIVE_POS_CONTROL_CMD) + joint.node_id); + frame.data[1] = static_cast(joint_position & 0xFF); + frame.data[2] = static_cast((joint_position >> 8) & 0xFF); + return; + } + else if ( + joint.control_level == integration_level_t::VELOCITY && + std::isfinite(joint.joint_command_velocity) && + joint.joint_command_velocity != joint.prev_joint_command_velocity) + { + joint.prev_joint_command_velocity = joint.joint_command_velocity; + int16_t joint_velocity = static_cast( + joint.orientation * + calculate_motor_velocity_from_desired_joint_velocity( + joint.joint_command_velocity, joint.gear_ratio)); + + // TEMPORARY: AUTOMATICALLY SET VALUES TO 900/0/-900 BASED ON SIGN OF VELOCITY COMMAND + if (joint_velocity > 0) { + joint_velocity = 900; + } else if (joint_velocity < 0) { + joint_velocity = -900; + } + else { + joint_velocity = 0; + } + // TEMPORARY END + + frame.dlc = 3; + frame.data[0] = static_cast( + static_cast(ControlCommands::VELOCITY_CONTROL_CMD) + joint.node_id); + frame.data[1] = static_cast(joint_velocity & 0xFF); + frame.data[2] = static_cast((joint_velocity >> 8) & 0xFF); + return; + } + else { + frame.dlc = 2; + frame.data[0] = static_cast( + static_cast(StatusCommands::MOTOR_STATE) + joint.node_id); + frame.data[1] = static_cast(ValidateRequest::VALID); } - - // Initializes command and state interface values - joint_state_position_.assign(num_joints, 0.0); - joint_state_velocity_.assign(num_joints, 0.0); - // joint_command_position_.assign(num_joints, 0); - joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); +} - // joint_command_position_ = initial_position_; - joint_command_velocity_.assign(num_joints, 0.0); +bool STEPPERHardwareInterface::format_status_command( + CANLib::CanFrame & frame, uint8_t command_id, uint8_t node_id) +{ + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); + frame.dlc = 2; + switch (static_cast(command_id)) { + case StatusCommands::MOTOR_STATE: + frame.data[0] = static_cast(static_cast(StatusCommands::MOTOR_STATE) + node_id); + frame.data[1] = static_cast(ValidateRequest::VALID); + return true; + case StatusCommands::MOTOR_STATUS: + frame.data[0] = static_cast(static_cast(StatusCommands::MOTOR_STATUS) + node_id); + frame.data[1] = static_cast(ValidateRequest::VALID); + return true; + default: + return false; + } +} - for (size_t i = 0; i < initial_position_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Joint %zu command vel in on_init: %f", i, joint_command_velocity_[i]); +bool STEPPERHardwareInterface::format_maintenance_command( + CANLib::CanFrame & frame, uint8_t node_id, const DecodedCommand & decoded_cmd) +{ + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); + frame.dlc = 2; + frame.data[0] = static_cast( + static_cast(MaintenanceCommands::MAINTENANCE_CMD) + node_id); + frame.data[1] = decoded_cmd.command_id; + + switch (static_cast(decoded_cmd.command_id)) { + case MaintenanceCommands::PCB_HEARTBEAT_CMD: + if (decoded_cmd.u8_data.size() != 1 || !decoded_cmd.i16_data.empty() || + !decoded_cmd.i32_data.empty()) + { + return false; + } + frame.data[0] = static_cast(MaintenanceCommands::PCB_HEARTBEAT_CMD); + frame.data[1] = decoded_cmd.u8_data[0]; + return true; + case MaintenanceCommands::MAINTENANCE_CMD: + if (decoded_cmd.u8_data.size() != 1 || !decoded_cmd.i16_data.empty() || + !decoded_cmd.i32_data.empty()) + { + return false; + } + frame.data[0] = static_cast(static_cast(MaintenanceCommands::MAINTENANCE_CMD) + node_id); + frame.data[1] = decoded_cmd.u8_data[0]; + return true; + case MaintenanceCommands::STEPPER_SPECS_CMD: + if (decoded_cmd.u8_data.size() != 1 || !decoded_cmd.i16_data.empty() || + !decoded_cmd.i32_data.empty()) + { + return false; + } + frame.data[0] = static_cast(static_cast(MaintenanceCommands::STEPPER_SPECS_CMD) + node_id); + frame.data[1] = static_cast(ValidateRequest::VALID); + return true; + default: + return false; } +} - motor_temperature_.assign(num_joints, 0.0); - motor_torque_current_.assign(num_joints, 0.0); +hardware_interface::CallbackReturn STEPPERHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } - encoder_position = 0.0; - motor_position = 0.0; - motor_velocity = 0.0; + STEPPERJoints_.clear(); + 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 = static_cast(can_command_id + 0x01); + + for (const auto & joint : info_.joints) { + std::vector state_if_names; + for (const auto & si : joint.state_interfaces) { + state_if_names.push_back(si.name); + } - joint_initialization_.assign(num_joints, false); + std::vector command_if_names; + for (const auto & ci : joint.command_interfaces) { + command_if_names.push_back(ci.name); + } - control_level_.resize(num_joints, integration_level_t::POSITION); + std::unordered_map params_map; + for (const auto & p : joint.parameters) { + params_map.emplace(p.first, p.second); + } - current_iteration = 0; + const int orientation = joint.parameters.count("joint_orientation") && + std::stoi(joint.parameters.at("joint_orientation")) == -1 ? -1 : 1; + const integration_level_t initial_mode = + joint.parameters.count("joint_type") && joint.parameters.at("joint_type") == "continuous" ? + integration_level_t::VELOCITY : integration_level_t::POSITION; + + STEPPERJoints_.push_back(StepperJoint{ + joint.name, + static_cast(std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0, 15)), + std::abs(std::stoi(joint.parameters.at("gear_ratio"))), + orientation, + initial_mode, + std::numeric_limits::quiet_NaN(), + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + std::numeric_limits::quiet_NaN(), + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + {}, + 0.0, + 0.0, + 0.0, + 0.0, + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + state_if_names, + command_if_names, + params_map + }); + } + num_joints = static_cast(STEPPERJoints_.size()); + elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; + write_count = 0; return hardware_interface::CallbackReturn::SUCCESS; } - hardware_interface::CallbackReturn STEPPERHardwareInterface::on_shutdown( const rclcpp_lifecycle::State & previous_state) { - // Reuse cleanup logic which shuts down the motor and then deinitializes shared pointers. - // Need this in case on_cleanup never gets called - return this->on_cleanup(previous_state); + return on_cleanup(previous_state); } - std::vector STEPPERHardwareInterface::export_state_interfaces() { std::vector state_interfaces; + for (auto & joint : STEPPERJoints_) { + for (const auto & iface : joint.state_interface_names) { + double * value = nullptr; + if (iface == hardware_interface::HW_IF_POSITION) { + value = &joint.joint_state_position; + } else if (iface == hardware_interface::HW_IF_VELOCITY) { + value = &joint.joint_state_velocity; + } else if (iface == "status") { + value = &joint.motor_status; + } - // Each STEPPER motor corresponds to a different joint. - 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 - 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])); + if (value == nullptr) { + continue; + } + state_interfaces.emplace_back(joint.name, iface, value); + } } - return state_interfaces; } - -std::vector -STEPPERHardwareInterface::export_command_interfaces() +std::vector STEPPERHardwareInterface::export_command_interfaces() { std::vector command_interfaces; + for (auto & joint : STEPPERJoints_) { + for (const auto & iface : joint.command_interface_names) { + double * value = nullptr; + if (iface == hardware_interface::HW_IF_POSITION) { + value = &joint.joint_command_position; + } else if (iface == hardware_interface::HW_IF_VELOCITY) { + value = &joint.joint_command_velocity; + } else if (iface == "status_request") { + value = &joint.motor_status_req; + } else if (iface == "maintenance_request") { + value = &joint.motor_maintenance_req; + } else if (iface == "maintenance_frame_high") { + value = &joint.maintenance_frame_high; + } else if (iface == "maintenance_frame_low") { + value = &joint.maintenance_frame_low; + } else if (iface == "maintenance_data_count") { + value = &joint.maintenance_data_count; + } - 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])); + if (value == nullptr) { + continue; + } + command_interfaces.emplace_back(joint.name, iface, value); + } } - return command_interfaces; } hardware_interface::CallbackReturn STEPPERHardwareInterface::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - // TODO: Find a way to publish from the lifecyle node itself rather than create our own node within it - // Node to publish to umdloop_can_node - node_ = rclcpp::Node::make_shared("stepper_hardware_node"); - science_can_publisher_ = node_->create_publisher("can_tx", rclcpp::QoS(10)); - - // Lambda function that takes the message as a shared pointer, dereferences it, - // and stores it in received_joint_data_ to be used - science_can_subscriber_ = node_->create_subscription( - "can_rx", - rclcpp::QoS(50).reliable(), - [this](const msgs::msg::CANA::SharedPtr received_message) - { - received_joint_data_ = *received_message; - } - ); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -hardware_interface::CallbackReturn STEPPERHardwareInterface::on_cleanup( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // If cleanup occurs before shutdown, this is the last opportunity to shutdown motor since pointers must be deleted here - for(int i = 0; i < num_joints; i++){ - auto joint_tx = msgs::msg::CANA(); - - // Motor Shutdown Command - joint_tx.data = {0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - science_can_publisher_->publish(joint_tx); - - // Brake Lock Command (don't think this works) - joint_tx.data = {0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - science_can_publisher_->publish(joint_tx); + if (!canBus.open( + can_interface, + std::bind(&STEPPERHardwareInterface::on_can_message, this, std::placeholders::_1))) + { + RCLCPP_ERROR(rclcpp::get_logger("STEPPERHardwareInterface"), "Failed to open CAN interface"); + return hardware_interface::CallbackReturn::ERROR; } - - // Reset shared pointers which essentially deletes it - science_can_publisher_.reset(); - science_can_subscriber_.reset(); - node_.reset(); - return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn STEPPERHardwareInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) +void STEPPERHardwareInterface::on_can_message(const CANLib::CanFrame & frame) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Activating ...please wait..."); - - for(int i = 0; i < num_joints; i++){ - auto joint_tx = msgs::msg::CANA(); - - // Brake Release command (pretty sure brakes don't work) - joint_tx.data = {0x77, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - science_can_publisher_->publish(joint_tx); + can_rx_frame_ = frame; + if (frame.id != can_response_id) { + return; } - for (size_t i = 0; i < initial_position_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Joint %zu initial position in on_activate: %f", i, initial_position_[i]); - } - - // Initialize command positions and velocities to zero to prevent initial movement - // Controllers will override these once they start - joint_command_position_.assign(num_joints, 0.0); - joint_command_velocity_.assign(num_joints, 0.0); - - for (size_t i = 0; i < joint_command_position_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Joint %zu command position initialized to: %f", i, joint_command_position_[i]); - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Joint %zu command velocity initialized to: %f", i, joint_command_velocity_[i]); - } + const uint8_t command_nibble = static_cast((frame.data[0] >> 4) & 0x0F); + const uint8_t device_id_nibble = static_cast(frame.data[0] & 0x0F); - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Successfully activated!"); - return hardware_interface::CallbackReturn::SUCCESS; -} + for (auto & joint : STEPPERJoints_) { + if (joint.node_id != device_id_nibble) { + continue; + } + if (command_nibble == (static_cast(StatusCommands::MOTOR_STATE) >> 4) || + command_nibble == (static_cast(ControlCommands::RELATIVE_POS_CONTROL_CMD) >> 4) || + command_nibble == (static_cast(ControlCommands::VELOCITY_CONTROL_CMD) >> 4)) { + const double raw_position = static_cast( + static_cast((frame.data[2] << 8) | frame.data[1])); + const double raw_velocity = static_cast( + static_cast((frame.data[4] << 8) | frame.data[3])); + + joint.motor_position = joint.orientation * + calculate_joint_position_from_motor_position(raw_position, joint.gear_ratio); + joint.motor_velocity = joint.orientation * + calculate_joint_velocity_from_motor_velocity(raw_velocity, joint.gear_ratio); + return; + } -hardware_interface::CallbackReturn STEPPERHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Deactivating ...please wait..."); - - for(int i = 0; i < num_joints; i++){ - auto joint_tx = msgs::msg::CANA(); - - // Motor Stop Command - joint_tx.data = {0x81, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - science_can_publisher_->publish(joint_tx); - } + if (command_nibble == (static_cast(StatusCommands::MOTOR_STATUS) >> 4)) { + joint.motor_status = static_cast(frame.data[1]); + return; + } - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Successfully deactivated all STEPPER motors!"); + if (command_nibble == (static_cast(MaintenanceCommands::MAINTENANCE_CMD) >> 4) && frame.data[0] == 1) { + RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Successfully sent maintenance command"); + return; + } - return hardware_interface::CallbackReturn::SUCCESS; -} + if (command_nibble == (static_cast(MaintenanceCommands::STEPPER_SPECS_CMD) >> 4)) { + const uint8_t command_id = frame.data[0]; + const uint8_t stepper_type = frame.data[1]; -double STEPPERHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ - // Converts from 0.01 deg to deg to radians/s - return (motor_position * 0.01 * (M_PI/180.0))/gear_ratio; -} + const uint16_t max_stepper_position = + static_cast(frame.data[2]) | + (static_cast(frame.data[3]) << 8); -double STEPPERHardwareInterface::calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ - // Converts from dps to radians/s - return (motor_velocity * (M_PI/180.0))/gear_ratio; -} + const uint16_t max_stepper_velocity = + static_cast(frame.data[4]) | + (static_cast(frame.data[5]) << 8); -int32_t STEPPERHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ - // radians -> deg -> 0.01 deg - return static_cast(std::round((joint_position*(180/M_PI)*100)*gear_ratio)); -} - -int32_t STEPPERHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio){ - // radians/s -> deg/s -> 0.01 deg/s - return static_cast(std::round((joint_velocity*(180/M_PI)*100)*gear_ratio)); + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Stepper Config Reply | " + "command_id: 0x%02X (%u) | " + "stepper_type: %u | " + "max_stepper_position: %u | " + "max_stepper_velocity: %u", + command_id, + command_id, + stepper_type, + max_stepper_position, + max_stepper_velocity + ); + return; + } + } } - - -hardware_interface::return_type STEPPERHardwareInterface::read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +hardware_interface::CallbackReturn STEPPERHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State &) { - - if (!node_) { - RCLCPP_ERROR(rclcpp::get_logger("STEPPERHardwareInterface"), - "read() called but node_ is nullptr!"); - return hardware_interface::return_type::ERROR; - } - - if (!science_can_publisher_) { - RCLCPP_ERROR(rclcpp::get_logger("STEPPERHardwareInterface"), - "read() called but science_can_publisher_ is nullptr!"); - return hardware_interface::return_type::ERROR; + for (const auto & joint : STEPPERJoints_) { + CANLib::CanFrame frame; + frame.id = can_command_id; + if (format_maintenance_command( + frame, joint.node_id, + DecodedCommand{ + static_cast(MaintenanceCommands::MAINTENANCE_CMD), + {static_cast(MaintenanceCommandOptions::MOTOR_SHUTDOWN_CMD)}, + {}, + {}})) + { + canBus.send(frame); + } } + canBus.close(); + return hardware_interface::CallbackReturn::SUCCESS; +} - if (rclcpp::ok()) - { - rclcpp::spin_some(node_); +hardware_interface::CallbackReturn STEPPERHardwareInterface::on_activate( + const rclcpp_lifecycle::State &) +{ + for (auto & joint : STEPPERJoints_) { + joint.joint_command_position = joint.joint_state_position; + joint.joint_command_velocity = 0.0; } + return hardware_interface::CallbackReturn::SUCCESS; +} - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - - // Since you can't publish two messages in one sweep without a delay, every other iteration of read - // will call angle or velocity messages - current_iteration++; - for(int i = 0; i < num_joints; i++) { - auto joint_tx = msgs::msg::CANA(); - if(current_iteration%2 == 0){ - // Command to read multi-turn angle - - joint_tx.data = {0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; +hardware_interface::CallbackReturn STEPPERHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State &) +{ + for (const auto & joint : STEPPERJoints_) { + CANLib::CanFrame frame; + frame.id = can_command_id; + if (format_maintenance_command( + frame, joint.node_id, + DecodedCommand{ + static_cast(MaintenanceCommands::MAINTENANCE_CMD), + {static_cast(MaintenanceCommandOptions::MOTOR_STOP_CMD)}, + {}, + {}})) + { + canBus.send(frame); } - else if(current_iteration%2 == 1){ - // Command to read motor status 2 + } + return hardware_interface::CallbackReturn::SUCCESS; +} - joint_tx.data = {0x9C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - } - science_can_publisher_->publish(joint_tx); +hardware_interface::return_type STEPPERHardwareInterface::read( + const rclcpp::Time &, const rclcpp::Duration &) +{ + for (auto & joint : STEPPERJoints_) { + joint.joint_state_position = joint.motor_position; + joint.joint_state_velocity = joint.motor_velocity; } + return hardware_interface::return_type::OK; +} - // Safety check - if (received_joint_data_.data.size() < 8) { - if (DEBUG_MODE == 1) { - RCLCPP_INFO( - rclcpp::get_logger("STEPPERHardwareInterface"), - "read(): received_joint_data_ has only %zu bytes, waiting for full frame", - received_joint_data_.data.size()); +hardware_interface::return_type STEPPERHardwareInterface::write( + const rclcpp::Time &, const rclcpp::Duration & period) +{ + 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(); } - return hardware_interface::return_type::OK; } - // Values retrieved - for(int i = 0; i < num_joints; i++){ - if(received_joint_data_.data[0] == 0x9C) { - - // DECODING CAN MESSAGE FOR VELOCITY - data[0] = 0x9C; - data[1] = received_joint_data_.data[1]; // Motor Temperature - data[2] = received_joint_data_.data[2]; // Torque low byte - data[3] = received_joint_data_.data[3]; // Torque high byte - data[4] = received_joint_data_.data[4]; // speed low byte - data[5] = received_joint_data_.data[5]; // speed high byte - data[6] = received_joint_data_.data[6]; // encoder position low byte - data[7] = received_joint_data_.data[7]; // encoder position high byte - - // ENCODER POSITION - // uint16 -> int16 -> double (for calcs) - encoder_position = static_cast(static_cast((data[7] << 8) | data[6])); - - // SPEED - // uint16 -> int16 -> double (for calcs) - motor_velocity = static_cast(static_cast((data[5] << 8) | data[4])); - - // CALCULATING JOINT STATE - joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity, joint_gear_ratios[i]); - - // TEMPERATURE (1 byte, degrees C) - motor_temperature_[i] = static_cast(data[1]); - - // TORQUE CURRENT (16-bit signed, 0.01A per LSB) - motor_torque_current_[i] = static_cast(static_cast((data[3] << 8) | data[2])) * 0.01; - - } - else if(received_joint_data_.data[0] == 0x92) { - - // DECODING CAN MESSAGE FOR POSITION - data[0] = 0x92; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = received_joint_data_.data[4]; // Multi-turn low byte - data[5] = received_joint_data_.data[5]; - data[6] = received_joint_data_.data[6]; - data[7] = received_joint_data_.data[7]; // Multi-turn high byte - - // POSITION - // uint32 -> int32 -> double (for calcs) - motor_position = static_cast(static_cast((data[7] << 24) | (data[6] << 16) | (data[5] << 8) | data[4])); - - if(DEBUG_MODE == 1) { - for(int j = 0; j < 8; j++){ - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Received at index %d: %d", j, received_joint_data_.data[j]); + for (auto & joint : STEPPERJoints_) { + const double curr_status_req = joint.motor_status_req; + if (curr_status_req < 0.0 && joint.prev_status_req >= 0.0) { + for (auto status_cmd : kStatusCommands) { + CANLib::CanFrame frame; + frame.id = can_command_id; + if (format_status_command(frame, static_cast(status_cmd), joint.node_id)) { + canBus.send(frame); } - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Motor Position: %f", motor_position); } - - // CALCULATING JOINT STATE - joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position, joint_gear_ratios[i]); - - // This initializes the command position for the joint (will not work if state isn't read immediately) - // if(!joint_initialization_[i]){ - // joint_command_position_[i] = joint_state_position_[i]; - // joint_initialization_[i] = true; - // } - } - else{ - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Reply not heard."); + } 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; + for (auto status_cmd : kStatusCommands) { + CANLib::CanFrame frame; + frame.id = can_command_id; + if (format_status_command(frame, static_cast(status_cmd), joint.node_id)) { + canBus.send(frame); + } + } } } - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Reading for joint: %s Motor Position: %f Joint position: %f Joint velocity: %f \nJoint Initialization State: %d\n", - info_.joints[i].name.c_str(), - motor_position, - joint_state_position_[i], - joint_state_velocity_[i], - static_cast(joint_initialization_[i])); - } - + joint.prev_status_req = curr_status_req; } - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type stepper_ros2_control::STEPPERHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - int32_t joint_angle = 0; - int16_t operating_velocity = 200; - int32_t joint_velocity = 0; - - for(int i = 0; i < num_joints; i++) { - auto joint_tx = msgs::msg::CANA(); // Must reinstantiate else data from past iteration gets repeated - - - if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { - - // CALCULATE DESIRED JOINT ANGLE - joint_angle = joint_orientation[i]*calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Writing positions for: %s Joint angle of motor (0.01 deg): %d Joint command: %f", - info_.joints[i].name.c_str(), - joint_angle, - joint_command_position_[i]); - } - - // ENCODING CAN MESSAGE - data[0] = 0xA4; - data[1] = 0x00; - data[2] = operating_velocity & 0xFF; - data[3] = (operating_velocity >> 8) & 0xFF; - data[4] = joint_angle & 0xFF; - data[5] = (joint_angle >> 8) & 0xFF; - data[6] = (joint_angle >> 16) & 0xFF; - data[7] = (joint_angle >> 24) & 0xFF; - + for (auto & joint : STEPPERJoints_) { + 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)); + pack_decoded_maintenance_frame(joint, decoded_maintenance_cmd); + + CANLib::CanFrame frame; + frame.id = can_command_id; + if (!format_maintenance_command(frame, joint.node_id, decoded_maintenance_cmd)) { + joint.prev_maintenance_req = joint.motor_maintenance_req; + continue; } - else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { - - // CALCULATE DESIRED JOINT VELOCITY - joint_velocity = joint_orientation[i]*calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d Joint command velocity: %f Orientation: %d", - info_.joints[i].name.c_str(), - joint_velocity, - joint_command_velocity_[i], - joint_orientation[i]); + + const double curr_maintenance_req = joint.motor_maintenance_req; + if (curr_maintenance_req < 0.0 && joint.prev_maintenance_req >= 0.0) { + canBus.send(frame); + } else if (curr_maintenance_req > 0.0) { + joint.elapsed_maintenance_request_time += period.seconds(); + if (joint.elapsed_maintenance_request_time > (1.0 / curr_maintenance_req)) { + joint.elapsed_maintenance_request_time = 0.0; + canBus.send(frame); } - - // ENCODING CAN MESSAGE - data[0] = 0xA2; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = joint_velocity & 0xFF; - data[5] = (joint_velocity >> 8) & 0xFF; - data[6] = (joint_velocity >> 16) & 0xFF; - data[7] = (joint_velocity >> 24) & 0xFF; - - } - else{ - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Joint command value not found or undefined command state"); } + joint.prev_maintenance_req = curr_maintenance_req; + } - for(int j = 0; j < 8; j++){ - joint_tx.data.push_back(static_cast(data[j])); + elapsed_update_time += period.seconds(); + if (update_rate > 0 && elapsed_update_time > (1.0 / static_cast(update_rate))) { + elapsed_update_time = 0.0; + if (!STEPPERJoints_.empty()) { + auto & joint = STEPPERJoints_[static_cast(write_count % num_joints)]; + ++write_count; + CANLib::CanFrame frame; + frame.id = can_command_id; + format_control_command(frame, joint); + canBus.send(frame); } - - science_can_publisher_->publish(joint_tx); } - - return hardware_interface::return_type::OK; } -// hardware_interface::return_type STEPPERHardwareInterface::perform_command_mode_switch( -// const std::vector& start_interfaces, -// const std::vector& stop_interfaces) -// { -// std::vector new_modes = {}; -// for (std::string key : start_interfaces) -// { -// for (int i = 0; i < num_joints; i++){ -// if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION){ -// new_modes.push_back(integration_level_t::POSITION); -// } -// if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY){ -// new_modes.push_back(integration_level_t::VELOCITY); -// } -// } -// } -// // All joints must be given new command mode at the same time -// // if (new_modes.size() != static_cast(num_joints)){ -// // return hardware_interface::return_type::ERROR; -// // } -// // All joints must have the same command mode -// // if (!std::all_of( -// // new_modes.begin() + 1, new_modes.end(), -// // [&](integration_level_t mode) { return mode == new_modes[0]; })) -// // { -// // return hardware_interface::return_type::ERROR; -// // } - -// // Stop motion on all relevant joints that are stopping -// for (std::string key : stop_interfaces) { -// for (int i = 0; i < num_joints; i++) { -// if (key.find(info_.joints[i].name) != std::string::npos) { -// // fix this -// joint_command_position_[i] = initial_position_[i]; -// joint_command_velocity_[i] = 0; -// control_level_[i] = integration_level_t::UNDEFINED; // Revert to undefined -// } -// } -// } -// // Set the new command modes. By this point everything should be undefined after the "stop motion" loop -// for (int i = 0; i < num_joints; i++) { -// if (control_level_[i] != integration_level_t::UNDEFINED) { -// // Something else is using the joint! Abort! -// return hardware_interface::return_type::ERROR; -// } -// control_level_[i] = new_modes[i]; -// } - -// return hardware_interface::return_type::OK; -// } - hardware_interface::return_type STEPPERHardwareInterface::perform_command_mode_switch( - const std::vector& start_interfaces, - const std::vector& stop_interfaces) + const std::vector & start_interfaces, + const std::vector & stop_interfaces) { - // Debug: print incoming requests - std::ostringstream ss; - ss << "perform_command_mode_switch called. start_interfaces: ["; - for (auto &s : start_interfaces) ss << s << ","; - ss << "] stop_interfaces: ["; - for (auto &s : stop_interfaces) ss << s << ","; - ss << "]"; - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), ss.str().c_str()); - - // For each joint, decide its new control mode based on start/stop interfaces. - // We allow partial starts/stops: only affected joints are switched. - std::vector requested_modes(num_joints, integration_level_t::UNDEFINED); - - // Process stop interfaces first: mark those joints as UNDEFINED - for (const auto &ifname : stop_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 || ifname == vel_if || ifname.find(info_.joints[i].name) != std::string::npos) { + std::vector requested_modes( + static_cast(num_joints), integration_level_t::UNDEFINED); + + for (const auto & ifname : stop_interfaces) { + for (size_t i = 0; i < STEPPERJoints_.size(); ++i) { + const auto & joint = STEPPERJoints_[i]; + const std::string pos_if = joint.name + "/" + hardware_interface::HW_IF_POSITION; + const std::string vel_if = joint.name + "/" + hardware_interface::HW_IF_VELOCITY; + if (ifname == pos_if || ifname == vel_if || ifname.find(joint.name) != std::string::npos) { requested_modes[i] = integration_level_t::UNDEFINED; } } } - // Process start interfaces: set POSITION or VELOCITY per joint - 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); + for (const auto & ifname : start_interfaces) { + for (size_t i = 0; i < STEPPERJoints_.size(); ++i) { + const auto & joint = STEPPERJoints_[i]; + const std::string pos_if = joint.name + "/" + hardware_interface::HW_IF_POSITION; + const std::string vel_if = joint.name + "/" + hardware_interface::HW_IF_VELOCITY; if (ifname == pos_if) { requested_modes[i] = integration_level_t::POSITION; } else if (ifname == vel_if) { @@ -570,54 +607,36 @@ hardware_interface::return_type STEPPERHardwareInterface::perform_command_mode_s } } - // Now apply the requested_modes to control_level_. - // For any joint with UNDEFINED in requested_modes, we only change it if it was explicitly stopped. - for (int i = 0; i < num_joints; ++i) { + for (size_t i = 0; i < STEPPERJoints_.size(); ++i) { + auto & joint = STEPPERJoints_[i]; if (requested_modes[i] == integration_level_t::UNDEFINED) { - // if stop requested, set to UNDEFINED; otherwise leave existing mode - // (we only set to UNDEFINED if this joint was mentioned in stop_interfaces) bool was_stopped = false; - for (const auto &ifname : stop_interfaces) { - if (ifname.find(info_.joints[i].name) != std::string::npos) { + for (const auto & ifname : stop_interfaces) { + if (ifname.find(joint.name) != std::string::npos) { was_stopped = true; break; } } if (was_stopped) { - control_level_[i] = integration_level_t::UNDEFINED; - joint_command_velocity_[i] = 0; - // optional: reset position cmd to current state to be safe - joint_command_position_[i] = joint_state_position_[i]; - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), - "Joint %s: stopped -> set UNDEFINED", info_.joints[i].name.c_str()); + joint.control_level = integration_level_t::UNDEFINED; + joint.joint_command_velocity = 0.0; + joint.joint_command_position = joint.joint_state_position; } - // else, leave control_level_ as-is + continue; + } + + joint.control_level = requested_modes[i]; + if (joint.control_level == integration_level_t::VELOCITY) { + joint.joint_command_velocity = 0.0; } else { - // Set the mode requested - control_level_[i] = requested_modes[i]; - // If switching to velocity, optionally set command velocity to current state to avoid jumps - if (requested_modes[i] == integration_level_t::VELOCITY) { - // joint_command_velocity_[i] = joint_state_velocity_[i]; - joint_command_velocity_[i] = 0; - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), - "Joint %s: switched to VELOCITY (cmd vel initialized from state: %f)", - info_.joints[i].name.c_str(), joint_command_velocity_[i]); - } else if (requested_modes[i] == integration_level_t::POSITION) { - joint_command_position_[i] = joint_state_position_[i]; - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), - "Joint %s: switched to POSITION (cmd pos initialized from state: %f)", - info_.joints[i].name.c_str(), joint_command_position_[i]); - } + joint.joint_command_position = joint.joint_state_position; } } return hardware_interface::return_type::OK; } - -} // namespace stepper_hardware_interface - -#include "pluginlib/class_list_macros.hpp" +} // namespace stepper_ros2_control PLUGINLIB_EXPORT_CLASS( stepper_ros2_control::STEPPERHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file diff --git a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp index 276bb739..bac540c0 100644 --- a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp +++ b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp @@ -1,130 +1,114 @@ #ifndef TALON_HARDWARE_INTERACE_HPP_ #define TALON_HARDWARE_INTERACE_HPP_ -#include +#include #include #include -#include - -#include #include +#include +#include -#include "hardware_interface/system_interface.hpp" #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 "talon_ros2_control/talon_control.h" namespace talon_ros2_control { -class TALONHardwareInterface : public hardware_interface::SystemInterface // Inheriting from System Interface + +class TALONHardwareInterface : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(TALONHardwareInterface) TALONHardwareInterface(); - // Initialization, so reading parameters, initializing variables, checking if all the joint state and command interfaces are correct hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - // Exports/exposes Interfaces that are available so that the controllers - // know what to read and write to std::vector export_state_interfaces() override; - std::vector export_command_interfaces() override; - // Lifecycle 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; + 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 logger_function(); void enable_system_thread(); private: - // Hardware Interface Parameters - int update_rate; - double elapsed_update_time; // Time since last hardware interface update - double elapsed_time; // Time since first hardware interface update - double elapsed_logger_time; // Time since last logger update - int logger_rate; // Logger update rate - int logger_state; // Logger on/off state - - // Keeps track of amount of joints - int num_joints; - - // Maximum displacement for prismatic joints - std::vector max_disp; - - // Store the state for the simulated robot - std::vector joint_state_position_; - std::vector joint_state_velocity_; - - // Store the command for the simulated robot - std::vector joint_command_position_; - std::vector joint_command_velocity_; - - // Telemetry data - std::vector motor_temperature_; // Motor temperature in °C - std::vector motor_torque_current_; // Motor output current in A - - // Talon specific information - std::vector joint_node_ids; - std::string can_interface; - std::vector talon_motors; - std::vector motor_configs_; - std::thread worker; - std::atomic is_running = false; - - // Modes for control mode - enum integration_level_t : std::uint8_t + enum class integration_level_t : std::uint8_t { UNDEFINED = 0, POSITION = 1, VELOCITY = 2, }; - // Active control mode - std::vector control_level_; - - // Types of joints enum class joint_type_t : std::uint8_t { REVOLUTE = 0, PRISMATIC = 1, }; - // Type of joint for each actuator - std::vector joint_type_; + struct TalonJoint + { + std::string name; + int node_id; + joint_type_t joint_type; + double max_disp; + MotorConfig motor_config; + TalonSRX * motor; + integration_level_t control_level; + + double joint_state_position; + double joint_state_velocity; + double motor_temperature; + double motor_torque_current; + double motor_status; + + double joint_command_position; + double joint_command_velocity; + double motor_status_req; + double prev_status_req; + double elapsed_status_request_time; + + std::vector state_interface_names; + std::vector command_interface_names; + std::unordered_map parameters; + }; + + int update_rate; + double elapsed_update_time; + double elapsed_time; + double elapsed_logger_time; + int logger_rate; + int logger_state; + int num_joints; + + std::string can_interface; + std::vector TALONJoints_; + std::thread worker; + std::atomic is_running{false}; }; -} // namespace talon_hardware_interface +} // namespace talon_ros2_control #endif // TALON_HARDWARE_INTERACE_HPP_ - diff --git a/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp b/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp index 44aa2344..26e41802 100644 --- a/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp +++ b/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp @@ -1,115 +1,87 @@ #include "talon_ros2_control/talon_hardware_interface.hpp" -#include -#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 "pluginlib/class_list_macros.hpp" #include "rclcpp/rclcpp.hpp" -#include "talon_ros2_control/talon_control.h" - namespace talon_ros2_control { -TALONHardwareInterface::TALONHardwareInterface() { -} -void TALONHardwareInterface::logger_function(){ +TALONHardwareInterface::TALONHardwareInterface() = default; - // Prevents breaking the logger - if (num_joints == 0) return; +void TALONHardwareInterface::logger_function() +{ + if (num_joints == 0) { + return; + } - // Building Message - std::string log_msg = "\033[2J\033[H \nTALON Logger"; - std::string control_mode = ""; - std::string joint_type = ""; std::ostringstream oss; - std::string status; - - // HWI Specific - oss << "\n--- HWI Specific ---\n" + oss << "\033[2J\033[H \nTALON 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 ---"; - for (int i = 0; i < num_joints; i++) { - if(static_cast(control_level_[i]) == 1) { - control_mode = "POSITION"; - } - else if(static_cast(control_level_[i]) == 2) { - control_mode = "VELOCITY"; - } - else { - control_mode = "UNDEFINED"; - } + for (const auto & joint : TALONJoints_) { + const char * control_mode = joint.control_level == integration_level_t::POSITION ? "POSITION" : + (joint.control_level == integration_level_t::VELOCITY ? "VELOCITY" : "UNDEFINED"); + const char * joint_type = joint.joint_type == joint_type_t::PRISMATIC ? "PRISMATIC" : "REVOLUTE"; - if(static_cast(joint_type_[i]) == 0) { - joint_type = "REVOLUTE"; - } - else if(static_cast(joint_type_[i]) == 1) { - joint_type = "PRISMATIC"; - } - - oss << "\nJOINT: " << info_.joints[i].name << "\n" - << "Parameters: Node ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] << "\n" + oss << "\nJOINT: " << joint.name << "\n" + << "Parameters: Node ID: 0x" << std::hex << std::uppercase << joint.node_id << std::dec << "\n" << "-- Commands --\n" << "Control Mode: " << control_mode << "\n" << "Joint Type: " << joint_type << "\n" - << "Joint Command Position: " << joint_command_position_[i] << "\n" - << "Joint Command Velocity: " << joint_command_velocity_[i] << "\n" + << "Joint Command Position: " << joint.joint_command_position << "\n" + << "Joint Command Velocity: " << joint.joint_command_velocity << "\n" + << "Status Request: " << joint.motor_status_req << "\n" << "-- State --\n" - << "Joint Position: " << joint_state_position_[i] << " | Position in TU: " << getPositionTalonUnits(talon_motors[i]) << " \n" - << "Joint Velocity: " << joint_state_velocity_[i] << "\n"; + << "Joint Position: " << joint.joint_state_position + << " | Joint Velocity: " << joint.joint_state_velocity + << " | Status: " << joint.motor_status << "\n"; } - log_msg += oss.str(); - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), log_msg.c_str()); + RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "%s", oss.str().c_str()); } hardware_interface::CallbackReturn TALONHardwareInterface::on_init( - const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file + const hardware_interface::HardwareInfo & info) { - if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } - - // -- Parameters -- + can_interface = info_.hardware_parameters.at("can_interface"); 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")); - for (auto& joint : info_.joints) { - joint_node_ids.push_back(std::stoi(joint.parameters.at("node_id"))); + TALONJoints_.clear(); + for (const auto & joint : info_.joints) { + std::vector state_if_names; + for (const auto & si : joint.state_interfaces) { + state_if_names.push_back(si.name); + } - std::string joint_type = joint.parameters.at("joint_type"); + std::vector command_if_names; + for (const auto & ci : joint.command_interfaces) { + command_if_names.push_back(ci.name); + } - // Revolute or Prismatic - if (joint_type == "revolute") { - joint_type_.push_back(joint_type_t::REVOLUTE); - max_disp.push_back(std::nan("")); // Not used for revolute joints - } else if (joint_type == "prismatic" && joint.parameters.count("max_disp")) { - joint_type_.push_back(joint_type_t::PRISMATIC); - max_disp.push_back(std::abs(std::stod(joint.parameters.at("max_disp")))); - } else { - RCLCPP_ERROR(rclcpp::get_logger("TALONHardwareInterface"), "Invalid joint_type parameter for joint %s. Must be 'revolute' or 'prismatic'. If prismatic, it must have a 'max_disp' parameter.", joint.name.c_str()); - return hardware_interface::CallbackReturn::ERROR; + std::unordered_map params_map; + for (const auto & p : joint.parameters) { + params_map.emplace(p.first, p.second); } - // Build per-joint motor config (uses MotorConfig defaults if param not specified) MotorConfig mc; if (joint.parameters.count("encoder_resolution")) { mc.encoder_resolution = std::stod(joint.parameters.at("encoder_resolution")); @@ -141,257 +113,264 @@ hardware_interface::CallbackReturn TALONHardwareInterface::on_init( if (joint.parameters.count("config_timeout_ms")) { mc.config_timeout_ms = std::stoi(joint.parameters.at("config_timeout_ms")); } - motor_configs_.push_back(mc); - } - - num_joints = static_cast(info_.joints.size()); - - // Initializes command and state interface values - joint_state_position_.assign(num_joints, 0); - joint_state_velocity_.assign(num_joints, 0); - - joint_command_position_.assign(num_joints, 0); - joint_command_velocity_.assign(num_joints, 0); - motor_temperature_.assign(num_joints, 0.0); - motor_torque_current_.assign(num_joints, 0.0); - - control_level_.resize(num_joints, integration_level_t::POSITION); + const joint_type_t joint_type = joint.parameters.at("joint_type") == "prismatic" ? + joint_type_t::PRISMATIC : joint_type_t::REVOLUTE; + + TALONJoints_.push_back(TalonJoint{ + joint.name, + std::stoi(joint.parameters.at("node_id")), + joint_type, + joint.parameters.count("max_disp") ? std::abs(std::stod(joint.parameters.at("max_disp"))) : + std::numeric_limits::quiet_NaN(), + mc, + nullptr, + integration_level_t::POSITION, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + state_if_names, + command_if_names, + params_map + }); + } + num_joints = static_cast(TALONJoints_.size()); + elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn TALONHardwareInterface::on_shutdown( const rclcpp_lifecycle::State & previous_state) { - // Reuse cleanup logic which shuts down the motor and then deinitializes shared pointers. - // Need this in case on_cleanup never gets called - return this->on_cleanup(previous_state); + return on_cleanup(previous_state); } std::vector TALONHardwareInterface::export_state_interfaces() { std::vector state_interfaces; + for (auto & joint : TALONJoints_) { + for (const auto & iface : joint.state_interface_names) { + double * value = nullptr; + if (iface == hardware_interface::HW_IF_POSITION) { + value = &joint.joint_state_position; + } else if (iface == hardware_interface::HW_IF_VELOCITY) { + value = &joint.joint_state_velocity; + } else if (iface == "motor_temperature") { + value = &joint.motor_temperature; + } else if (iface == "torque_current") { + value = &joint.motor_torque_current; + } else if (iface == "status") { + value = &joint.motor_status; + } - 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 - 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])); + if (value != nullptr) { + state_interfaces.emplace_back(joint.name, iface, value); + } + } } - return state_interfaces; } std::vector TALONHardwareInterface::export_command_interfaces() { std::vector command_interfaces; + for (auto & joint : TALONJoints_) { + for (const auto & iface : joint.command_interface_names) { + double * value = nullptr; + if (iface == hardware_interface::HW_IF_POSITION) { + value = &joint.joint_command_position; + } else if (iface == hardware_interface::HW_IF_VELOCITY) { + value = &joint.joint_command_velocity; + } else if (iface == "status_request") { + value = &joint.motor_status_req; + } - 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])); + if (value != nullptr) { + command_interfaces.emplace_back(joint.name, iface, value); + } + } } - return command_interfaces; } -/* -* Setup communication with motor (init configs, send first CAN message) -*/ hardware_interface::CallbackReturn TALONHardwareInterface::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Beginning configure."); - - for(int i = 0; i < num_joints; i++){ - TalonSRX *motor = new TalonSRX(joint_node_ids[i], can_interface); - talon_motors.push_back(motor); - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), - "talon_motor initialized (node_id=%d, encoder_res=%.1f, gear_ratio=%.2f).", - joint_node_ids[i], motor_configs_[i].encoder_resolution, motor_configs_[i].gear_ratio); - initMotor(motor, motor_configs_[i], can_interface); - } - - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Configure complete."); - - return hardware_interface::CallbackReturn::SUCCESS; + const rclcpp_lifecycle::State &) +{ + for (auto & joint : TALONJoints_) { + joint.motor = new TalonSRX(joint.node_id, can_interface); + initMotor(joint.motor, joint.motor_config, can_interface); + } + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn TALONHardwareInterface::on_cleanup( - const rclcpp_lifecycle::State & /*previous_state*/) { - - for(int i = 0; i < num_joints; i++){ - stopMotor(talon_motors[i], 50); + const rclcpp_lifecycle::State &) +{ + for (auto & joint : TALONJoints_) { + if (joint.motor != nullptr) { + stopMotor(joint.motor, 50); + delete joint.motor; + joint.motor = nullptr; } - - return hardware_interface::CallbackReturn::SUCCESS; + } + return hardware_interface::CallbackReturn::SUCCESS; } -void TALONHardwareInterface::enable_system_thread() { +void TALONHardwareInterface::enable_system_thread() +{ while (is_running.load()) { ctre::phoenix::unmanaged::Unmanaged::FeedEnable(100); std::this_thread::sleep_for(std::chrono::milliseconds(100)); } } -/* -* Activate system -*/ hardware_interface::CallbackReturn TALONHardwareInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Activating ...please wait..."); - is_running.store(true); worker = std::thread(&TALONHardwareInterface::enable_system_thread, this); - - // setPositionFromDisplacement(talon_motor, 0.0, 50); // dangerous atm since this is an incremental encoder, dont want this to crush itself - - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Successfully activated!"); - return hardware_interface::CallbackReturn::SUCCESS; } -/* -* Deactivate system -*/ hardware_interface::CallbackReturn TALONHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State &) { - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Deactivating ...please wait..."); - - for(int i = 0; i < num_joints; i++){ - stopMotor(talon_motors[i], 50); + for (auto & joint : TALONJoints_) { + if (joint.motor != nullptr) { + stopMotor(joint.motor, 50); + } } is_running.store(false); if (worker.joinable()) { worker.join(); } - - // setPositionFromDisplacement(talon_motor, 0.0, 50); // dangerous atm since this is an incremental encoder, dont want this to crush itself - - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Successfully deactivated!"); - return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::return_type TALONHardwareInterface::read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time &, const rclcpp::Duration &) { - for(int i = 0; i < num_joints; i++){ - if (joint_type_[i] == joint_type_t::PRISMATIC) { - joint_state_position_[i] = getPositionDistance(talon_motors[i], motor_configs_[i]); // meters - joint_state_velocity_[i] = getLinearVelocity(talon_motors[i], motor_configs_[i]); // m/s + for (auto & joint : TALONJoints_) { + if (joint.motor == nullptr) { + continue; + } + + if (joint.joint_type == joint_type_t::PRISMATIC) { + joint.joint_state_position = getPositionDistance(joint.motor, joint.motor_config); + joint.joint_state_velocity = getLinearVelocity(joint.motor, joint.motor_config); } else { - joint_state_position_[i] = getPositionRadians(talon_motors[i], motor_configs_[i]); // radians - joint_state_velocity_[i] = getAngularVelocity(talon_motors[i], motor_configs_[i]); // rad/s + joint.joint_state_position = getPositionRadians(joint.motor, joint.motor_config); + joint.joint_state_velocity = getAngularVelocity(joint.motor, joint.motor_config); } - // Telemetry - motor_temperature_[i] = talon_motors[i]->GetTemperature(); - motor_torque_current_[i] = talon_motors[i]->GetOutputCurrent(); + joint.motor_temperature = joint.motor->GetTemperature(); + joint.motor_torque_current = joint.motor->GetOutputCurrent(); + joint.motor_status = 1.0; } return hardware_interface::return_type::OK; } -hardware_interface::return_type talon_ros2_control::TALONHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +hardware_interface::return_type TALONHardwareInterface::write( + const rclcpp::Time &, const rclcpp::Duration & period) { - elapsed_update_time+=period.seconds(); - double update_period = 1.0/update_rate; - elapsed_time+=period.seconds(); - - // Logger update - elapsed_logger_time+=period.seconds(); - double logging_period = 1.0/logger_rate; - if(elapsed_logger_time > logging_period){ + elapsed_update_time += period.seconds(); + elapsed_time += period.seconds(); + elapsed_logger_time += period.seconds(); + + if (logger_rate > 0 && elapsed_logger_time > (1.0 / static_cast(logger_rate))) { elapsed_logger_time = 0.0; if (logger_state == 1) { logger_function(); } } - // HWI can only go as fast as the controller manager. To limit frequency of bus messages, - // keep track of time passed over iterations of this function and if it exceeds the - // desired frequency of the HWI, skip message - if(elapsed_update_time > update_period){ - elapsed_update_time = 0.0; - - // Motor Command for each joint at given frequency - for(int i = 0; i < num_joints; i++){ - if(control_level_[i] == integration_level_t::POSITION && joint_type_[i] == joint_type_t::PRISMATIC && std::isfinite(joint_command_position_[i])) { - - // TO DO: implement joint limits so i dont gotta do this - joint_command_position_[i] = std::clamp(joint_command_position_[i], 0.0, max_disp[i]); - // COMMAND PRISMATIC POSITION - setPositionFromDisplacement(talon_motors[i], joint_command_position_[i], 50, motor_configs_[i]); - - } else if(control_level_[i] == integration_level_t::VELOCITY && joint_type_[i] == joint_type_t::PRISMATIC && std::isfinite(joint_command_velocity_[i])) { - - // COMMAND PRISMATIC VELOCITY - setVelocityFromLinearVelocity(talon_motors[i], joint_command_velocity_[i], 50, motor_configs_[i]); - - } else if(control_level_[i] == integration_level_t::POSITION && joint_type_[i] == joint_type_t::REVOLUTE && std::isfinite(joint_command_position_[i])) { - - // COMMAND REVOLUTE POSITION - setPositionFromJointCommand(talon_motors[i], joint_command_position_[i], 50, motor_configs_[i]); - - } else if(control_level_[i] == integration_level_t::VELOCITY && joint_type_[i] == joint_type_t::REVOLUTE && std::isfinite(joint_command_velocity_[i])) { - - // COMMAND REVOLUTE VELOCITY - setVelocityFromAngularVelocity(talon_motors[i], joint_command_velocity_[i], 50, motor_configs_[i]); + for (auto & joint : TALONJoints_) { + const double curr_status_req = joint.motor_status_req; + if (curr_status_req < 0.0 && joint.prev_status_req >= 0.0) { + joint.motor_status = 1.0; + } 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; + joint.motor_status = 1.0; + } + } + joint.prev_status_req = curr_status_req; + } + if (update_rate > 0 && elapsed_update_time > (1.0 / static_cast(update_rate))) { + elapsed_update_time = 0.0; + for (auto & joint : TALONJoints_) { + if (joint.motor == nullptr) { + continue; } - else { - // RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Joint command value not found or undefined command state"); + + if ( + joint.control_level == integration_level_t::POSITION && + joint.joint_type == joint_type_t::PRISMATIC && + std::isfinite(joint.joint_command_position)) + { + joint.joint_command_position = std::clamp(joint.joint_command_position, 0.0, joint.max_disp); + setPositionFromDisplacement(joint.motor, joint.joint_command_position, 50, joint.motor_config); + } else if ( + joint.control_level == integration_level_t::VELOCITY && + joint.joint_type == joint_type_t::PRISMATIC && + std::isfinite(joint.joint_command_velocity)) + { + setVelocityFromLinearVelocity(joint.motor, joint.joint_command_velocity, 50, joint.motor_config); + } else if ( + joint.control_level == integration_level_t::POSITION && + joint.joint_type == joint_type_t::REVOLUTE && + std::isfinite(joint.joint_command_position)) + { + setPositionFromJointCommand(joint.motor, joint.joint_command_position, 50, joint.motor_config); + } else if ( + joint.control_level == integration_level_t::VELOCITY && + joint.joint_type == joint_type_t::REVOLUTE && + std::isfinite(joint.joint_command_velocity)) + { + setVelocityFromAngularVelocity(joint.motor, joint.joint_command_velocity, 50, joint.motor_config); } } } - + return hardware_interface::return_type::OK; } hardware_interface::return_type TALONHardwareInterface::perform_command_mode_switch( - const std::vector& start_interfaces, - const std::vector& stop_interfaces) + const std::vector & start_interfaces, + const std::vector & stop_interfaces) { - // Debug: print incoming requests - std::ostringstream ss; - ss << "perform_command_mode_switch called. start_interfaces: ["; - for (auto &s : start_interfaces) ss << s << ","; - ss << "] stop_interfaces: ["; - for (auto &s : stop_interfaces) ss << s << ","; - ss << "]"; - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), ss.str().c_str()); - - // For each joint, decide its new control mode based on start/stop interfaces. - // We allow partial starts/stops: only affected joints are switched. - std::vector requested_modes(num_joints, integration_level_t::UNDEFINED); - - // Process stop interfaces first: mark those joints as UNDEFINED - for (const auto &ifname : stop_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 || ifname == vel_if || ifname.find(info_.joints[i].name) != std::string::npos) { + std::vector requested_modes( + static_cast(num_joints), integration_level_t::UNDEFINED); + + for (const auto & ifname : stop_interfaces) { + for (size_t i = 0; i < TALONJoints_.size(); ++i) { + const auto & joint = TALONJoints_[i]; + const std::string pos_if = joint.name + "/" + hardware_interface::HW_IF_POSITION; + const std::string vel_if = joint.name + "/" + hardware_interface::HW_IF_VELOCITY; + if (ifname == pos_if || ifname == vel_if || ifname.find(joint.name) != std::string::npos) { requested_modes[i] = integration_level_t::UNDEFINED; } } } - // Process start interfaces: set POSITION or VELOCITY per joint - 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); + for (const auto & ifname : start_interfaces) { + for (size_t i = 0; i < TALONJoints_.size(); ++i) { + const auto & joint = TALONJoints_[i]; + const std::string pos_if = joint.name + "/" + hardware_interface::HW_IF_POSITION; + const std::string vel_if = joint.name + "/" + hardware_interface::HW_IF_VELOCITY; if (ifname == pos_if) { requested_modes[i] = integration_level_t::POSITION; } else if (ifname == vel_if) { @@ -400,53 +379,36 @@ hardware_interface::return_type TALONHardwareInterface::perform_command_mode_swi } } - // Now apply the requested_modes to control_level_. - // For any joint with UNDEFINED in requested_modes, we only change it if it was explicitly stopped. - for (int i = 0; i < num_joints; ++i) { + for (size_t i = 0; i < TALONJoints_.size(); ++i) { + auto & joint = TALONJoints_[i]; if (requested_modes[i] == integration_level_t::UNDEFINED) { - // if stop requested, set to UNDEFINED; otherwise leave existing mode - // (we only set to UNDEFINED if this joint was mentioned in stop_interfaces) bool was_stopped = false; - for (const auto &ifname : stop_interfaces) { - if (ifname.find(info_.joints[i].name) != std::string::npos) { + for (const auto & ifname : stop_interfaces) { + if (ifname.find(joint.name) != std::string::npos) { was_stopped = true; break; } } if (was_stopped) { - control_level_[i] = integration_level_t::UNDEFINED; - joint_command_velocity_[i] = 0; - // optional: reset position cmd to current state to be safe - joint_command_position_[i] = joint_state_position_[i]; - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), - "Joint %s: stopped -> set UNDEFINED", info_.joints[i].name.c_str()); + joint.control_level = integration_level_t::UNDEFINED; + joint.joint_command_velocity = 0.0; + joint.joint_command_position = joint.joint_state_position; } - // else, leave control_level_ as-is + continue; + } + + joint.control_level = requested_modes[i]; + if (joint.control_level == integration_level_t::VELOCITY) { + joint.joint_command_velocity = 0.0; } else { - // Set the mode requested - control_level_[i] = requested_modes[i]; - // If switching to velocity, optionally set command velocity to current state to avoid jumps - if (requested_modes[i] == integration_level_t::VELOCITY) { - // joint_command_velocity_[i] = joint_state_velocity_[i]; - joint_command_velocity_[i] = 0; - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), - "Joint %s: switched to VELOCITY (cmd vel initialized from state: %f)", - info_.joints[i].name.c_str(), joint_command_velocity_[i]); - } else if (requested_modes[i] == integration_level_t::POSITION) { - joint_command_position_[i] = joint_state_position_[i]; - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), - "Joint %s: switched to POSITION (cmd pos initialized from state: %f)", - info_.joints[i].name.c_str(), joint_command_position_[i]); - } + joint.joint_command_position = joint.joint_state_position; } } return hardware_interface::return_type::OK; } -} // namespace talon_hardware_interface - -#include "pluginlib/class_list_macros.hpp" +} // namespace talon_ros2_control PLUGINLIB_EXPORT_CLASS( talon_ros2_control::TALONHardwareInterface, hardware_interface::SystemInterface) diff --git a/src/infrastructure/umdloop_can_library/src/SocketCanBus.cpp b/src/infrastructure/umdloop_can_library/src/SocketCanBus.cpp index 15755d6a..ad646120 100644 --- a/src/infrastructure/umdloop_can_library/src/SocketCanBus.cpp +++ b/src/infrastructure/umdloop_can_library/src/SocketCanBus.cpp @@ -84,7 +84,7 @@ bool SocketCanBus::open(const std::string& interface_name, ReceiveCallback callb } // Disable loopback (do not receive our own transmitted frames) - int loopback = 0; + int loopback = 1; setsockopt(socketFd_, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, sizeof(loopback)); // Disable CAN FD — we only use classical CAN frames diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index 83a12abd..9617b224 100644 --- a/src/msgs/CMakeLists.txt +++ b/src/msgs/CMakeLists.txt @@ -17,10 +17,12 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/CANA.msg" "msg/CANB.msg" "msg/Currentdraw.msg" - "msg/JointReceiver.msg" + "msg/JointStatus.msg" "msg/SystemInfo.msg" "msg/Heading.msg" "srv/SetController.srv" + "srv/StatusReq.srv" + "srv/MaintenanceReq.srv" "action/NavigateToGPS.action" DEPENDENCIES std_msgs geometry_msgs sensor_msgs builtin_interfaces action_msgs ) diff --git a/src/msgs/msg/JointReceiver.msg b/src/msgs/msg/JointReceiver.msg deleted file mode 100644 index 61f1efed..00000000 --- a/src/msgs/msg/JointReceiver.msg +++ /dev/null @@ -1,7 +0,0 @@ -string joint -int8 id -string motor_type -int8 temperature -float64 current -float64 motor_speed -float64 motor_angle \ No newline at end of file diff --git a/src/msgs/msg/JointStatus.msg b/src/msgs/msg/JointStatus.msg new file mode 100644 index 00000000..9bef7db0 --- /dev/null +++ b/src/msgs/msg/JointStatus.msg @@ -0,0 +1,4 @@ +string joint_name +int8 temperature +float64 torque_current +int8 motor_status \ No newline at end of file diff --git a/src/msgs/msg/SystemInfo.msg b/src/msgs/msg/SystemInfo.msg index efd29a61..706fb2c9 100644 --- a/src/msgs/msg/SystemInfo.msg +++ b/src/msgs/msg/SystemInfo.msg @@ -1 +1,2 @@ -JointReceiver[] joints \ No newline at end of file +std_msgs/Header header +JointStatus[] joints \ No newline at end of file diff --git a/src/msgs/srv/MaintenanceReq.srv b/src/msgs/srv/MaintenanceReq.srv new file mode 100644 index 00000000..9658f79c --- /dev/null +++ b/src/msgs/srv/MaintenanceReq.srv @@ -0,0 +1,15 @@ +# request_rate < 0: one-shot +# request_rate = 0: stop +# request_rate > 0: request rate in Hz + +string joint_name +int32 request_rate +uint8 command_id + +# payload container +int32[] i32_data +int16[] i16_data +uint8[] u8_data +--- +bool success +string message \ No newline at end of file diff --git a/src/msgs/srv/StatusReq.srv b/src/msgs/srv/StatusReq.srv new file mode 100644 index 00000000..9290d795 --- /dev/null +++ b/src/msgs/srv/StatusReq.srv @@ -0,0 +1,9 @@ +# request_rate < 0: one-shot +# request_rate = 0: stop +# request_rate > 0: request rate in Hz + +string joint_name +int32 request_rate +--- +bool success +string message \ No newline at end of file diff --git a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml index 8af0fdcc..aa4f8a92 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -29,8 +29,17 @@ controller_manager: manual_end_effector_gripper_claw_controller: type: arm_controllers/ManualEndEffectorGripperClawController - motor_status_broadcaster: - type: general_controllers/MotorStatusBroadcaster + two_dof_motor_status_controller: + type: general_controllers/MotorStatusController + + three_dof_motor_status_controller: + type: general_controllers/MotorStatusController + + cam_position_controller: + type: position_controllers/JointGroupPositionController + + rotary_encoder_state_request_controller: + type: forward_command_controller/ForwardCommandController twodof_joint_trajectory_controller: @@ -132,6 +141,21 @@ arm_velocity_controller: - actuator interface_name: velocity +cam_position_controller: + ros__parameters: + joints: + - cam_0 + - cam_1 + interface_name: position + +rotary_encoder_state_request_controller: + ros__parameters: + joints: + - base_yaw_encoder + - elbow_pitch_encoder + - shoulder_pitch_encoder + interface_name: state_request + manual_arm_cylindrical_controller: ros__parameters: joints: @@ -196,13 +220,151 @@ manual_end_effector_gripper_claw_controller: joint_max_positions: - 0.03 # Actuator max displacement -motor_status_broadcaster: +# Default command interfaces: +# - status_request +# - maintenance_request +# - maintenance_frame_high +# - maintenance_frame_low +# - maintenance_data_count + +# Default state interfaces: +# - motor_temperature +# - torque_current +# - status +two_dof_motor_status_controller: + ros__parameters: + joints: + - base_yaw + - shoulder_pitch + - elbow_pitch + - wrist_pitch + - wrist_roll + - actuator + - gripper_claw + - cam_0 + - cam_1 + gpios: + - limit_switch_0 + - limit_switch_1 + - limit_switch_2 + - limit_switch_3 + - limit_switch_4 + command_interfaces: + gripper_claw: + interfaces: + - status_request + limit_switch_0: + interfaces: + - status_request + limit_switch_1: + interfaces: + - status_request + limit_switch_2: + interfaces: + - status_request + limit_switch_3: + interfaces: + - status_request + limit_switch_4: + interfaces: + - status_request + state_interfaces: + actuator: + interfaces: + - status + cam_0: + interfaces: + - status + cam_1: + interfaces: + - status + limit_switch_0: + interfaces: + - status + limit_switch_1: + interfaces: + - status + limit_switch_2: + interfaces: + - status + limit_switch_3: + interfaces: + - status + limit_switch_4: + interfaces: + - status + publish_rate: 10.0 + +three_dof_motor_status_controller: ros__parameters: joints: - base_yaw - shoulder_pitch - elbow_pitch - interfaces: - - motor_temperature - - torque_current + - wrist_motor_a + - wrist_motor_b + - wrist_motor_c + - actuator + - gripper_claw + - cam_0 + - cam_1 + gpios: + - limit_switch_0 + - limit_switch_1 + - limit_switch_2 + - limit_switch_3 + - limit_switch_4 + command_interfaces: + gripper_claw: + interfaces: + - status_request + wrist_motor_a: + interfaces: + - status_request + wrist_motor_b: + interfaces: + - status_request + wrist_motor_c: + interfaces: + - status_request + limit_switch_0: + interfaces: + - status_request + limit_switch_1: + interfaces: + - status_request + limit_switch_2: + interfaces: + - status_request + limit_switch_3: + interfaces: + - status_request + limit_switch_4: + interfaces: + - status_request + state_interfaces: + actuator: + interfaces: + - status + cam_0: + interfaces: + - status + cam_1: + interfaces: + - status + limit_switch_0: + interfaces: + - status + limit_switch_1: + interfaces: + - status + limit_switch_2: + interfaces: + - status + limit_switch_3: + interfaces: + - status + limit_switch_4: + interfaces: + - status publish_rate: 10.0 diff --git a/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py b/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py index 2035791b..0fa388be 100644 --- a/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py +++ b/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py @@ -258,10 +258,18 @@ def launch_setup(context, *args, **kwargs): arguments=["joint_state_broadcaster"], ) - motor_status_broadcaster_spawner = Node( + motor_status_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["motor_status_broadcaster", "-c", "/controller_manager"], + arguments=[ + PythonExpression([ + '"three_dof_motor_status_controller" if "', + use_3dof, + '" == "true" else "two_dof_motor_status_controller"' + ]), + "-c", + "/controller_manager", + ], ) @@ -313,6 +321,18 @@ def launch_setup(context, *args, **kwargs): ) ] + active_robot_controller_names = [ + "rotary_encoder_state_request_controller", + "cam_position_controller", + ] + for controller in active_robot_controller_names: + robot_controller_spawners += [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "-c", "/controller_manager"], + ) + ] controller_switcher_node = RegisterEventHandler( event_handler=OnProcessExit( target_action=inactive_robot_controller_spawners[-1], @@ -341,10 +361,10 @@ def launch_setup(context, *args, **kwargs): ) ) - delay_motor_status_broadcaster_after_joint_state_broadcaster = RegisterEventHandler( + delay_motor_status_controller_after_joint_state_broadcaster = RegisterEventHandler( event_handler=OnProcessExit( target_action=joint_state_broadcaster_spawner, - on_exit=[motor_status_broadcaster_spawner], + on_exit=[motor_status_controller_spawner], ) ) @@ -389,7 +409,7 @@ def launch_setup(context, *args, **kwargs): control_node, robot_state_pub_node, delay_joint_state_broadcaster_spawner_after_ros2_control_node, - delay_motor_status_broadcaster_after_joint_state_broadcaster, + delay_motor_status_controller_after_joint_state_broadcaster, delay_rviz_after_joint_state_broadcaster_spawner, controller_switcher_node, ] + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner \ diff --git a/src/subsystems/arm/arm_bringup/package.xml b/src/subsystems/arm/arm_bringup/package.xml index 9075e4e9..bc813d56 100644 --- a/src/subsystems/arm/arm_bringup/package.xml +++ b/src/subsystems/arm/arm_bringup/package.xml @@ -26,6 +26,9 @@ controller_manager joint_state_broadcaster + forward_command_controller + position_controllers + velocity_controllers joint_state_publisher joint_state_publisher_gui launch_ros diff --git a/src/subsystems/drive/README.md b/src/subsystems/drive/README.md index 16359cc0..68b07b3a 100644 --- a/src/subsystems/drive/README.md +++ b/src/subsystems/drive/README.md @@ -76,4 +76,11 @@ ros2 launch drive_bringup athena_drive.launch.py mode:=base_station source install/setup.bash ros2 service call /set_controller athena_drive_msgs/srv/SetController "{controller_names: [INCLUDE CONTROLLER(S) YOU WANT WITHIN BRACKETS]}" ``` -Example: `ros2 service call /set_controller athena_drive_msgs/srv/SetController "{controller_names: [drive_velocity_controller]}"` \ No newline at end of file +Example: `ros2 service call /set_controller athena_drive_msgs/srv/SetController "{controller_names: [drive_velocity_controller]}"` + +ros2 topic pub --once /power_module_gpio_controller/commands control_msgs/msg/DynamicInterfaceGroupValues " +interface_groups: ['drive_power_module'] +interface_values: +- interface_names: ['kill_jetson'] + values: [1.0] +" \ No newline at end of file diff --git a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml index bf052fc8..27319c2c 100644 --- a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml +++ b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml @@ -42,15 +42,15 @@ controller_manager: # type: drive_controllers/SwerveDriveController # LED GPIO Controller - led_gpio_controller: + drive_led_gpio_controller: type: gpio_controllers/GpioCommandController - # Killswitch GPIO Controller - killswitch_gpio_controller: + # PowerModule GPIO Controller + power_module_gpio_controller: type: gpio_controllers/GpioCommandController - motor_status_broadcaster: - type: general_controllers/MotorStatusBroadcaster + motor_status_controller: + type: general_controllers/MotorStatusController rear_ackermann_controller: type: athena_drive_controllers/RearAckermannController @@ -72,13 +72,13 @@ ackermann_steering_controller: front_ackermann_controller: ros__parameters: drive_joints: - - "propulsion_fl_joint" - - "propulsion_fr_joint" - - "propulsion_bl_joint" - - "propulsion_br_joint" + - propulsion_fl_joint + - propulsion_fr_joint + - propulsion_bl_joint + - propulsion_br_joint steer_joints: - - "steer_fl_joint" - - "steer_fr_joint" + - steer_fl_joint + - steer_fr_joint wheelbase: 0.8382 track_width: 0.6604 wheel_radius: 0.125 @@ -112,62 +112,85 @@ drive_position_controller: interface_name: position # LED GPIO Controller Configuration -led_gpio_controller: +drive_led_gpio_controller: ros__parameters: gpios: - - status_led + - drive_led command_interfaces: - status_led: + drive_led: interfaces: - - led_command - state_interfaces: - status_led: - interfaces: - - led_state - - is_connected + - red + - green + - blue -# Killswitch GPIO Controller Configuration -killswitch_gpio_controller: +# PowerModule GPIO Controller Configuration +power_module_gpio_controller: ros__parameters: gpios: - - drive_killswitch + - drive_power_module command_interfaces: - drive_killswitch: + drive_power_module: interfaces: - kill_all - - kill_main - kill_jetson + - kill_main + - kill_by_voltage state_interfaces: - drive_killswitch: + drive_power_module: interfaces: - kill_all_sent - - kill_main_sent - kill_jetson_sent - - is_connected - -motor_status_broadcaster: + - kill_main_sent + - kill_by_voltage_sent + +# Default command interfaces: +# - status_request +# - maintenance_request +# - maintenance_frame_high +# - maintenance_frame_low +# - maintenance_data_count + +# Default state interfaces: +# - motor_temperature +# - torque_current +# - status +motor_status_controller: ros__parameters: joints: - propulsion_fl_joint - propulsion_fr_joint - propulsion_bl_joint - propulsion_br_joint - interfaces: - - motor_temperature - - torque_current + gpios: + - drive_led + - drive_power_module + command_interfaces: + drive_led: + interfaces: + - status_request + drive_power_module: + interfaces: + - status_request + state_interfaces: + drive_led: + interfaces: + - status + drive_power_module: + interfaces: + - status publish_rate: 10.0 rear_ackermann_controller: ros__parameters: steer_joints: - - "steer_bl_joint" - - "steer_br_joint" + - steer_bl_joint + - steer_br_joint drive_joints: - - "propulsion_fl_joint" - - "propulsion_fr_joint" - - "propulsion_bl_joint" - - "propulsion_br_joint" + - propulsion_fl_joint + - propulsion_fr_joint + - propulsion_bl_joint + - propulsion_br_joint wheelbase: 0.8382 track_width: 0.645 wheel_radius: 0.125 diff --git a/src/subsystems/drive/drive_bringup/config/joystick.yaml b/src/subsystems/drive/drive_bringup/config/joystick.yaml index 75451986..6b8adb93 100644 --- a/src/subsystems/drive/drive_bringup/config/joystick.yaml +++ b/src/subsystems/drive/drive_bringup/config/joystick.yaml @@ -1,4 +1,5 @@ /**: ros__parameters: - # 0: PS4 1: Thrustmaster - joystick_type: 1 \ No newline at end of file + # 0: PS4 1: Thrustmaster 2: Xbox One 3: 8BitDo Ultimate + joystick_type: 1 + joystick_index: -1 \ No newline at end of file diff --git a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py index a939508f..101af9d2 100644 --- a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py +++ b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py @@ -112,6 +112,13 @@ def generate_launch_description(): description="Robot controller to start.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "deactivate_odrive", + default_value="false", + description="Deactivate the ODrive joints in the URDF when using mock hardware to prevent excessive CAN flow.", + ) + ) return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) @@ -130,6 +137,7 @@ def launch_setup(context, *args, **kwargs): use_mock_hardware = LaunchConfiguration("use_mock_hardware") mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") robot_controller = LaunchConfiguration("robot_controller") + deactivate_odrive = LaunchConfiguration("deactivate_odrive") robot_description_path = PathJoinSubstitution( [FindPackageShare(description_package), "urdf", description_file] @@ -167,6 +175,8 @@ def launch_setup(context, *args, **kwargs): "mock_sensor_commands:=", mock_sensor_commands, " ", + "deactivate_odrive:=", + deactivate_odrive, ] ) @@ -229,10 +239,10 @@ def launch_setup(context, *args, **kwargs): arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) - motor_status_broadcaster_spawner = Node( + motor_status_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["motor_status_broadcaster", "-c", "/controller_manager"], + arguments=["motor_status_controller", "-c", "/controller_manager"], ) robot_controller_names = [robot_controller] @@ -246,7 +256,7 @@ def launch_setup(context, *args, **kwargs): ) ] - gpio_controller_names = ["led_gpio_controller", "killswitch_gpio_controller"] + gpio_controller_names = ["drive_led_gpio_controller", "power_module_gpio_controller"] gpio_controller_spawners = [] for controller in gpio_controller_names: gpio_controller_spawners += [ @@ -311,10 +321,10 @@ def launch_setup(context, *args, **kwargs): ) ) - delay_motor_status_broadcaster_after_joint_state_broadcaster = RegisterEventHandler( + delay_motor_status_controller_after_joint_state_broadcaster = RegisterEventHandler( event_handler=OnProcessExit( target_action=joint_state_broadcaster_spawner, - on_exit=[motor_status_broadcaster_spawner], + on_exit=[motor_status_controller_spawner], ) ) @@ -359,7 +369,7 @@ def launch_setup(context, *args, **kwargs): control_node, robot_state_pub_node, delay_joint_state_broadcaster_spawner_after_ros2_control_node, - delay_motor_status_broadcaster_after_joint_state_broadcaster, + delay_motor_status_controller_after_joint_state_broadcaster, delay_rviz_after_joint_state_broadcaster_spawner, controller_switcher_node, ] + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner \ diff --git a/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp b/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp index fb21c936..2af5cfda 100644 --- a/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp +++ b/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp @@ -24,6 +24,8 @@ #include "controller_interface/helpers.hpp" #include "rclcpp/rclcpp.hpp" +#define DEBUG_MODE 0 + namespace drive_controllers { RearAckermannController::RearAckermannController() : controller_interface::ControllerInterface() {} @@ -211,14 +213,16 @@ controller_interface::return_type RearAckermannController::update( command_interfaces_[5].set_value(rr_wheel_ang_vel); const double rad_s_to_rpm = 60.0 / (2.0 * M_PI); - RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 500, - "Wheel speeds [RPM] - FL: %.2f, FR: %.2f, RL: %.2f, RR: %.2f | Steer [rad] - RL: %.3f, RR: %.3f", - fl_wheel_ang_vel * rad_s_to_rpm, - fr_wheel_ang_vel * rad_s_to_rpm, - rl_wheel_ang_vel * rad_s_to_rpm, - rr_wheel_ang_vel * rad_s_to_rpm, - rear_left_angle_clamped, - rear_right_angle_clamped); + if (DEBUG_MODE == 1) { + RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 500, + "Wheel speeds [RPM] - FL: %.2f, FR: %.2f, RL: %.2f, RR: %.2f | Steer [rad] - RL: %.3f, RR: %.3f", + fl_wheel_ang_vel * rad_s_to_rpm, + fr_wheel_ang_vel * rad_s_to_rpm, + rl_wheel_ang_vel * rad_s_to_rpm, + rr_wheel_ang_vel * rad_s_to_rpm, + rear_left_angle_clamped, + rear_right_angle_clamped); + } return controller_interface::return_type::OK; } diff --git a/src/subsystems/general/README.md b/src/subsystems/general/README.md new file mode 100644 index 00000000..cafe6892 --- /dev/null +++ b/src/subsystems/general/README.md @@ -0,0 +1,61 @@ +ros2 service call /motor_status_controller/status_request msgs/srv/StatusReq "{joint_name: propulsion_fl_joint, request_rate: 0}" + +ros2 service call /motor_status_controller/status_request msgs/srv/StatusReq "{joint_name: stepper_motor_a, request_rate: -1}" + +ros2 service call /motor_status_controller/status_request msgs/srv/StatusReq "{joint_name: dc_auger, request_rate: -1}" + +ros2 service call /motor_status_controller/maintenance_request msgs/srv/MaintenanceReq "{ + joint_name: stepper_motor_a, + request_rate: 1, + command_id: 0x60, + i32_data: [], + i16_data: [], + u8_data: [1] +}" + +ros2 service call /motor_status_controller/maintenance_request msgs/srv/MaintenanceReq "{ + joint_name: dc_auger, + request_rate: 1, + command_id: 0x10, + i32_data: [], + i16_data: [], + u8_data: [2] +}" + +ros2 service call /motor_status_controller/maintenance_request msgs/srv/MaintenanceReq "{ + joint_name: propulsion_fl_joint, + request_rate: -1, + command_id: 0x81, + i32_data: [], + i16_data: [], + u8_data: [] +}" + +ros2 service call /motor_status_controller/maintenance_request msgs/srv/MaintenanceReq "{ + joint_name: wrist_roll, + request_rate: -1, + command_id: 0x40, + i32_data: [], + i16_data: [], + u8_data: [150] +}" + +ros2 topic pub --once /power_module_gpio_controller/commands control_msgs/msg/DynamicInterfaceGroupValues " +interface_groups: +- drive_power_module +interface_values: +- interface_names: + - kill_jetson + values: + - 1.0 +" + +ros2 topic pub --once /drive_led_gpio_controller/commands control_msgs/msg/DynamicInterfaceGroupValues " +interface_groups: +- drive_led +interface_values: +- interface_names: + - red + values: + - 255.0 +" diff --git a/src/subsystems/general/general_controllers/CMakeLists.txt b/src/subsystems/general/general_controllers/CMakeLists.txt index ae82a91a..80b9813f 100644 --- a/src/subsystems/general/general_controllers/CMakeLists.txt +++ b/src/subsystems/general/general_controllers/CMakeLists.txt @@ -7,11 +7,11 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface - diagnostic_msgs hardware_interface pluginlib rclcpp rclcpp_lifecycle + msgs ) find_package(ament_cmake REQUIRED) @@ -20,19 +20,19 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -generate_parameter_library(motor_status_broadcaster_parameters - config/motor_status_broadcaster.yaml +generate_parameter_library(motor_status_controller_parameters + config/motor_status_controller.yaml ) add_library( general_controllers SHARED - src/motor_status_broadcaster.cpp + src/motor_status_controller.cpp ) target_include_directories(general_controllers PUBLIC "$" "$") -target_link_libraries(general_controllers motor_status_broadcaster_parameters) +target_link_libraries(general_controllers motor_status_controller_parameters) ament_target_dependencies(general_controllers ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(general_controllers PRIVATE "GENERAL_CONTROLLERS_BUILDING_DLL") diff --git a/src/subsystems/general/general_controllers/config/motor_status_broadcaster.yaml b/src/subsystems/general/general_controllers/config/motor_status_broadcaster.yaml deleted file mode 100644 index 7e032709..00000000 --- a/src/subsystems/general/general_controllers/config/motor_status_broadcaster.yaml +++ /dev/null @@ -1,19 +0,0 @@ -motor_status_broadcaster: - joints: { - type: string_array, - default_value: [], - description: "Names of the joints to monitor for motor telemetry." - } - interfaces: { - type: string_array, - default_value: ["motor_temperature", "torque_current"], - description: "State interface names to read from each joint." - } - publish_rate: { - type: double, - default_value: 10.0, - description: "Rate (Hz) at which to publish diagnostics. Set to 0 to publish every update cycle.", - validation: { - gt_eq<>: [0.0] - } - } diff --git a/src/subsystems/general/general_controllers/config/motor_status_controller.yaml b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml new file mode 100644 index 00000000..bcb6f84f --- /dev/null +++ b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml @@ -0,0 +1,57 @@ +motor_status_controller: + joints: { + type: string_array, + default_value: [], + description: "Names of the joints to monitor for motor telemetry." + } + gpios: { + type: string_array, + default_value: [], + description: "Names of the GPIOs to monitor for status telemetry." + } + command_interfaces: + __map_joints: + interfaces: { + type: string_array, + default_value: ["status_request", "maintenance_request", "maintenance_frame_high", "maintenance_frame_low", "maintenance_data_count"], + description: "Command interface names to read from each joint. Leave empty if no command interfaces are needed.", + validation: { + unique<>: null + } + } + __map_gpios: + interfaces: { + type: string_array, + default_value: ["status_request"], + description: "Command interface names to read from each GPIO. Leave empty if no command interfaces are needed.", + validation: { + unique<>: null + } + } + state_interfaces: + __map_joints: + interfaces: { + type: string_array, + default_value: ["motor_temperature", "torque_current", "status"], + description: "State interface names to read from each joint.", + validation: { + unique<>: null + } + } + __map_gpios: + interfaces: { + type: string_array, + default_value: ["status"], + description: "State interface names to read from each GPIO.", + validation: { + unique<>: null + } + } + publish_rate: { + type: double, + default_value: 10.0, + description: "Rate (Hz) at which to publish diagnostics. Set to 0 to publish every update cycle.", + validation: { + gt_eq<>: [0.0] + } + } diff --git a/src/subsystems/general/general_controllers/general_controllers.xml b/src/subsystems/general/general_controllers/general_controllers.xml index 5a68e387..5808140a 100644 --- a/src/subsystems/general/general_controllers/general_controllers.xml +++ b/src/subsystems/general/general_controllers/general_controllers.xml @@ -1,9 +1,10 @@ - - Broadcasts motor telemetry (temperature and torque current) as diagnostics. + Main functions include broadcasting motor telemetry (temperature, torque current, status, brake status) and sending requests to obtain + information about a specific motor. diff --git a/src/subsystems/general/general_controllers/include/general_controllers/motor_status_broadcaster.hpp b/src/subsystems/general/general_controllers/include/general_controllers/motor_status_broadcaster.hpp deleted file mode 100644 index 169a0e96..00000000 --- a/src/subsystems/general/general_controllers/include/general_controllers/motor_status_broadcaster.hpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright (c) 2025, UMDLoop -// -// 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 GENERAL_CONTROLLERS__MOTOR_STATUS_BROADCASTER_HPP_ -#define GENERAL_CONTROLLERS__MOTOR_STATUS_BROADCASTER_HPP_ - -#include -#include -#include -#include - -#include "controller_interface/controller_interface.hpp" -#include "diagnostic_msgs/msg/diagnostic_array.hpp" -#include "general_controllers/visibility_control.h" -#include -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" - -namespace general_controllers -{ - -class MotorStatusBroadcaster : public controller_interface::ControllerInterface -{ -public: - GENERAL_CONTROLLERS__VISIBILITY_PUBLIC - MotorStatusBroadcaster(); - - GENERAL_CONTROLLERS__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_init() override; - - GENERAL_CONTROLLERS__VISIBILITY_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - GENERAL_CONTROLLERS__VISIBILITY_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - GENERAL_CONTROLLERS__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - GENERAL_CONTROLLERS__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - GENERAL_CONTROLLERS__VISIBILITY_PUBLIC - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - GENERAL_CONTROLLERS__VISIBILITY_PUBLIC - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - -protected: - std::shared_ptr param_listener_; - motor_status_broadcaster::Params params_; - - rclcpp::Publisher::SharedPtr diagnostics_publisher_; - - // Map from "joint/interface" to index in state_interfaces_ - std::unordered_map state_interface_map_; - - // Publish rate limiting - rclcpp::Duration publish_period_{0, 0}; - rclcpp::Time last_publish_time_{0, 0, RCL_CLOCK_UNINITIALIZED}; -}; - -} // namespace general_controllers - -#endif // GENERAL_CONTROLLERS__MOTOR_STATUS_BROADCASTER_HPP_ diff --git a/src/subsystems/general/general_controllers/include/general_controllers/motor_status_controller.hpp b/src/subsystems/general/general_controllers/include/general_controllers/motor_status_controller.hpp new file mode 100644 index 00000000..8f837869 --- /dev/null +++ b/src/subsystems/general/general_controllers/include/general_controllers/motor_status_controller.hpp @@ -0,0 +1,165 @@ +// Copyright (c) 2025, UMDLoop +// +// 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 GENERAL_CONTROLLERS__motor_status_controller_HPP_ +#define GENERAL_CONTROLLERS__motor_status_controller_HPP_ + +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "msgs/msg/joint_status.hpp" +#include "msgs/msg/system_info.hpp" +#include "msgs/srv/maintenance_req.hpp" +#include "msgs/srv/status_req.hpp" +#include "general_controllers/visibility_control.h" +#include +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace general_controllers +{ + +class MotorStatusController : public controller_interface::ControllerInterface +{ +public: + GENERAL_CONTROLLERS__VISIBILITY_PUBLIC + MotorStatusController(); + + GENERAL_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + GENERAL_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + GENERAL_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GENERAL_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + GENERAL_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + GENERAL_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + GENERAL_CONTROLLERS__VISIBILITY_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + GENERAL_CONTROLLERS__VISIBILITY_PUBLIC + void logger_function(); + +protected: + std::shared_ptr param_listener_; + motor_status_controller::Params params_; + + // Publisher for motor status information + rclcpp::Publisher::SharedPtr motor_status_publisher_; + + // Services for command interfaces + rclcpp::Service::SharedPtr status_request_service_; + rclcpp::Service::SharedPtr maintenance_request_service_; + std::string status_req_resource_name; + int status_request_rate; + std::string maintenance_req_joint_name; + int maintenance_request_rate; + double maintenance_frame_high; + double maintenance_frame_low; + double maintenance_data_count; + bool status_one_shot_sent; + bool maintenance_one_shot_sent; + + // Map from "joint/interface" to index in state_interfaces_ + std::unordered_map state_interface_map_; + + // Map from "joint/interface" to index in command_interfaces_ + std::unordered_map command_interface_map_; + + // Publish rate limiting + rclcpp::Duration publish_period_{0, 0}; + rclcpp::Time last_publish_time_{0, 0, RCL_CLOCK_UNINITIALIZED}; + rclcpp::Time status_one_shot_time{0, 0, RCL_CLOCK_UNINITIALIZED}; + rclcpp::Time maintenance_one_shot_time{0, 0, RCL_CLOCK_UNINITIALIZED}; + rclcpp::Duration one_shot_delay{0, 100000000}; + + std::vector available_state_interfaces; + std::vector available_command_interfaces; + + struct PackedCommand + { + int32_t counts; // 24 bits for count of each data type (u8, i16, i32) + int64_t payload; // 8 bits for command_id, followed by packed data (u8, i16, i32) + }; + + inline PackedCommand pack_command_full( + uint8_t command_id, + const std::vector& u8_data, + const std::vector& i16_data, + const std::vector& i32_data) + { + size_t total_bits = 8 + 8 * u8_data.size() + 16 * i16_data.size() + 32 * i32_data.size(); + if (total_bits > 64) { + RCLCPP_ERROR(rclcpp::get_logger("MotorStatusController"), "Payload exceeds 64 bits: total_bits=%zu", total_bits); + return PackedCommand{0, 0}; + } + + // PACK COUNTS (only 24 bits needed: 8 bits each for u8/i16/i32 counts) + uint32_t counts = 0; + int count_offset = 24; + auto push_count = [&](uint32_t value, int bits) { + count_offset -= bits; + counts |= (value & ((1U << bits) - 1)) << count_offset; + }; + push_count(static_cast(u8_data.size()), 8); + push_count(static_cast(i16_data.size()), 8); + push_count(static_cast(i32_data.size()), 8); + + // PACK PAYLOAD + uint64_t payload = 0; + int payload_offset = 64; + auto push_payload = [&](uint64_t value, int bits) { + payload_offset -= bits; + payload |= (value & ((1ULL << bits) - 1)) << payload_offset; + }; + push_payload(command_id, 8); + for (auto v : u8_data) push_payload(v, 8); + for (auto v : i16_data) push_payload(static_cast(v), 16); + for (auto v : i32_data) push_payload(static_cast(v), 32); + + return PackedCommand{ + static_cast(counts), + static_cast(payload) + }; + } + + enum class MotorStatus : uint8_t { + UNKNOWN = 0, + IDLE = 1, + ACTIVE = 2, + WARNING = 3, + ERROR = 4, + DISABLED = 5 + }; +}; + +} // namespace general_controllers + +#endif // GENERAL_CONTROLLERS__motor_status_controller_HPP_ diff --git a/src/subsystems/general/general_controllers/package.xml b/src/subsystems/general/general_controllers/package.xml index c7b9e418..af454c61 100644 --- a/src/subsystems/general/general_controllers/package.xml +++ b/src/subsystems/general/general_controllers/package.xml @@ -12,7 +12,7 @@ generate_parameter_library controller_interface - diagnostic_msgs + msgs hardware_interface pluginlib rclcpp diff --git a/src/subsystems/general/general_controllers/src/motor_status_broadcaster.cpp b/src/subsystems/general/general_controllers/src/motor_status_broadcaster.cpp deleted file mode 100644 index 5414087d..00000000 --- a/src/subsystems/general/general_controllers/src/motor_status_broadcaster.cpp +++ /dev/null @@ -1,189 +0,0 @@ -// Copyright (c) 2025, UMDLoop -// -// 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 "general_controllers/motor_status_broadcaster.hpp" - -#include -#include - -#include "pluginlib/class_list_macros.hpp" - -namespace general_controllers -{ - -MotorStatusBroadcaster::MotorStatusBroadcaster() {} - -controller_interface::CallbackReturn MotorStatusBroadcaster::on_init() -{ - try { - param_listener_ = std::make_shared(get_node()); - params_ = param_listener_->get_params(); - } catch (const std::exception & e) { - RCLCPP_ERROR( - get_node()->get_logger(), "Exception during on_init: %s", e.what()); - return controller_interface::CallbackReturn::ERROR; - } - - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::InterfaceConfiguration -MotorStatusBroadcaster::command_interface_configuration() const -{ - // Broadcaster only reads, no command interfaces - return controller_interface::InterfaceConfiguration{ - controller_interface::interface_configuration_type::NONE}; -} - -controller_interface::InterfaceConfiguration -MotorStatusBroadcaster::state_interface_configuration() const -{ - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - - for (const auto & joint : params_.joints) { - for (const auto & iface : params_.interfaces) { - config.names.push_back(joint + "/" + iface); - } - } - - return config; -} - -controller_interface::CallbackReturn MotorStatusBroadcaster::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - params_ = param_listener_->get_params(); - - if (params_.joints.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "No joints specified for motor_status_broadcaster."); - return controller_interface::CallbackReturn::ERROR; - } - - if (params_.interfaces.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "No interfaces specified for motor_status_broadcaster."); - return controller_interface::CallbackReturn::ERROR; - } - - // Set up publish rate - if (params_.publish_rate > 0.0) { - publish_period_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_rate); - } else { - publish_period_ = rclcpp::Duration(0, 0); - } - - diagnostics_publisher_ = get_node()->create_publisher( - "~/diagnostics", rclcpp::SystemDefaultsQoS()); - - RCLCPP_INFO( - get_node()->get_logger(), - "Configured motor_status_broadcaster for %zu joints, %zu interfaces, publish rate: %.1f Hz", - params_.joints.size(), params_.interfaces.size(), params_.publish_rate); - - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn MotorStatusBroadcaster::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // Build lookup map from "joint/interface" -> state_interfaces_ index - state_interface_map_.clear(); - for (size_t i = 0; i < state_interfaces_.size(); ++i) { - state_interface_map_[state_interfaces_[i].get_prefix_name() + "/" + - state_interfaces_[i].get_interface_name()] = i; - } - - last_publish_time_ = get_node()->now(); - - RCLCPP_INFO( - get_node()->get_logger(), - "MotorStatusBroadcaster activated with %zu state interfaces", - state_interfaces_.size()); - - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn MotorStatusBroadcaster::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - state_interface_map_.clear(); - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::return_type MotorStatusBroadcaster::update( - const rclcpp::Time & time, const rclcpp::Duration & /*period*/) -{ - // Rate limit publishing - if (publish_period_.seconds() > 0.0 && (time - last_publish_time_) < publish_period_) { - return controller_interface::return_type::OK; - } - last_publish_time_ = time; - - diagnostic_msgs::msg::DiagnosticArray diag_msg; - diag_msg.header.stamp = time; - - for (const auto & joint : params_.joints) { - diagnostic_msgs::msg::DiagnosticStatus status; - status.name = "Motor: " + joint; - status.hardware_id = joint; - status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - status.message = "OK"; - - for (const auto & iface : params_.interfaces) { - std::string key = joint + "/" + iface; - auto it = state_interface_map_.find(key); - - diagnostic_msgs::msg::KeyValue kv; - kv.key = iface; - - if (it != state_interface_map_.end()) { - double value = state_interfaces_[it->second].get_value(); - - // Format value with 2 decimal places - std::ostringstream oss; - oss.precision(2); - oss << std::fixed << value; - kv.value = oss.str(); - - // Flag high temperature as warning (>70C) or error (>90C) - if (iface == "motor_temperature") { - if (value > 90.0) { - status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - status.message = "OVERHEATING"; - } else if (value > 70.0 && - status.level < diagnostic_msgs::msg::DiagnosticStatus::WARN) { - status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - status.message = "High temperature"; - } - } - } else { - kv.value = "N/A"; - } - - status.values.push_back(kv); - } - - diag_msg.status.push_back(status); - } - - diagnostics_publisher_->publish(diag_msg); - - return controller_interface::return_type::OK; -} - -} // namespace general_controllers - -PLUGINLIB_EXPORT_CLASS( - general_controllers::MotorStatusBroadcaster, - controller_interface::ControllerInterface) diff --git a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp new file mode 100644 index 00000000..632153f2 --- /dev/null +++ b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp @@ -0,0 +1,449 @@ +// Copyright (c) 2025, UMDLoop +// +// 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 "general_controllers/motor_status_controller.hpp" + +#include +#include +#include +#include + +#include "pluginlib/class_list_macros.hpp" + +#define DEBUG_MODE 0 + +namespace general_controllers +{ + +MotorStatusController::MotorStatusController() {} + +void MotorStatusController::logger_function() +{ + std::string log_msg = "\033[2J\033[H \nMotor Status Controller Logger"; + + // HWI Specific + std::ostringstream oss; + + // for(const auto & joint : params_.joints) { + // if (available_state_interfaces.empty()) { + // RCLCPP_INFO( + // get_node()->get_logger(), + // "Joint '%s' has no motor status state interfaces available.", + // joint.c_str()); + // continue; + // } + + // std::ostringstream interfaces_stream; + // for (size_t i = 0; i < available_state_interfaces.size(); ++i) { + // if (i > 0) { + // interfaces_stream << ", "; + // } + // interfaces_stream << available_state_interfaces[i]; + // } + + // RCLCPP_INFO( + // get_node()->get_logger(), + // "Joint '%s' will publish available motor status interfaces: %s", + // joint.c_str(), + // interfaces_stream.str().c_str()); + + // if (available_command_interfaces.empty()) { + // RCLCPP_INFO( + // get_node()->get_logger(), + // "Joint '%s' has no motor status command interfaces available.", + // joint.c_str()); + // continue; + // } + + // interfaces_stream.str(""); + // for (size_t i = 0; i < available_command_interfaces.size(); ++i) { + // if (i > 0) { + // interfaces_stream << ", "; + // } + // interfaces_stream << available_command_interfaces[i]; + // } + + // RCLCPP_INFO( + // get_node()->get_logger(), + // "Joint '%s' will use available motor status command interfaces: %s", + // joint.c_str(), + // interfaces_stream.str().c_str()); + // } + + log_msg += oss.str(); + RCLCPP_INFO(get_node()->get_logger(), log_msg.c_str()); +} + +controller_interface::CallbackReturn MotorStatusController::on_init() +{ + try { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } catch (const std::exception & e) { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception during on_init: %s", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + status_req_resource_name = ""; + status_request_rate = 0; + maintenance_req_joint_name = ""; + maintenance_request_rate = 0; + status_one_shot_sent = false; + maintenance_one_shot_sent = false; + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +MotorStatusController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & [joint_name, joint_cfg] : params_.command_interfaces.joints_map){ + for(const auto & interface_name : joint_cfg.interfaces){ + command_interfaces_config.names.push_back(joint_name + "/" + interface_name); + } + } + for (const auto & [gpio_name, gpio_cfg] : params_.command_interfaces.gpios_map){ + for(const auto & interface_name : gpio_cfg.interfaces){ + command_interfaces_config.names.push_back(gpio_name + "/" + interface_name); + } + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration +MotorStatusController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & [joint_name, joint_cfg] : params_.state_interfaces.joints_map){ + for(const auto & interface_name : joint_cfg.interfaces){ + state_interfaces_config.names.push_back(joint_name + "/" + interface_name); + } + } + for (const auto & [gpio_name, gpio_cfg] : params_.state_interfaces.gpios_map){ + for(const auto & interface_name : gpio_cfg.interfaces){ + state_interfaces_config.names.push_back(gpio_name + "/" + interface_name); + } + } + + return state_interfaces_config; +} + +controller_interface::CallbackReturn MotorStatusController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + if (params_.joints.empty() && params_.gpios.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "No joints or gpios specified for motor_status_controller."); + return controller_interface::CallbackReturn::ERROR; + } + + // if (params_.state_interfaces.joints_map.empty()) { + // RCLCPP_ERROR(get_node()->get_logger(), "No interfaces specified for motor_status_controller."); + // return controller_interface::CallbackReturn::ERROR; + // } + + // Set up publish rate + if (params_.publish_rate > 0.0) { + publish_period_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_rate); + } else { + publish_period_ = rclcpp::Duration(0, 0); + } + + motor_status_publisher_ = get_node()->create_publisher( + "~/motor_status", rclcpp::SystemDefaultsQoS()); + + status_request_service_ = get_node()->create_service( + "~/status_request", + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + if (request->joint_name.empty()) { + response->success = false; + response->message = "Joint/GPIO name cannot be empty"; + RCLCPP_WARN(get_node()->get_logger(), "%s", response->message.c_str()); + return; + } + + status_req_resource_name = request->joint_name; + status_request_rate = request->request_rate; + + std::string msg = + "Received status_request for resource " + status_req_resource_name + + " at request_rate: " + std::to_string(status_request_rate); + response->success = true; + response->message = msg; + RCLCPP_INFO(get_node()->get_logger(), "%s", msg.c_str()); + }); + + maintenance_request_service_ = get_node()->create_service( + "~/maintenance_request", + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + if (request->joint_name.empty()) { + response->success = false; + response->message = "Joint name cannot be empty"; + RCLCPP_WARN(get_node()->get_logger(), "%s", response->message.c_str()); + return; + } + + maintenance_req_joint_name = request->joint_name; + maintenance_request_rate = request->request_rate; + + auto packed = pack_command_full( + request->command_id, + request->u8_data, + request->i16_data, + request->i32_data); + + auto payload_to_doubles = [](int64_t value) -> std::pair + { + uint64_t u = static_cast(value); + double high = static_cast(u >> 32); + double low = static_cast(u & 0xFFFFFFFF); + return {high, low}; + }; + + auto [high, low] = payload_to_doubles(packed.payload); + + maintenance_frame_high = high; + maintenance_frame_low = low; + maintenance_data_count = static_cast(packed.counts); + + std::ostringstream oss; + + oss << "Received maintenance_request for joint " + << maintenance_req_joint_name + << " with frame: 0x" + << std::hex << std::uppercase + << std::setfill('0') << std::setw(8) << static_cast(maintenance_frame_high) + << std::setfill('0') << std::setw(8) << static_cast(maintenance_frame_low) + << std::dec << std::nouppercase + << " and data count(s): " + << maintenance_data_count + << " at request_rate: " + << maintenance_request_rate; + + std::string msg = oss.str(); + response->success = true; + response->message = msg; + RCLCPP_INFO(get_node()->get_logger(), "%s", msg.c_str()); + }); + + RCLCPP_INFO( + get_node()->get_logger(), + "Configured motor_status_controller for %zu joints and %zu gpios at publish rate: %.1f Hz", + params_.joints.size(), params_.gpios.size(), params_.publish_rate); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MotorStatusController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Build lookup map from "joint/interface" -> state_interfaces_ index + state_interface_map_.clear(); + for (size_t i = 0; i < state_interfaces_.size(); ++i) { + state_interface_map_[state_interfaces_[i].get_prefix_name() + "/" + + state_interfaces_[i].get_interface_name()] = i; + } + + // for (const auto & joint : params_.joints) { + // for (const auto & iface : params_.state_interfaces) { + // const std::string key = joint + "/" + iface; + // if (state_interface_map_.find(key) != state_interface_map_.end()) { + // available_state_interfaces.push_back(iface); + // } + // } + // } + + // Build lookup map from "joint/interface" -> command_interfaces_ index + command_interface_map_.clear(); + for (size_t i = 0; i < command_interfaces_.size(); ++i) { + command_interface_map_[command_interfaces_[i].get_prefix_name() + "/" + + command_interfaces_[i].get_interface_name()] = i; + } + + // for (const auto & joint : params_.joints) { + // for (const auto & iface : params_.command_interfaces) { + // const std::string key = joint + "/" + iface; + // if (command_interface_map_.find(key) != command_interface_map_.end()) { + // available_command_interfaces.push_back(iface); + // } + // } + // } + + last_publish_time_ = get_node()->now(); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MotorStatusController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + state_interface_map_.clear(); + command_interface_map_.clear(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type MotorStatusController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + // Rate limit publishing + const bool periodic_publish_due = + (publish_period_.seconds() <= 0.0 || (time - last_publish_time_) >= publish_period_); + + if (!periodic_publish_due) { + return controller_interface::return_type::OK; + } + last_publish_time_ = time; + + msgs::msg::SystemInfo system_info_msg; + system_info_msg.header.stamp = time; + + // Command Interfaces + auto update_command_interfaces = + [this, &time](const auto & interface_map, const bool allow_maintenance) + { + for (const auto & [resource_name, resource_cfg] : interface_map) { + for (const auto & interface_name : resource_cfg.interfaces) { + std::string key = resource_name + "/" + interface_name; + auto it = command_interface_map_.find(key); + if (it == command_interface_map_.end()) { + continue; + } + + if (allow_maintenance && interface_name == "maintenance_frame_high" && + resource_name == maintenance_req_joint_name) + { + command_interfaces_[it->second].set_value(maintenance_frame_high); + } + + if (allow_maintenance && interface_name == "maintenance_frame_low" && + resource_name == maintenance_req_joint_name) + { + command_interfaces_[it->second].set_value(maintenance_frame_low); + } + + if (allow_maintenance && interface_name == "maintenance_data_count" && + resource_name == maintenance_req_joint_name) + { + command_interfaces_[it->second].set_value(maintenance_data_count); + } + + if (interface_name == "status_request" && resource_name == status_req_resource_name) { + if (status_request_rate < 0 && status_one_shot_sent == false) { + command_interfaces_[it->second].set_value(status_request_rate); + RCLCPP_WARN(get_node()->get_logger(), "Status one shot sent: "); + status_one_shot_sent = true; + status_one_shot_time = time; + } + else if (status_request_rate < 0 && status_one_shot_sent == true && + (time - status_one_shot_time) >= one_shot_delay) + { + status_request_rate = 0; + command_interfaces_[it->second].set_value(status_request_rate); + status_one_shot_sent = false; + RCLCPP_WARN(get_node()->get_logger(), "Status one shot reset."); + } + else if (status_request_rate >= 0) { + command_interfaces_[it->second].set_value(status_request_rate); + } + } + + if (allow_maintenance && interface_name == "maintenance_request" && + resource_name == maintenance_req_joint_name) + { + if (maintenance_request_rate < 0 && maintenance_one_shot_sent == false) { + command_interfaces_[it->second].set_value(maintenance_request_rate); + RCLCPP_WARN(get_node()->get_logger(), "Maintenance one shot sent: "); + maintenance_one_shot_sent = true; + maintenance_one_shot_time = time; + } + else if (maintenance_request_rate < 0 && maintenance_one_shot_sent == true && + (time - maintenance_one_shot_time) >= one_shot_delay) + { + maintenance_request_rate = 0; + command_interfaces_[it->second].set_value(maintenance_request_rate); + maintenance_one_shot_sent = false; + RCLCPP_WARN(get_node()->get_logger(), "Maintenance one shot reset."); + } + else if (maintenance_request_rate >= 0) { + command_interfaces_[it->second].set_value(maintenance_request_rate); + } + } + } + } + }; + + update_command_interfaces(params_.command_interfaces.joints_map, true); + update_command_interfaces(params_.command_interfaces.gpios_map, false); + + auto append_state_status = + [this, &system_info_msg](const auto & interface_map) + { + for (const auto & [resource_name, resource_cfg] : interface_map) { + msgs::msg::JointStatus status; + status.joint_name = resource_name; + status.temperature = std::numeric_limits::quiet_NaN(); + status.torque_current = std::numeric_limits::quiet_NaN(); + status.motor_status = std::numeric_limits::quiet_NaN(); + + for (const auto & interface_name : resource_cfg.interfaces) { + std::string key = resource_name + "/" + interface_name; + auto it = state_interface_map_.find(key); + + if (it == state_interface_map_.end()) { + continue; + } + + double value = state_interfaces_[it->second].get_value(); + + if (interface_name == "motor_temperature") { + status.temperature = static_cast(value); + } else if (interface_name == "torque_current") { + status.torque_current = value; + } else if (interface_name == "status") { + status.motor_status = static_cast(value); + if (value > sizeof(MotorStatus)) { + RCLCPP_WARN(get_node()->get_logger(), "Invalid motor status value"); + } + } + } + + system_info_msg.joints.push_back(status); + } + }; + + append_state_status(params_.state_interfaces.joints_map); + append_state_status(params_.state_interfaces.gpios_map); + + motor_status_publisher_->publish(system_info_msg); + + return controller_interface::return_type::OK; +} + +} // namespace general_controllers + +PLUGINLIB_EXPORT_CLASS( + general_controllers::MotorStatusController, + controller_interface::ControllerInterface) 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..a75ca5d5 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -15,12 +15,14 @@ controller_manager: science_controller: type: science_controllers/ScienceManual - # Laser GPIO Controller (CAN-based) laser_gpio_controller: type: gpio_controllers/GpioCommandController - motor_status_broadcaster: - type: general_controllers/MotorStatusBroadcaster + fluoro_led_gpio_controller: + type: gpio_controllers/GpioCommandController + + motor_status_controller: + type: general_controllers/MotorStatusController joint_group_position_controller: ros__parameters: @@ -31,10 +33,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 +49,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 +68,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] @@ -93,14 +95,106 @@ laser_gpio_controller: - temperature - is_connected -motor_status_broadcaster: +fluoro_led_gpio_controller: + ros__parameters: + gpios: + - fluoro_led + command_interfaces: + fluoro_led: + interfaces: + - intensity + +# Default command interfaces: +# - status_request +# - maintenance_request +# - maintenance_frame_high +# - maintenance_frame_low +# - maintenance_data_count + +# Default state interfaces: +# - motor_temperature +# - torque_current +# - status +motor_status_controller: ros__parameters: joints: - stepper_motor_a - stepper_motor_b - - talon_lift - - talon_scoop - interfaces: - - motor_temperature - - torque_current - publish_rate: 10.0 \ No newline at end of file + - servo_rack_and_pinion_l + - servo_rack_and_pinion_r + - servo_scoop_a + - servo_scoop_b + - dc_scoop + - servo_sampler_lift_l + - servo_sampler_lift_r + - dc_auger + - servo_auger_lift + gpios: + - spectrometry_laser + - fluoro_led + - science_limit_switch + command_interfaces: + spectrometry_laser: + interfaces: + - status_request + fluoro_led: + interfaces: + - status_request + science_limit_switch: + interfaces: + - status_request + dc_scoop: + interfaces: + - status_request + - maintenance_request + - maintenance_frame_high + - maintenance_frame_low + - maintenance_data_count + dc_auger: + interfaces: + - status_request + - maintenance_request + - maintenance_frame_high + - maintenance_frame_low + - maintenance_data_count + state_interfaces: + stepper_motor_a: + interfaces: + - status + stepper_motor_b: + interfaces: + - status + servo_rack_and_pinion_l: + interfaces: + - status + servo_rack_and_pinion_r: + interfaces: + - status + servo_scoop_a: + interfaces: + - status + servo_scoop_b: + interfaces: + - status + dc_scoop: + interfaces: + - status + servo_sampler_lift_l: + interfaces: + - status + servo_sampler_lift_r: + interfaces: + - status + dc_auger: + interfaces: + - status + servo_auger_lift: + interfaces: + - status + fluoro_led: + interfaces: + - status + science_limit_switch: + interfaces: + - status + publish_rate: 10.0 diff --git a/src/subsystems/science/science_bringup/config/joystick.yaml b/src/subsystems/science/science_bringup/config/joystick.yaml index df9a1073..02e9ea17 100644 --- a/src/subsystems/science/science_bringup/config/joystick.yaml +++ b/src/subsystems/science/science_bringup/config/joystick.yaml @@ -1,4 +1,5 @@ /**: ros__parameters: # 0: PS4 1: Thrustmaster - joystick_type: 0 + joystick_type: 2 + joystick_index: -1 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 1d968135..66fe1d28 100644 --- a/src/subsystems/science/science_bringup/launch/athena_science.launch.py +++ b/src/subsystems/science/science_bringup/launch/athena_science.launch.py @@ -190,10 +190,10 @@ def generate_launch_description(): arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) - motor_status_broadcaster_spawner = Node( + motor_status_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["motor_status_broadcaster", "-c", "/controller_manager"], + arguments=["motor_status_controller", "-c", "/controller_manager"], ) # CONTROLLER MANAGERS @@ -223,7 +223,7 @@ def generate_launch_description(): ] # GPIO controller spawner for Laser - gpio_controller_names = ["laser_gpio_controller"] + gpio_controller_names = ["laser_gpio_controller", "fluoro_led_gpio_controller"] gpio_controller_spawners = [] for controller in gpio_controller_names: gpio_controller_spawners += [ @@ -278,11 +278,11 @@ def generate_launch_description(): ) ) - # Delay motor_status_broadcaster (inactive) after joint_state_broadcaster - delay_motor_status_broadcaster_after_joint_state_broadcaster = RegisterEventHandler( + # Delay motor_status_controller (inactive) after joint_state_broadcaster + delay_motor_status_controller_after_joint_state_broadcaster = RegisterEventHandler( event_handler=OnProcessExit( target_action=joint_state_broadcaster_spawner, - on_exit=[motor_status_broadcaster_spawner], + on_exit=[motor_status_controller_spawner], ) ) @@ -346,7 +346,7 @@ def generate_launch_description(): robot_state_pub_node, rviz_node, delay_joint_state_broadcaster_spawner_after_ros2_control_node, - delay_motor_status_broadcaster_after_joint_state_broadcaster, + delay_motor_status_controller_after_joint_state_broadcaster, # umdloop_can_node, controller_switcher_node, joystick_publisher, diff --git a/src/subsystems/science/science_bringup/package.xml b/src/subsystems/science/science_bringup/package.xml index 89dc983b..6e5c4815 100644 --- a/src/subsystems/science/science_bringup/package.xml +++ b/src/subsystems/science/science_bringup/package.xml @@ -22,6 +22,7 @@ controller_manager_msgs forward_command_controller + gpio_controllers joint_state_broadcaster general_controllers 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