From 9b5f18daacab79c7220c5aa65d9c4c29d6807515 Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Sun, 19 Apr 2026 13:41:32 -0400 Subject: [PATCH 01/37] saving changes --- .../arm/arm.servo.ros2_control.xacro | 6 ++++- .../science/science.servo.ros2_control.xacro | 24 +++++++++++++++++++ .../src/rmd_hardware_interface.cpp | 7 +++--- .../servo_hardware_interface.hpp | 16 +++++++++---- .../src/servo_hardware_interface.cpp | 19 +++++++++------ .../src/smc_hardware_interface.cpp | 8 +++---- src/msgs/CMakeLists.txt | 2 +- .../{JointReceiver.msg => JointStatus.msg} | 0 src/msgs/msg/SystemInfo.msg | 2 +- .../config/athena_arm_controllers.yaml | 4 ++-- .../arm_bringup/launch/athena_arm.launch.py | 10 ++++---- .../config/athena_drive_controllers.yaml | 4 ++-- .../launch/athena_drive.launch.py | 10 ++++---- .../general_controllers/CMakeLists.txt | 8 +++---- ...ster.yaml => motor_status_controller.yaml} | 4 ++-- ...caster.hpp => motor_status_controller.hpp} | 12 +++++----- ...caster.cpp => motor_status_controller.cpp} | 10 ++++---- .../config/athena_science_controllers.yaml | 4 ++-- .../launch/athena_science.launch.py | 12 +++++----- 19 files changed, 99 insertions(+), 63 deletions(-) rename src/msgs/msg/{JointReceiver.msg => JointStatus.msg} (100%) rename src/subsystems/general/general_controllers/config/{motor_status_broadcaster.yaml => motor_status_controller.yaml} (80%) rename src/subsystems/general/general_controllers/include/general_controllers/{motor_status_broadcaster.hpp => motor_status_controller.hpp} (87%) rename src/subsystems/general/general_controllers/src/{motor_status_broadcaster.cpp => motor_status_controller.cpp} (94%) 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..4dce02ad 100644 --- a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro @@ -20,7 +20,11 @@ prismatic - + + + + + 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..e3ae23e9 100644 --- a/src/description/ros2_control/science/science.servo.ros2_control.xacro +++ b/src/description/ros2_control/science/science.servo.ros2_control.xacro @@ -20,8 +20,11 @@ revolute + + + @@ -33,8 +36,11 @@ revolute + + + @@ -45,7 +51,11 @@ revolute + + + + @@ -56,7 +66,11 @@ revolute + + + + @@ -68,8 +82,11 @@ revolute + + + @@ -81,8 +98,11 @@ revolute + + + @@ -94,7 +114,11 @@ revolute + + + + 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..b81a4f7a 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 @@ -333,15 +333,14 @@ 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_update_time+=period.seconds(); + double update_period = 1.0/update_rate; + elapsed_time+=period.seconds(); elapsed_logger_time+=period.seconds(); double logging_period = 1.0/logger_rate; if(elapsed_logger_time > logging_period){ 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..d1fd714e 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 @@ -162,15 +162,21 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh // Type of joint for each actuator std::vector joint_type_; - // CAN Commands + // Multiplexor values 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 uint8_t MOTOR_STATE_CMD = 0x40; + static constexpr uint8_t MOTOR_STATUS_CMD = 0x50; + static constexpr uint8_t MAINTENANCE_CMD = 0x60; + static constexpr uint8_t SERVO_SPECS_CMD = 0x70; + + // Multiplexor Nibbles + int8_t motor_state_nibble; + int8_t motor_status_nibble; + int8_t maintenance_nibble; + int8_t servo_specs_nibble; }; } // namespace servo_hardware_interface 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..1b2a6a38 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 @@ -217,6 +217,11 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( } } + // Command Nibble of Multiplexor ID + motor_state_nibble = (MOTOR_STATE_CMD >> 4) & 0x0F; + motor_status_nibble = (MOTOR_STATUS_CMD >> 4) & 0x0F; + maintenance_nibble = (MAINTENANCE_CMD >> 4) & 0x0F; + return hardware_interface::CallbackReturn::SUCCESS; } @@ -286,7 +291,7 @@ void SERVOHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { 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]){ + if(can_rx_frame_.id == can_response_id && command_nibble == motor_state_nibble && device_id_nibble == joint_node_ids[i]){ // DECODING CAN MESSAGE FOR VELOCITY data[1] = can_rx_frame_.data[1]; // Position low byte @@ -315,7 +320,7 @@ void SERVOHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); } } - else if(can_rx_frame_.id == can_response_id && command_nibble == MOTOR_STATUS_CMD && device_id_nibble == joint_node_ids[i]){ + else if(can_rx_frame_.id == can_response_id && command_nibble == motor_status_nibble && device_id_nibble == joint_node_ids[i]){ // Populate device status device_status[i] = can_rx_frame_.data[1]; @@ -335,7 +340,7 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_cleanup( 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 command_nibble = maintenance_nibble; int8_t maintenance_command = 2; // Motor Shutdown Command int8_t device_id_nibble; for(int i = 0; i < num_joints; i++){ @@ -378,7 +383,7 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Deactivating ...please wait..."); - int8_t command_nibble = MAINTENANCE_CMD; + int8_t command_nibble = maintenance_nibble; int8_t maintenance_command = 1; // Motor Stop Command int8_t device_id_nibble; @@ -429,13 +434,13 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ { elapsed_update_time+=period.seconds(); double update_period = 1.0/update_rate; + elapsed_time+=period.seconds(); + 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(); @@ -534,7 +539,7 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ } else { - command_nibble = MOTOR_STATE_CMD; // Motor State Command for each joint at given frequency + command_nibble = motor_state_nibble; // 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) }; 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..aa010d20 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 @@ -343,16 +343,14 @@ hardware_interface::return_type SMCHardwareInterface::read( hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::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; - int16_t operating_velocity = 200; int32_t joint_velocity = 0; - elapsed_time+=period.seconds(); - // Logger update + elapsed_update_time+=period.seconds(); + double update_period = 1.0/update_rate; + elapsed_time+=period.seconds(); elapsed_logger_time+=period.seconds(); double logging_period = 1.0/logger_rate; if(elapsed_logger_time > logging_period){ diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index 83a12abd..dd0de4aa 100644 --- a/src/msgs/CMakeLists.txt +++ b/src/msgs/CMakeLists.txt @@ -17,7 +17,7 @@ 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" diff --git a/src/msgs/msg/JointReceiver.msg b/src/msgs/msg/JointStatus.msg similarity index 100% rename from src/msgs/msg/JointReceiver.msg rename to src/msgs/msg/JointStatus.msg diff --git a/src/msgs/msg/SystemInfo.msg b/src/msgs/msg/SystemInfo.msg index efd29a61..7ba1d41a 100644 --- a/src/msgs/msg/SystemInfo.msg +++ b/src/msgs/msg/SystemInfo.msg @@ -1 +1 @@ -JointReceiver[] joints \ No newline at end of file +JointStatus[] joints \ 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..94e7b9a1 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -29,7 +29,7 @@ controller_manager: manual_end_effector_gripper_claw_controller: type: arm_controllers/ManualEndEffectorGripperClawController - motor_status_broadcaster: + motor_status_controller: type: general_controllers/MotorStatusBroadcaster @@ -196,7 +196,7 @@ manual_end_effector_gripper_claw_controller: joint_max_positions: - 0.03 # Actuator max displacement -motor_status_broadcaster: +motor_status_controller: ros__parameters: joints: - base_yaw 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..b7e6c888 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,10 @@ 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=["motor_status_controller", "-c", "/controller_manager"], ) @@ -341,10 +341,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 +389,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_bringup/config/athena_drive_controllers.yaml b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml index bf052fc8..a5df877a 100644 --- a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml +++ b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml @@ -49,7 +49,7 @@ controller_manager: killswitch_gpio_controller: type: gpio_controllers/GpioCommandController - motor_status_broadcaster: + motor_status_controller: type: general_controllers/MotorStatusBroadcaster rear_ackermann_controller: @@ -145,7 +145,7 @@ killswitch_gpio_controller: - kill_jetson_sent - is_connected -motor_status_broadcaster: +motor_status_controller: ros__parameters: joints: - propulsion_fl_joint 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..122e1025 100644 --- a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py +++ b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py @@ -229,10 +229,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] @@ -311,10 +311,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 +359,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/general/general_controllers/CMakeLists.txt b/src/subsystems/general/general_controllers/CMakeLists.txt index ae82a91a..36c83bfc 100644 --- a/src/subsystems/general/general_controllers/CMakeLists.txt +++ b/src/subsystems/general/general_controllers/CMakeLists.txt @@ -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_controller.yaml similarity index 80% rename from src/subsystems/general/general_controllers/config/motor_status_broadcaster.yaml rename to src/subsystems/general/general_controllers/config/motor_status_controller.yaml index 7e032709..4483730f 100644 --- a/src/subsystems/general/general_controllers/config/motor_status_broadcaster.yaml +++ b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml @@ -1,4 +1,4 @@ -motor_status_broadcaster: +motor_status_controller: joints: { type: string_array, default_value: [], @@ -6,7 +6,7 @@ motor_status_broadcaster: } interfaces: { type: string_array, - default_value: ["motor_temperature", "torque_current"], + default_value: ["motor_temperature", "torque_current", "status", "brake_status"], description: "State interface names to read from each joint." } publish_rate: { 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_controller.hpp similarity index 87% rename from src/subsystems/general/general_controllers/include/general_controllers/motor_status_broadcaster.hpp rename to src/subsystems/general/general_controllers/include/general_controllers/motor_status_controller.hpp index 169a0e96..e1092e0d 100644 --- a/src/subsystems/general/general_controllers/include/general_controllers/motor_status_broadcaster.hpp +++ b/src/subsystems/general/general_controllers/include/general_controllers/motor_status_controller.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef GENERAL_CONTROLLERS__motor_status_controller_HPP_ +#define GENERAL_CONTROLLERS__motor_status_controller_HPP_ #include #include @@ -23,7 +23,7 @@ #include "controller_interface/controller_interface.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "general_controllers/visibility_control.h" -#include +#include #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -62,8 +62,8 @@ class MotorStatusBroadcaster : public controller_interface::ControllerInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: - std::shared_ptr param_listener_; - motor_status_broadcaster::Params params_; + std::shared_ptr param_listener_; + motor_status_controller::Params params_; rclcpp::Publisher::SharedPtr diagnostics_publisher_; @@ -77,4 +77,4 @@ class MotorStatusBroadcaster : public controller_interface::ControllerInterface } // namespace general_controllers -#endif // GENERAL_CONTROLLERS__MOTOR_STATUS_BROADCASTER_HPP_ +#endif // GENERAL_CONTROLLERS__motor_status_controller_HPP_ diff --git a/src/subsystems/general/general_controllers/src/motor_status_broadcaster.cpp b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp similarity index 94% rename from src/subsystems/general/general_controllers/src/motor_status_broadcaster.cpp rename to src/subsystems/general/general_controllers/src/motor_status_controller.cpp index 5414087d..404016e8 100644 --- a/src/subsystems/general/general_controllers/src/motor_status_broadcaster.cpp +++ b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "general_controllers/motor_status_broadcaster.hpp" +#include "general_controllers/motor_status_controller.hpp" #include #include @@ -27,7 +27,7 @@ MotorStatusBroadcaster::MotorStatusBroadcaster() {} controller_interface::CallbackReturn MotorStatusBroadcaster::on_init() { try { - param_listener_ = std::make_shared(get_node()); + param_listener_ = std::make_shared(get_node()); params_ = param_listener_->get_params(); } catch (const std::exception & e) { RCLCPP_ERROR( @@ -67,12 +67,12 @@ controller_interface::CallbackReturn MotorStatusBroadcaster::on_configure( params_ = param_listener_->get_params(); if (params_.joints.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "No joints specified for motor_status_broadcaster."); + RCLCPP_ERROR(get_node()->get_logger(), "No joints specified for motor_status_controller."); return controller_interface::CallbackReturn::ERROR; } if (params_.interfaces.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "No interfaces specified for motor_status_broadcaster."); + RCLCPP_ERROR(get_node()->get_logger(), "No interfaces specified for motor_status_controller."); return controller_interface::CallbackReturn::ERROR; } @@ -88,7 +88,7 @@ controller_interface::CallbackReturn MotorStatusBroadcaster::on_configure( RCLCPP_INFO( get_node()->get_logger(), - "Configured motor_status_broadcaster for %zu joints, %zu interfaces, publish rate: %.1f Hz", + "Configured motor_status_controller for %zu joints, %zu interfaces, publish rate: %.1f Hz", params_.joints.size(), params_.interfaces.size(), params_.publish_rate); return controller_interface::CallbackReturn::SUCCESS; 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..d7b954bc 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -19,7 +19,7 @@ controller_manager: laser_gpio_controller: type: gpio_controllers/GpioCommandController - motor_status_broadcaster: + motor_status_controller: type: general_controllers/MotorStatusBroadcaster joint_group_position_controller: @@ -93,7 +93,7 @@ laser_gpio_controller: - temperature - is_connected -motor_status_broadcaster: +motor_status_controller: ros__parameters: joints: - stepper_motor_a 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..09d429f5 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 @@ -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, From 56a0d1432cf338e1f27af847a4eebd7e3a301c96 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Sun, 19 Apr 2026 20:29:55 -0400 Subject: [PATCH 02/37] changed broadcaster to controller and included other telemetry information --- src/msgs/msg/JointStatus.msg | 10 +-- src/msgs/msg/SystemInfo.msg | 1 + .../config/athena_arm_controllers.yaml | 2 +- .../config/athena_drive_controllers.yaml | 2 +- .../general_controllers/CMakeLists.txt | 2 +- .../general_controllers.xml | 7 +- .../motor_status_controller.hpp | 21 ++++- .../general/general_controllers/package.xml | 2 +- .../src/motor_status_controller.cpp | 89 ++++++++++--------- .../config/athena_science_controllers.yaml | 2 +- 10 files changed, 76 insertions(+), 62 deletions(-) diff --git a/src/msgs/msg/JointStatus.msg b/src/msgs/msg/JointStatus.msg index 61f1efed..af71bf94 100644 --- a/src/msgs/msg/JointStatus.msg +++ b/src/msgs/msg/JointStatus.msg @@ -1,7 +1,5 @@ -string joint -int8 id -string motor_type +string joint_name int8 temperature -float64 current -float64 motor_speed -float64 motor_angle \ No newline at end of file +float64 torque_current +int8 motor_status +string brake_status \ No newline at end of file diff --git a/src/msgs/msg/SystemInfo.msg b/src/msgs/msg/SystemInfo.msg index 7ba1d41a..706fb2c9 100644 --- a/src/msgs/msg/SystemInfo.msg +++ b/src/msgs/msg/SystemInfo.msg @@ -1 +1,2 @@ +std_msgs/Header header JointStatus[] joints \ 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 94e7b9a1..5570d90e 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -30,7 +30,7 @@ controller_manager: type: arm_controllers/ManualEndEffectorGripperClawController motor_status_controller: - type: general_controllers/MotorStatusBroadcaster + type: general_controllers/MotorStatusController twodof_joint_trajectory_controller: 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 a5df877a..1e705c36 100644 --- a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml +++ b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml @@ -50,7 +50,7 @@ controller_manager: type: gpio_controllers/GpioCommandController motor_status_controller: - type: general_controllers/MotorStatusBroadcaster + type: general_controllers/MotorStatusController rear_ackermann_controller: type: athena_drive_controllers/RearAckermannController diff --git a/src/subsystems/general/general_controllers/CMakeLists.txt b/src/subsystems/general/general_controllers/CMakeLists.txt index 36c83bfc..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) 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_controller.hpp b/src/subsystems/general/general_controllers/include/general_controllers/motor_status_controller.hpp index e1092e0d..ef01fb08 100644 --- 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 @@ -21,7 +21,7 @@ #include #include "controller_interface/controller_interface.hpp" -#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "msgs/msg/system_info.hpp" #include "general_controllers/visibility_control.h" #include #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -30,11 +30,11 @@ namespace general_controllers { -class MotorStatusBroadcaster : public controller_interface::ControllerInterface +class MotorStatusController : public controller_interface::ControllerInterface { public: GENERAL_CONTROLLERS__VISIBILITY_PUBLIC - MotorStatusBroadcaster(); + MotorStatusController(); GENERAL_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; @@ -65,7 +65,7 @@ class MotorStatusBroadcaster : public controller_interface::ControllerInterface std::shared_ptr param_listener_; motor_status_controller::Params params_; - rclcpp::Publisher::SharedPtr diagnostics_publisher_; + rclcpp::Publisher::SharedPtr motor_status_publisher_; // Map from "joint/interface" to index in state_interfaces_ std::unordered_map state_interface_map_; @@ -73,6 +73,19 @@ class MotorStatusBroadcaster : public controller_interface::ControllerInterface // Publish rate limiting rclcpp::Duration publish_period_{0, 0}; rclcpp::Time last_publish_time_{0, 0, RCL_CLOCK_UNINITIALIZED}; + + enum class MotorStatus : uint8_t { + UNKNOWN = 0, + IDLE = 1, + ACTIVE = 2, + STOPPED = 3, + DISABLED = 4, + }; + + enum class BrakeStatus : uint8_t { + LOCKED = 0, + RELEASED = 1 + }; }; } // namespace general_controllers 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_controller.cpp b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp index 404016e8..d1dc8ffc 100644 --- a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp +++ b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp @@ -22,9 +22,9 @@ namespace general_controllers { -MotorStatusBroadcaster::MotorStatusBroadcaster() {} +MotorStatusController::MotorStatusController() {} -controller_interface::CallbackReturn MotorStatusBroadcaster::on_init() +controller_interface::CallbackReturn MotorStatusController::on_init() { try { param_listener_ = std::make_shared(get_node()); @@ -39,7 +39,7 @@ controller_interface::CallbackReturn MotorStatusBroadcaster::on_init() } controller_interface::InterfaceConfiguration -MotorStatusBroadcaster::command_interface_configuration() const +MotorStatusController::command_interface_configuration() const { // Broadcaster only reads, no command interfaces return controller_interface::InterfaceConfiguration{ @@ -47,7 +47,7 @@ MotorStatusBroadcaster::command_interface_configuration() const } controller_interface::InterfaceConfiguration -MotorStatusBroadcaster::state_interface_configuration() const +MotorStatusController::state_interface_configuration() const { controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -61,7 +61,7 @@ MotorStatusBroadcaster::state_interface_configuration() const return config; } -controller_interface::CallbackReturn MotorStatusBroadcaster::on_configure( +controller_interface::CallbackReturn MotorStatusController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); @@ -83,8 +83,8 @@ controller_interface::CallbackReturn MotorStatusBroadcaster::on_configure( publish_period_ = rclcpp::Duration(0, 0); } - diagnostics_publisher_ = get_node()->create_publisher( - "~/diagnostics", rclcpp::SystemDefaultsQoS()); + motor_status_publisher_ = get_node()->create_publisher( + "~/motor_status", rclcpp::SystemDefaultsQoS()); RCLCPP_INFO( get_node()->get_logger(), @@ -94,7 +94,7 @@ controller_interface::CallbackReturn MotorStatusBroadcaster::on_configure( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn MotorStatusBroadcaster::on_activate( +controller_interface::CallbackReturn MotorStatusController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // Build lookup map from "joint/interface" -> state_interfaces_ index @@ -108,20 +108,20 @@ controller_interface::CallbackReturn MotorStatusBroadcaster::on_activate( RCLCPP_INFO( get_node()->get_logger(), - "MotorStatusBroadcaster activated with %zu state interfaces", + "MotorStatusController activated with %zu state interfaces", state_interfaces_.size()); return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn MotorStatusBroadcaster::on_deactivate( +controller_interface::CallbackReturn MotorStatusController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { state_interface_map_.clear(); return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type MotorStatusBroadcaster::update( +controller_interface::return_type MotorStatusController::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { // Rate limit publishing @@ -130,54 +130,55 @@ controller_interface::return_type MotorStatusBroadcaster::update( } last_publish_time_ = time; - diagnostic_msgs::msg::DiagnosticArray diag_msg; - diag_msg.header.stamp = time; + msgs::msg::SystemInfo system_info_msg; + system_info_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"; - + msgs::msg::JointStatus status; + status.joint_name = joint; + + // Initializing values in case state interfaces don't exist for them + status.temperature = std::numeric_limits::quiet_NaN(); + status.torque_current = std::numeric_limits::quiet_NaN(); + status.motor_status = std::numeric_limits::quiet_NaN(); + status.brake_status = "No Brakes"; + 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"; + if (iface == "temperature") { + status.temperature = static_cast(value); + } else if (iface == "torque_current") { + status.torque_current = value; + } else if (iface == "status") { + status.motor_status = static_cast(value); + if (value > sizeof(MotorStatus)){ + RCLCPP_WARN(get_node()->get_logger(), "Invalid motor status value"); + } + } else if (iface == "brake_status") { + switch (static_cast(value)){ + case BrakeStatus::LOCKED: + status.brake_status = "Brakes are Locked"; + break; + case BrakeStatus::RELEASED: + status.brake_status = "Brakes are released"; + break; + default: + RCLCPP_WARN(get_node()->get_logger(), "Invalid brake status value"); + break; } } - } else { - kv.value = "N/A"; } - - status.values.push_back(kv); } - diag_msg.status.push_back(status); + system_info_msg.joints.push_back(status); } - diagnostics_publisher_->publish(diag_msg); + motor_status_publisher_->publish(system_info_msg); return controller_interface::return_type::OK; } @@ -185,5 +186,5 @@ controller_interface::return_type MotorStatusBroadcaster::update( } // namespace general_controllers PLUGINLIB_EXPORT_CLASS( - general_controllers::MotorStatusBroadcaster, + 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 d7b954bc..95a8b089 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -20,7 +20,7 @@ controller_manager: type: gpio_controllers/GpioCommandController motor_status_controller: - type: general_controllers/MotorStatusBroadcaster + type: general_controllers/MotorStatusController joint_group_position_controller: ros__parameters: From 6dc2ccaa0e6f313e5fd40a3e20e7ec7fa7812c64 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Mon, 6 Apr 2026 23:14:50 -0400 Subject: [PATCH 03/37] odrive upgrades --- .../drive/drive.odrive.ros2_control.xacro | 27 +- src/description/urdf/athena_drive.urdf.xacro | 8 +- .../src/odrive_hardware_interface.cpp | 232 ++++++++++++++---- .../launch/athena_drive.launch.py | 10 + 4 files changed, 216 insertions(+), 61 deletions(-) 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..5c91198f 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,10 @@ 26 - - 0.0 - + + + @@ -25,10 +34,10 @@ 26 - - 0.0 - + + + diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index 66e8d7a2..bd7dd22e 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -3,11 +3,11 @@ - + @@ -70,7 +70,11 @@ simulation_controllers="$(arg simulation_controllers)"/> - + + + + + 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/subsystems/drive/drive_bringup/launch/athena_drive.launch.py b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py index 122e1025..a6650637 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, ] ) From c39757144f0f62742ff72f4b7823c680c4a41530 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Mon, 20 Apr 2026 08:18:24 -0400 Subject: [PATCH 04/37] saving progress --- .../ros2_control/arm/arm.rmd.ros2_control.xacro | 8 ++++++++ .../drive/drive.rmd.ros2_control.xacro | 16 ++++++++++++---- .../config/motor_status_controller.yaml | 7 ++++++- .../src/motor_status_controller.cpp | 8 ++++---- 4 files changed, 30 insertions(+), 9 deletions(-) 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..fc2aff9a 100644 --- a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro @@ -15,6 +15,10 @@ 100 1.0 150 + + + + @@ -26,6 +30,10 @@ 100 -1.0 150 + + + + 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..d9d576a6 100644 --- a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro @@ -15,8 +15,10 @@ 1 -1.0 150 - + + + @@ -28,8 +30,10 @@ 1 1.0 150 - + + + @@ -41,8 +45,10 @@ 1 1.0 150 - + + + @@ -54,8 +60,10 @@ 1 -1.0 150 - + + + diff --git a/src/subsystems/general/general_controllers/config/motor_status_controller.yaml b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml index 4483730f..d7a5bc82 100644 --- a/src/subsystems/general/general_controllers/config/motor_status_controller.yaml +++ b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml @@ -4,7 +4,12 @@ motor_status_controller: default_value: [], description: "Names of the joints to monitor for motor telemetry." } - interfaces: { + command_interfaces: { + type: string_array, + default_value: ["status_request", "maintenance_request"], + description: "Command interface names to read from each joint. Leave empty if no command interfaces are needed." + } + state_interfaces: { type: string_array, default_value: ["motor_temperature", "torque_current", "status", "brake_status"], description: "State interface names to read from each joint." diff --git a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp index d1dc8ffc..141c3e23 100644 --- a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp +++ b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp @@ -53,7 +53,7 @@ MotorStatusController::state_interface_configuration() const config.type = controller_interface::interface_configuration_type::INDIVIDUAL; for (const auto & joint : params_.joints) { - for (const auto & iface : params_.interfaces) { + for (const auto & iface : params_.state_interfaces) { config.names.push_back(joint + "/" + iface); } } @@ -71,7 +71,7 @@ controller_interface::CallbackReturn MotorStatusController::on_configure( return controller_interface::CallbackReturn::ERROR; } - if (params_.interfaces.empty()) { + if (params_.state_interfaces.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "No interfaces specified for motor_status_controller."); return controller_interface::CallbackReturn::ERROR; } @@ -89,7 +89,7 @@ controller_interface::CallbackReturn MotorStatusController::on_configure( RCLCPP_INFO( get_node()->get_logger(), "Configured motor_status_controller for %zu joints, %zu interfaces, publish rate: %.1f Hz", - params_.joints.size(), params_.interfaces.size(), params_.publish_rate); + params_.joints.size(), params_.state_interfaces.size(), params_.publish_rate); return controller_interface::CallbackReturn::SUCCESS; } @@ -143,7 +143,7 @@ controller_interface::return_type MotorStatusController::update( status.motor_status = std::numeric_limits::quiet_NaN(); status.brake_status = "No Brakes"; - for (const auto & iface : params_.interfaces) { + for (const auto & iface : params_.state_interfaces) { std::string key = joint + "/" + iface; auto it = state_interface_map_.find(key); From 99d13f65d0c53d32f0ac5fb0c849752c63b312d8 Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Mon, 20 Apr 2026 11:36:46 -0400 Subject: [PATCH 05/37] added request services --- .codex | 0 src/msgs/CMakeLists.txt | 2 + src/msgs/srv/MaintenanceReq.srv | 7 ++ src/msgs/srv/StatusReq.srv | 7 ++ .../motor_status_controller.hpp | 14 ++- .../src/motor_status_controller.cpp | 96 ++++++++++++++++++- 6 files changed, 120 insertions(+), 6 deletions(-) create mode 100644 .codex create mode 100644 src/msgs/srv/MaintenanceReq.srv create mode 100644 src/msgs/srv/StatusReq.srv diff --git a/.codex b/.codex new file mode 100644 index 00000000..e69de29b diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index dd0de4aa..9617b224 100644 --- a/src/msgs/CMakeLists.txt +++ b/src/msgs/CMakeLists.txt @@ -21,6 +21,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "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/srv/MaintenanceReq.srv b/src/msgs/srv/MaintenanceReq.srv new file mode 100644 index 00000000..43fd2f4c --- /dev/null +++ b/src/msgs/srv/MaintenanceReq.srv @@ -0,0 +1,7 @@ +# request_rate < 0: one-shot +# request_rate = 0: stop +# request_rate > 0: request rate in Hz +int32 request_rate +--- +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..43fd2f4c --- /dev/null +++ b/src/msgs/srv/StatusReq.srv @@ -0,0 +1,7 @@ +# request_rate < 0: one-shot +# request_rate = 0: stop +# request_rate > 0: request rate in Hz +int32 request_rate +--- +bool success +string message \ No newline at end of file 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 index ef01fb08..e33c2eef 100644 --- 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 @@ -22,6 +22,8 @@ #include "controller_interface/controller_interface.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" @@ -64,15 +66,25 @@ class MotorStatusController : public controller_interface::ControllerInterface 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_; + // 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}; + bool publish_enabled_{false}; + bool publish_once_requested_{false}; enum class MotorStatus : uint8_t { UNKNOWN = 0, diff --git a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp index 141c3e23..492f965c 100644 --- a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp +++ b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp @@ -41,9 +41,18 @@ controller_interface::CallbackReturn MotorStatusController::on_init() controller_interface::InterfaceConfiguration MotorStatusController::command_interface_configuration() const { - // Broadcaster only reads, no command interfaces - return controller_interface::InterfaceConfiguration{ - controller_interface::interface_configuration_type::NONE}; + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(params_.joints.size() * params_.command_interfaces.size()); + for (const auto & joint : params_.joints) + { + for (const auto & iface : params_.command_interfaces) { + command_interfaces_config.names.push_back(joint + "/" + iface); + } + } + + return command_interfaces_config; } controller_interface::InterfaceConfiguration @@ -86,6 +95,55 @@ controller_interface::CallbackReturn MotorStatusController::on_configure( motor_status_publisher_ = get_node()->create_publisher( "~/motor_status", rclcpp::SystemDefaultsQoS()); + auto configure_request_rate = + [this](const int32_t request_rate, const std::string & service_name) -> std::string + { + if (request_rate < 0) { + publish_enabled_ = false; + publish_once_requested_ = true; + return service_name + " queued a one-shot publish"; + } + + publish_once_requested_ = false; + if (request_rate == 0) { + publish_enabled_ = false; + publish_period_ = rclcpp::Duration(0, 0); + return service_name + " stopped publishing"; + } + + publish_enabled_ = true; + publish_period_ = rclcpp::Duration::from_seconds(1.0 / static_cast(request_rate)); + return service_name + " set publish rate to " + std::to_string(request_rate) + " Hz"; + }; + + status_request_service_ = get_node()->create_service( + "~/status_request", + [this, configure_request_rate]( + const std::shared_ptr request, + std::shared_ptr response) + { + response->success = true; + response->message = configure_request_rate(request->request_rate, "status_request"); + RCLCPP_INFO( + get_node()->get_logger(), + "Received status_request request_rate: %d", + request->request_rate); + }); + + maintenance_request_service_ = get_node()->create_service( + "~/maintenance_request", + [this, configure_request_rate]( + const std::shared_ptr request, + std::shared_ptr response) + { + response->success = true; + response->message = configure_request_rate(request->request_rate, "maintenance_request"); + RCLCPP_INFO( + get_node()->get_logger(), + "Received maintenance_request request_rate: %d", + request->request_rate); + }); + RCLCPP_INFO( get_node()->get_logger(), "Configured motor_status_controller for %zu joints, %zu interfaces, publish rate: %.1f Hz", @@ -104,6 +162,13 @@ controller_interface::CallbackReturn MotorStatusController::on_activate( state_interfaces_[i].get_interface_name()] = i; } + // 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; + } + last_publish_time_ = get_node()->now(); RCLCPP_INFO( @@ -118,22 +183,43 @@ 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 - if (publish_period_.seconds() > 0.0 && (time - last_publish_time_) < publish_period_) { + const bool periodic_publish_due = + publish_enabled_ && + (publish_period_.seconds() <= 0.0 || (time - last_publish_time_) >= publish_period_); + + if (!publish_once_requested_ && !periodic_publish_due) { return controller_interface::return_type::OK; } + + publish_once_requested_ = false; last_publish_time_ = time; msgs::msg::SystemInfo system_info_msg; system_info_msg.header.stamp = time; for (const auto & joint : params_.joints) { + + // Command Interfaces + for (const auto & iface : params_.command_interfaces) { + std::string key = joint + "/" + iface; + auto it = command_interface_map_.find(key); + if (it != command_interface_map_.end()) { + double value = command_interfaces_[it->second].get_value(); + RCLCPP_INFO(get_node()->get_logger(), "Command Interface - Joint: %s, Interface: %s, Value: %f", + joint.c_str(), iface.c_str(), value); + } else { + RCLCPP_WARN(get_node()->get_logger(), "Command interface %s not found for joint %s", iface.c_str(), joint.c_str()); + } + } + + // State Interfaces msgs::msg::JointStatus status; status.joint_name = joint; From 45119737dd4490ddf2aca53678a5ae1868ca8196 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Mon, 20 Apr 2026 14:44:49 -0400 Subject: [PATCH 06/37] included status decoding in RMDs --- .../arm/arm.rmd.ros2_control.xacro | 2 + .../arm/arm.smc_2dof.ros2_control.xacro | 9 ++ .../drive/drive.odrive.ros2_control.xacro | 6 ++ .../drive/drive.rmd.ros2_control.xacro | 4 + .../rmd_hardware_interface.hpp | 46 +++++++-- .../src/rmd_hardware_interface.cpp | 95 ++++++++++++++++--- src/msgs/msg/JointStatus.msg | 3 +- .../config/motor_status_controller.yaml | 2 +- .../motor_status_controller.hpp | 10 +- .../src/motor_status_controller.cpp | 13 --- 10 files changed, 147 insertions(+), 43 deletions(-) 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 fc2aff9a..dd2c900d 100644 --- a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro @@ -23,6 +23,7 @@ + @@ -38,6 +39,7 @@ + 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..45708546 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 @@ -17,10 +17,13 @@ 300 + + + @@ -30,10 +33,13 @@ 1000 + + + @@ -43,10 +49,13 @@ 1000 + + + 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 5c91198f..b79bc967 100644 --- a/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro @@ -23,10 +23,13 @@ 26 + + + @@ -34,10 +37,13 @@ 26 + + + 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 d9d576a6..125434e1 100644 --- a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro @@ -23,6 +23,7 @@ + @@ -38,6 +39,7 @@ + @@ -53,6 +55,7 @@ + @@ -68,6 +71,7 @@ + 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..4bcb6ccd 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 @@ -89,21 +89,25 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface // Keeps track of amount of joints int num_joints; - // Store the state for the simulated robot + // Store the state interfaces for the RMD std::vector joint_state_position_; std::vector joint_state_velocity_; + std::vector motor_temperature_; // Motor temperature in °C + std::vector motor_torque_current_; // Motor torque current in A + std::vector motor_status_; - // Store the command for the simulated robot + // Store the command interfaces for the RMD std::vector joint_command_position_; std::vector joint_command_velocity_; + std::vector motor_status_req_; + std::vector motor_maintenance_req_; + + std::vector prev_status_req_; // To track changes in status request for one-shot requests + std::vector elapsed_status_request_time_; // To track time for periodic status requests - // Place holders for data from the canBus, will be accessed in read() + // 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; @@ -121,7 +125,7 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface std::vector joint_orientation; // Modes for control mode - enum integration_level_t : std::uint8_t + enum class integration_level_t : std::uint8_t { UNDEFINED = 0, POSITION = 1, @@ -132,16 +136,40 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface std::vector control_level_; // CAN Commands + static constexpr uint8_t WRITE_ENCODER_MULTI_TURN_ZERO_CMD = 0x63; 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_1_CMD = 0x9A; 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 MotorStatus : uint8_t + { + UNKNOWN = 0, + IDLE = 1, + ACTIVE = 2, + WARNING = 3, + ERROR = 4, + DISABLED = 5 + }; + 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 + }; +}; } // namespace rmd_hardware_interface #endif // RMD_HARDWARE_INTERACE_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 b81a4f7a..4ca92f4a 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 @@ -141,20 +141,25 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_init( elapsed_update_time = 0.0; elapsed_time = 0.0; elapsed_logger_time = 0.0; + elapsed_status_request_time_.assign(num_joints, 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); + motor_temperature_.assign(num_joints, std::numeric_limits::quiet_NaN()); + motor_torque_current_.assign(num_joints, std::numeric_limits::quiet_NaN()); + motor_status_.assign(num_joints, std::numeric_limits::quiet_NaN()); joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); joint_command_velocity_.assign(num_joints, 0); + motor_status_req_.assign(num_joints, 0); + motor_maintenance_req_.assign(num_joints, 0); + + prev_status_req_.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; @@ -188,6 +193,9 @@ std::vector RMDHardwareInterface::export_sta state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, "torque_current", &motor_torque_current_[i])); + + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, "status", &motor_status_[i])); } return state_interfaces; @@ -197,6 +205,9 @@ std::vector RMDHardwareInterface::export_sta std::vector RMDHardwareInterface::export_command_interfaces() { + // TODO: Make a struct that contains each value needed to fill out the third parameter in Command interface + // Then, you can loop through the joints and given command interfaces in the xacro file and populate command + // interfaces via changing this struct std::vector command_interfaces; for(int i = 0; i < num_joints; i++){ @@ -205,6 +216,12 @@ RMDHardwareInterface::export_command_interfaces() command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_command_velocity_[i])); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "status_request", &motor_status_req_[i])); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "maintenance_request", &motor_maintenance_req_[i])); } return command_interfaces; @@ -228,14 +245,15 @@ void RMDHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { std::vector data(8, 0x00); + // Decode CAN Message 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) + + // Speed Control, Absolute Position Control, and Motor Status 2 all share the same reply 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,6 +262,12 @@ 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) + 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; + // POSITION // uint16 -> int16 -> double (for calcs) motor_position[i] = static_cast(static_cast((data[7] << 8) | data[6])); @@ -251,13 +275,32 @@ void RMDHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { // VELOCITY // uint16 -> int16 -> double (for calcs) motor_velocity[i] = static_cast(static_cast((data[5] << 8) | data[4])); + } + else if(can_rx_frame_.id == joint_node_read_ids[i] && + can_rx_frame_.data[0] == MOTOR_STATUS_1_CMD && + can_rx_frame_.data[1] != 0x00) { - // TEMPERATURE (1 byte, degrees C) + // DECODING Motor Status 1 Message + 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 byted + + // TEMPERATURE (degrees C) motor_temperature_[i] = static_cast(data[1]); + + // Initialize motor status command + motor_status_[i] = 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) + motor_status_[i] = motor_status_[i] + static_cast(data[3]); + + // ERROR STATE + motor_status_[i] = motor_status_[i] + static_cast(static_cast((data[7] << 8) | data[6])); + } else { if(logger_state == 1) { // RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Reply not heard."); @@ -338,8 +381,6 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( int32_t joint_velocity = 0; // Logger update - elapsed_update_time+=period.seconds(); - double update_period = 1.0/update_rate; elapsed_time+=period.seconds(); elapsed_logger_time+=period.seconds(); double logging_period = 1.0/logger_rate; @@ -350,10 +391,42 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( } } + // Status request handling + + for (int i = 0; i < num_joints; i++) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint_node_write_ids[i]; + can_tx_frame_.dlc = 8; + can_tx_frame_.data[0] = MOTOR_STATUS_1_CMD; + + double curr_status_req = motor_status_req_[i]; + if (curr_status_req < 0 && prev_status_req_[i] >= 0) // Send one shot status request + { + canBus.send(can_tx_frame_); + } + else if (motor_status_req_[i] > 0) // Send status request at specified frequency (Hz) + { + elapsed_status_request_time_[i] += period.seconds(); + double status_request_period = 1.0/motor_status_req_[i]; + if(elapsed_status_request_time_[i] > status_request_period){ + elapsed_status_request_time_[i] = 0.0; + canBus.send(can_tx_frame_); + } + } + else + { + continue; + } + prev_status_req_[i] = curr_status_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++) { diff --git a/src/msgs/msg/JointStatus.msg b/src/msgs/msg/JointStatus.msg index af71bf94..9bef7db0 100644 --- a/src/msgs/msg/JointStatus.msg +++ b/src/msgs/msg/JointStatus.msg @@ -1,5 +1,4 @@ string joint_name int8 temperature float64 torque_current -int8 motor_status -string brake_status \ No newline at end of file +int8 motor_status \ No newline at end of file diff --git a/src/subsystems/general/general_controllers/config/motor_status_controller.yaml b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml index d7a5bc82..db901446 100644 --- a/src/subsystems/general/general_controllers/config/motor_status_controller.yaml +++ b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml @@ -11,7 +11,7 @@ motor_status_controller: } state_interfaces: { type: string_array, - default_value: ["motor_temperature", "torque_current", "status", "brake_status"], + default_value: ["motor_temperature", "torque_current", "status"], description: "State interface names to read from each joint." } publish_rate: { 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 index e33c2eef..4eee0c40 100644 --- 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 @@ -90,13 +90,9 @@ class MotorStatusController : public controller_interface::ControllerInterface UNKNOWN = 0, IDLE = 1, ACTIVE = 2, - STOPPED = 3, - DISABLED = 4, - }; - - enum class BrakeStatus : uint8_t { - LOCKED = 0, - RELEASED = 1 + WARNING = 3, + ERROR = 4, + DISABLED = 5 }; }; diff --git a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp index 492f965c..857ec70b 100644 --- a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp +++ b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp @@ -227,7 +227,6 @@ controller_interface::return_type MotorStatusController::update( status.temperature = std::numeric_limits::quiet_NaN(); status.torque_current = std::numeric_limits::quiet_NaN(); status.motor_status = std::numeric_limits::quiet_NaN(); - status.brake_status = "No Brakes"; for (const auto & iface : params_.state_interfaces) { std::string key = joint + "/" + iface; @@ -245,18 +244,6 @@ controller_interface::return_type MotorStatusController::update( if (value > sizeof(MotorStatus)){ RCLCPP_WARN(get_node()->get_logger(), "Invalid motor status value"); } - } else if (iface == "brake_status") { - switch (static_cast(value)){ - case BrakeStatus::LOCKED: - status.brake_status = "Brakes are Locked"; - break; - case BrakeStatus::RELEASED: - status.brake_status = "Brakes are released"; - break; - default: - RCLCPP_WARN(get_node()->get_logger(), "Invalid brake status value"); - break; - } } } } From 0da4b271d05b7330e778446479c90321b1da5b4e Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Mon, 20 Apr 2026 15:29:50 -0400 Subject: [PATCH 07/37] decode error value and warn/error out depending on response --- .../rmd_hardware_interface.hpp | 20 +++++++ .../src/rmd_hardware_interface.cpp | 56 +++++++++++++++++++ 2 files changed, 76 insertions(+) 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 4bcb6ccd..40708008 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 @@ -76,6 +76,7 @@ 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); private: // Hardware Interface Parameters @@ -169,6 +170,25 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface OVER_TEMPERATURE = 0x1000, ENCODER_CALIBRATION_ERROR = 0x2000 }; + + struct StatusEntry { + RMDMotorStatus flag; + const char* name; + }; + + 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"} + }; }; } // namespace rmd_hardware_interface 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 4ca92f4a..059d5874 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 @@ -106,6 +106,55 @@ void RMDHardwareInterface::logger_function(){ 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; + + // 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; +} hardware_interface::CallbackReturn RMDHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file @@ -367,8 +416,15 @@ hardware_interface::return_type RMDHardwareInterface::read( 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]); + + // Process status logs warning and errors and returns false if fatal error occurs + if (!process_status(motor_status_[i], rclcpp::get_logger("RMDHardwareInterface"))) { + return hardware_interface::return_type::ERROR; + } } + + return hardware_interface::return_type::OK; } From 28937b27a95775f99a6792e43acac1b3c74cd083 Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Tue, 21 Apr 2026 16:10:02 -0400 Subject: [PATCH 08/37] reformatted rmd HWI --- .../drive/drive.odrive.ros2_control.xacro | 12 +- .../drive/drive.rmd.ros2_control.xacro | 4 + .../rmd_hardware_interface.hpp | 103 +++-- .../src/rmd_hardware_interface.cpp | 430 +++++++++++------- src/msgs/srv/MaintenanceReq.srv | 3 + src/msgs/srv/StatusReq.srv | 2 + .../src/rear_ackermann_controller.cpp | 20 +- .../config/motor_status_controller.yaml | 2 +- .../motor_status_controller.hpp | 7 +- .../src/motor_status_controller.cpp | 85 ++-- 10 files changed, 417 insertions(+), 251 deletions(-) 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 b79bc967..ec25c66b 100644 --- a/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro @@ -23,13 +23,13 @@ 26 - - + - + @@ -37,13 +37,13 @@ 26 - - + - + 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 125434e1..df1fbdab 100644 --- a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro @@ -19,6 +19,7 @@ + @@ -35,6 +36,7 @@ + @@ -51,6 +53,7 @@ + @@ -67,6 +70,7 @@ + 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 40708008..6f418a5b 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" @@ -89,29 +90,6 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface // Keeps track of amount of joints int num_joints; - - // Store the state interfaces for the RMD - std::vector joint_state_position_; - std::vector joint_state_velocity_; - std::vector motor_temperature_; // Motor temperature in °C - std::vector motor_torque_current_; // Motor torque current in A - std::vector motor_status_; - - // Store the command interfaces for the RMD - std::vector joint_command_position_; - std::vector joint_command_velocity_; - std::vector motor_status_req_; - std::vector motor_maintenance_req_; - - std::vector prev_status_req_; // To track changes in status request for one-shot requests - std::vector elapsed_status_request_time_; // To track time for periodic status requests - - // Place holders for data from the CANBus, will be accessed in read() - std::vector motor_velocity; - std::vector motor_position; - - // Velocity at which **joint** rotates to reach position in 1 dps - uint16_t operating_velocity; // CAN Library Setup CANLib::SocketCanBus canBus; @@ -133,20 +111,75 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface VELOCITY = 2, }; - // Active control mode for each actuator - std::vector control_level_; + 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 joint_command_position; + double joint_command_velocity; + double motor_status_req; + double motor_maintenance_req; + double maintenance_cmd_id; + + double prev_status_req; + double prev_maintenance_req; + double prev_maintenance_cmd_id; + 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; + }; + + std::vector RMDJoints_; // CAN Commands - static constexpr uint8_t WRITE_ENCODER_MULTI_TURN_ZERO_CMD = 0x63; - 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_1_CMD = 0x9A; - 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; + // 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, + MOTOR_STATUS_1_CMD = 0x9A, + MOTOR_STATUS_2_CMD = 0X9C, + }; + // General motor status (can be interpreted by controller) enum class MotorStatus : uint8_t { UNKNOWN = 0, @@ -157,6 +190,7 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface DISABLED = 5 }; + // Specific motor status flags enum class RMDMotorStatus : uint16_t { BRAKE_LOCKED = 0x0000, BRAKE_RELEASED = 0x0001, @@ -171,6 +205,7 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface ENCODER_CALIBRATION_ERROR = 0x2000 }; + // Corresponding descriptions for specific motor status flags struct StatusEntry { RMDMotorStatus flag; const char* name; 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 059d5874..b30056d0 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 @@ -73,33 +73,67 @@ 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" + oss << "\nJOINT: " << 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" << "-- 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" + << "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 + << " | Motor Maintenance Command ID: " << joint.maintenance_cmd_id << "\n" + << "Previous: Motor Status Request Rate: " << joint.prev_status_req + << " | Motor Maintenance Request Rate: " << joint.prev_maintenance_req + << " | Motor Maintenance Command ID: " << joint.prev_maintenance_cmd_id << "\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 << "\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\n"; + + /* + // Interfaces + oss << "-- Interfaces --\n"; + oss << "State Interfaces: "; + for (size_t si = 0; si < joint.state_interface_names.size(); ++si) { + if (si) oss << ", "; + oss << joint.state_interface_names[si]; + } + oss << "\n"; + + oss << "Command Interfaces: "; + for (size_t ci = 0; ci < joint.command_interface_names.size(); ++ci) { + if (ci) oss << ", "; + oss << joint.command_interface_names[ci]; + } + oss << "\n"; + + // Parameters map + oss << "-- Parameters --\n"; + bool first_param = true; + for (const auto &p : joint.parameters) { + if (!first_param) oss << ", "; + oss << p.first << ": " << p.second; + first_param = false; + } + oss << "\n"; + */ } log_msg += oss.str(); @@ -165,23 +199,71 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_init( } // Setting Parameters + RMDJoints_.clear(); 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) - ); + // 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); + } + + 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 = std::stoi(joint.parameters.at("joint_orientation")) == -1 ? -1 : 1, + .operating_velocity = static_cast(std::clamp( + std::stoi(joint.parameters.at("operating_velocity")), + 0, + static_cast(3.5 * gear_ratio) + )), + .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(), + + .joint_command_position = std::numeric_limits::quiet_NaN(), + .joint_command_velocity = 0.0, + .motor_status_req = 0.0, + .motor_maintenance_req = 0.0, + .maintenance_cmd_id = std::numeric_limits::quiet_NaN(), + + .prev_status_req = 0.0, + .prev_maintenance_req = 0.0, + .prev_maintenance_cmd_id = std::numeric_limits::quiet_NaN(), + .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 + } + ); } - num_joints = static_cast(info_.joints.size()); + num_joints = static_cast(RMDJoints_.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")); @@ -190,26 +272,6 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_init( elapsed_update_time = 0.0; elapsed_time = 0.0; elapsed_logger_time = 0.0; - elapsed_status_request_time_.assign(num_joints, 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); - motor_temperature_.assign(num_joints, std::numeric_limits::quiet_NaN()); - motor_torque_current_.assign(num_joints, std::numeric_limits::quiet_NaN()); - motor_status_.assign(num_joints, std::numeric_limits::quiet_NaN()); - - joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); - joint_command_velocity_.assign(num_joints, 0); - motor_status_req_.assign(num_joints, 0); - motor_maintenance_req_.assign(num_joints, 0); - - prev_status_req_.assign(num_joints, 0.0); - - motor_position.assign(num_joints, 0.0); - motor_velocity.assign(num_joints, 0.0); - - control_level_.resize(num_joints, integration_level_t::POSITION); return hardware_interface::CallbackReturn::SUCCESS; } @@ -229,24 +291,28 @@ 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( - info_.joints[i].name, "status", &motor_status_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.name, iface, val)); + } } - return state_interfaces; } @@ -254,23 +320,29 @@ std::vector RMDHardwareInterface::export_sta std::vector RMDHardwareInterface::export_command_interfaces() { - // TODO: Make a struct that contains each value needed to fill out the third parameter in Command interface - // Then, you can loop through the joints and given command interfaces in the xacro file and populate command - // interfaces via changing this struct 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])); - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, "status_request", &motor_status_req_[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_cmd_id") { + val = &joint.maintenance_cmd_id; + } 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( - info_.joints[i].name, "maintenance_request", &motor_maintenance_req_[i])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, iface, val)); + } } return command_interfaces; @@ -295,11 +367,12 @@ void RMDHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { std::vector data(8, 0x00); // Decode CAN Message - 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) && + 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)) && can_rx_frame_.data[1] != 0x00) { // Speed Control, Absolute Position Control, and Motor Status 2 all share the same reply @@ -312,22 +385,22 @@ void RMDHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { data[7] = can_rx_frame_.data[7]; // encoder position high byte // TEMPERATURE (1 byte, degrees C) - motor_temperature_[i] = static_cast(data[1]); + joint.motor_temperature = 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; + 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_ids[i] && - can_rx_frame_.data[0] == MOTOR_STATUS_1_CMD && - can_rx_frame_.data[1] != 0x00) { + else if(can_rx_frame_.id == joint.node_read_id && + can_rx_frame_.data[0] == static_cast(StatusCommands::MOTOR_STATUS_1_CMD) && + can_rx_frame_.data[1] != 0x00) { // DECODING Motor Status 1 Message data[1] = can_rx_frame_.data[1]; // Motor Temperature @@ -339,16 +412,16 @@ void RMDHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { data[7] = can_rx_frame_.data[7]; // Error state high byted // TEMPERATURE (degrees C) - motor_temperature_[i] = static_cast(data[1]); + joint.motor_temperature = static_cast(data[1]); // Initialize motor status command - motor_status_[i] = 0.0; + joint.motor_status = 0.0; // BRAKE STATUS (0: Locked, 1: Released) - motor_status_[i] = motor_status_[i] + static_cast(data[3]); + joint.motor_status = joint.motor_status + static_cast(data[3]); // ERROR STATE - motor_status_[i] = motor_status_[i] + static_cast(static_cast((data[7] << 8) | data[6])); + joint.motor_status = joint.motor_status + static_cast(static_cast((data[7] << 8) | data[6])); } else { if(logger_state == 1) { @@ -362,11 +435,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(); @@ -379,13 +450,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!"); @@ -398,9 +471,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!"); @@ -413,18 +486,16 @@ 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(motor_status_[i], rclcpp::get_logger("RMDHardwareInterface"))) { + if (!process_status(joint.motor_status, rclcpp::get_logger("RMDHardwareInterface"))) { return hardware_interface::return_type::ERROR; } } - - return hardware_interface::return_type::OK; } @@ -432,7 +503,7 @@ hardware_interface::return_type RMDHardwareInterface::read( hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - int data[8] = {0x00}; + uint8_t data[8] = {0x00}; int32_t joint_angle = 0; int32_t joint_velocity = 0; @@ -448,24 +519,54 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( } // Status request handling + for (auto & joint : RMDJoints_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = joint.node_write_id; + can_tx_frame_.dlc = 8; + can_tx_frame_.data[0] = static_cast(StatusCommands::MOTOR_STATUS_1_CMD); + + double curr_status_req = joint.motor_status_req; + if (curr_status_req < 0 && joint.prev_status_req >= 0) // Send one shot status request + { + canBus.send(can_tx_frame_); + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "One shot status request sent for joint '%s'", joint.name.c_str()); + } + else if (joint.motor_status_req > 0) // Send status request at specified frequency (Hz) + { + joint.elapsed_status_request_time += period.seconds(); + double status_request_period = 1.0 / joint.motor_status_req; + if(joint.elapsed_status_request_time > status_request_period){ + joint.elapsed_status_request_time = 0.0; + canBus.send(can_tx_frame_); + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Periodic status request sent for joint '%s'", joint.name.c_str()); + } + } + else + { + continue; + } + joint.prev_status_req = curr_status_req; + } - for (int i = 0; i < num_joints; i++) { + // Maintenance request handling + for (auto & joint : RMDJoints_) { can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_write_ids[i]; + can_tx_frame_.id = joint.node_write_id; can_tx_frame_.dlc = 8; - can_tx_frame_.data[0] = MOTOR_STATUS_1_CMD; - double curr_status_req = motor_status_req_[i]; - if (curr_status_req < 0 && prev_status_req_[i] >= 0) // Send one shot status request + double curr_maintenance_req = joint.motor_maintenance_req; + uint8_t maintenance_cmd_id = 0x00; + can_tx_frame_.data[0] = maintenance_cmd_id; + if (curr_maintenance_req < 0 && joint.prev_maintenance_req >= 0) // Send one shot maintenance request { canBus.send(can_tx_frame_); } - else if (motor_status_req_[i] > 0) // Send status request at specified frequency (Hz) + else if(joint.motor_maintenance_req > 0) // Send maintenance request at specified frequency (Hz) { - elapsed_status_request_time_[i] += period.seconds(); - double status_request_period = 1.0/motor_status_req_[i]; - if(elapsed_status_request_time_[i] > status_request_period){ - elapsed_status_request_time_[i] = 0.0; + 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_); } } @@ -473,11 +574,9 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( { continue; } - prev_status_req_[i] = curr_status_req; + 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 @@ -485,45 +584,50 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( 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_) { + std::fill(std::begin(data), std::end(data), 0x00); 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])) { - + if (joint.control_level == integration_level_t::POSITION && std::isfinite(joint.joint_command_position) && + joint.joint_command_position != joint.prev_joint_command_position) { // CALCULATE DESIRED JOINT ANGLE - joint_angle = joint_orientation[i]*calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); - + joint_angle = joint.orientation * calculate_motor_position_from_desired_joint_position( + joint.joint_command_position, joint.gear_ratio); + // ENCODING CAN MESSAGE - data[0] = ABSOLUTE_POS_CONTROL_CMD; + data[0] = static_cast(ControlCommands::ABSOLUTE_POS_CONTROL_CMD); data[1] = 0x00; - data[2] = operating_velocity & 0xFF; - data[3] = (operating_velocity >> 8) & 0xFF; + data[2] = joint.operating_velocity & 0xFF; + data[3] = (joint.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])) { - + 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) { // CALCULATE DESIRED JOINT VELOCITY - joint_velocity = joint_orientation[i]*calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); - + joint_velocity = joint.orientation * calculate_motor_velocity_from_desired_joint_velocity( + joint.joint_command_velocity, joint.gear_ratio); + // ENCODING CAN MESSAGE - data[0] = SPEED_CONTROL_CMD; + data[0] = static_cast(ControlCommands::SPEED_CONTROL_CMD); data[1] = 0x00; data[2] = 0x00; - data[3] = 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; + + joint.prev_joint_command_velocity = joint.joint_command_velocity; + } else { + // No new control command to send; send status poll instead + data[0] = static_cast(StatusCommands::MOTOR_STATUS_2_CMD); + // RCLCPP_WARN(rclcpp::get_logger("RMDHardwareInterface"), "Joint command value not found or undefined command state. Sending Motor Status 2 command for joint '%s'", joint.name.c_str()); } // Cast data to uint8_t @@ -532,6 +636,12 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( can_tx_frame_.data[j] = data[j]; } canBus.send(can_tx_frame_); + RCLCPP_INFO( + rclcpp::get_logger("RMDHardwareInterface"), + "Sent CAN message for joint '%s' with command '%s'", + joint.name.c_str(), + std::to_string(data[0]).c_str() + ); } } @@ -579,43 +689,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) { + 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); } } } @@ -629,4 +739,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/msgs/srv/MaintenanceReq.srv b/src/msgs/srv/MaintenanceReq.srv index 43fd2f4c..201bae95 100644 --- a/src/msgs/srv/MaintenanceReq.srv +++ b/src/msgs/srv/MaintenanceReq.srv @@ -1,7 +1,10 @@ # 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 --- bool success string message \ No newline at end of file diff --git a/src/msgs/srv/StatusReq.srv b/src/msgs/srv/StatusReq.srv index 43fd2f4c..9290d795 100644 --- a/src/msgs/srv/StatusReq.srv +++ b/src/msgs/srv/StatusReq.srv @@ -1,6 +1,8 @@ # request_rate < 0: one-shot # request_rate = 0: stop # request_rate > 0: request rate in Hz + +string joint_name int32 request_rate --- bool success 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/general_controllers/config/motor_status_controller.yaml b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml index db901446..3045a06a 100644 --- a/src/subsystems/general/general_controllers/config/motor_status_controller.yaml +++ b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml @@ -6,7 +6,7 @@ motor_status_controller: } command_interfaces: { type: string_array, - default_value: ["status_request", "maintenance_request"], + default_value: ["status_request", "maintenance_request", "maintenance_cmd_id"], description: "Command interface names to read from each joint. Leave empty if no command interfaces are needed." } state_interfaces: { 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 index 4eee0c40..95d22365 100644 --- 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 @@ -73,6 +73,11 @@ class MotorStatusController : public controller_interface::ControllerInterface // Services for command interfaces rclcpp::Service::SharedPtr status_request_service_; rclcpp::Service::SharedPtr maintenance_request_service_; + std::string status_req_joint_name; + int status_request_rate; + std::string maintenance_req_joint_name; + int maintenance_request_rate; + uint8_t maintenance_cmd_id; // Map from "joint/interface" to index in state_interfaces_ std::unordered_map state_interface_map_; @@ -83,8 +88,6 @@ class MotorStatusController : public controller_interface::ControllerInterface // Publish rate limiting rclcpp::Duration publish_period_{0, 0}; rclcpp::Time last_publish_time_{0, 0, RCL_CLOCK_UNINITIALIZED}; - bool publish_enabled_{false}; - bool publish_once_requested_{false}; enum class MotorStatus : uint8_t { UNKNOWN = 0, diff --git a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp index 857ec70b..307049f2 100644 --- a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp +++ b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp @@ -35,6 +35,11 @@ controller_interface::CallbackReturn MotorStatusController::on_init() return controller_interface::CallbackReturn::ERROR; } + status_req_joint_name = ""; + status_request_rate = 0; + maintenance_req_joint_name = ""; + maintenance_request_rate = 0; + return controller_interface::CallbackReturn::SUCCESS; } @@ -95,53 +100,49 @@ controller_interface::CallbackReturn MotorStatusController::on_configure( motor_status_publisher_ = get_node()->create_publisher( "~/motor_status", rclcpp::SystemDefaultsQoS()); - auto configure_request_rate = - [this](const int32_t request_rate, const std::string & service_name) -> std::string - { - if (request_rate < 0) { - publish_enabled_ = false; - publish_once_requested_ = true; - return service_name + " queued a one-shot publish"; - } - - publish_once_requested_ = false; - if (request_rate == 0) { - publish_enabled_ = false; - publish_period_ = rclcpp::Duration(0, 0); - return service_name + " stopped publishing"; - } - - publish_enabled_ = true; - publish_period_ = rclcpp::Duration::from_seconds(1.0 / static_cast(request_rate)); - return service_name + " set publish rate to " + std::to_string(request_rate) + " Hz"; - }; - status_request_service_ = get_node()->create_service( "~/status_request", - [this, configure_request_rate]( + [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; + } + + status_req_joint_name = request->joint_name; + status_request_rate = request->request_rate; + + std::string msg = "Received status_request for joint " + status_req_joint_name + " at request_rate: " + std::to_string(status_request_rate); response->success = true; - response->message = configure_request_rate(request->request_rate, "status_request"); - RCLCPP_INFO( - get_node()->get_logger(), - "Received status_request request_rate: %d", - request->request_rate); + response->message = msg; + RCLCPP_INFO(get_node()->get_logger(), "%s", msg.c_str()); }); maintenance_request_service_ = get_node()->create_service( "~/maintenance_request", - [this, configure_request_rate]( + [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; + maintenance_cmd_id = request->command_id; + + std::string msg = "Received maintenance_request " + std::to_string(maintenance_cmd_id) + " for joint " + maintenance_req_joint_name + " at request_rate: " + std::to_string(maintenance_request_rate); response->success = true; - response->message = configure_request_rate(request->request_rate, "maintenance_request"); - RCLCPP_INFO( - get_node()->get_logger(), - "Received maintenance_request request_rate: %d", - request->request_rate); + response->message = msg; + RCLCPP_INFO(get_node()->get_logger(), "%s", msg.c_str()); }); RCLCPP_INFO( @@ -190,15 +191,13 @@ controller_interface::CallbackReturn MotorStatusController::on_deactivate( controller_interface::return_type MotorStatusController::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { + // Rate limit publishing const bool periodic_publish_due = - publish_enabled_ && (publish_period_.seconds() <= 0.0 || (time - last_publish_time_) >= publish_period_); - if (!publish_once_requested_ && !periodic_publish_due) { + if (!periodic_publish_due) { return controller_interface::return_type::OK; } - - publish_once_requested_ = false; last_publish_time_ = time; msgs::msg::SystemInfo system_info_msg; @@ -211,9 +210,15 @@ controller_interface::return_type MotorStatusController::update( std::string key = joint + "/" + iface; auto it = command_interface_map_.find(key); if (it != command_interface_map_.end()) { - double value = command_interfaces_[it->second].get_value(); - RCLCPP_INFO(get_node()->get_logger(), "Command Interface - Joint: %s, Interface: %s, Value: %f", - joint.c_str(), iface.c_str(), value); + if (iface == "maintenance_cmd_id" && joint == maintenance_req_joint_name) { + command_interfaces_[it->second].set_value(maintenance_cmd_id); + } + else if (iface == "status_request" && joint == status_req_joint_name) { + command_interfaces_[it->second].set_value(status_request_rate); + } + else if (iface == "maintenance_request" && joint == maintenance_req_joint_name) { + command_interfaces_[it->second].set_value(maintenance_request_rate); + } } else { RCLCPP_WARN(get_node()->get_logger(), "Command interface %s not found for joint %s", iface.c_str(), joint.c_str()); } From 60ccc04ab3eb8a747b8a34c5c906073c5f98e428 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Wed, 22 Apr 2026 04:14:18 -0400 Subject: [PATCH 09/37] saving some one shot progress --- .../src/rmd_hardware_interface.cpp | 6 ------ .../motor_status_controller.hpp | 5 ++++- .../src/motor_status_controller.cpp | 17 ++++++++++++++++- 3 files changed, 20 insertions(+), 8 deletions(-) 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 b30056d0..297b93a4 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 @@ -636,12 +636,6 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( can_tx_frame_.data[j] = data[j]; } canBus.send(can_tx_frame_); - RCLCPP_INFO( - rclcpp::get_logger("RMDHardwareInterface"), - "Sent CAN message for joint '%s' with command '%s'", - joint.name.c_str(), - std::to_string(data[0]).c_str() - ); } } 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 index 95d22365..31653578 100644 --- 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 @@ -78,6 +78,7 @@ class MotorStatusController : public controller_interface::ControllerInterface std::string maintenance_req_joint_name; int maintenance_request_rate; uint8_t maintenance_cmd_id; + bool one_shot_sent; // Map from "joint/interface" to index in state_interfaces_ std::unordered_map state_interface_map_; @@ -88,7 +89,9 @@ class MotorStatusController : public controller_interface::ControllerInterface // Publish rate limiting rclcpp::Duration publish_period_{0, 0}; rclcpp::Time last_publish_time_{0, 0, RCL_CLOCK_UNINITIALIZED}; - + rclcpp::Time one_shot_time{0, 0, RCL_CLOCK_UNINITIALIZED}; + rclcpp::Duration one_shot_delay{0, 500000000}; + enum class MotorStatus : uint8_t { UNKNOWN = 0, IDLE = 1, diff --git a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp index 307049f2..59fc0f7a 100644 --- a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp +++ b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp @@ -39,6 +39,7 @@ controller_interface::CallbackReturn MotorStatusController::on_init() status_request_rate = 0; maintenance_req_joint_name = ""; maintenance_request_rate = 0; + one_shot_sent = false; return controller_interface::CallbackReturn::SUCCESS; } @@ -214,7 +215,21 @@ controller_interface::return_type MotorStatusController::update( command_interfaces_[it->second].set_value(maintenance_cmd_id); } else if (iface == "status_request" && joint == status_req_joint_name) { - command_interfaces_[it->second].set_value(status_request_rate); + if (status_request_rate < 0 && one_shot_sent == false) { + command_interfaces_[it->second].set_value(status_request_rate); + RCLCPP_WARN(get_node()->get_logger(), "One shot sent: ."); + one_shot_sent = true; + one_shot_time = time; + } + else if (status_request_rate < 0 && one_shot_sent == true && (time - one_shot_time) >= one_shot_delay) { + status_request_rate = 0; + command_interfaces_[it->second].set_value(status_request_rate); + one_shot_sent = false; + RCLCPP_WARN(get_node()->get_logger(), "One shot reset."); + } + else if (status_request_rate >= 0) { + command_interfaces_[it->second].set_value(status_request_rate); + } } else if (iface == "maintenance_request" && joint == maintenance_req_joint_name) { command_interfaces_[it->second].set_value(maintenance_request_rate); From ca20393d65dd465d57c0eb8fe7ba71a355cdfec8 Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Thu, 23 Apr 2026 06:25:05 -0400 Subject: [PATCH 10/37] all i know is pain (i think i got encoding and decoding across command interfaces working) --- .../drive/drive.rmd.ros2_control.xacro | 16 +- .../rmd_hardware_interface.hpp | 211 ++++++++---- .../src/rmd_hardware_interface.cpp | 318 ++++++++++++------ src/msgs/srv/MaintenanceReq.srv | 5 + .../config/motor_status_controller.yaml | 2 +- .../motor_status_controller.hpp | 59 +++- .../src/motor_status_controller.cpp | 81 ++++- 7 files changed, 504 insertions(+), 188 deletions(-) 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 df1fbdab..593bd109 100644 --- a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro @@ -19,7 +19,9 @@ - + + + @@ -36,7 +38,9 @@ - + + + @@ -53,7 +57,9 @@ - + + + @@ -70,7 +76,9 @@ - + + + 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 6f418a5b..a1d968c7 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 @@ -25,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 @@ -69,40 +74,6 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; - // -- Helper Functions -- - void send_command(int can_id, int cmd_id); - 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 process_status(uint16_t status, const rclcpp::Logger& logger); - -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; - - // 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_write_ids; - std::vector joint_node_read_ids; - std::vector joint_gear_ratios; - std::vector joint_orientation; - // Modes for control mode enum class integration_level_t : std::uint8_t { @@ -111,6 +82,21 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface 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; @@ -131,11 +117,14 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface double joint_command_velocity; double motor_status_req; double motor_maintenance_req; - double maintenance_cmd_id; + 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 prev_maintenance_cmd_id; double elapsed_status_request_time; double elapsed_maintenance_request_time; double motor_velocity; @@ -148,6 +137,56 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface 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); + 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 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 + 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; + + // 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_write_ids; + std::vector joint_node_read_ids; + std::vector joint_gear_ratios; + std::vector joint_orientation; + std::vector RMDJoints_; // CAN Commands @@ -165,6 +204,7 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface MOTOR_STOP_CMD = 0x81, }; + // Control commands: Commands that actuate the motor enum class ControlCommands : uint8_t { TORQUE_CONTROL_CMD = 0xA1, @@ -175,10 +215,18 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface // 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 { @@ -190,27 +238,6 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface DISABLED = 5 }; - // 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 - }; - - // Corresponding descriptions for specific motor status flags - struct StatusEntry { - RMDMotorStatus flag; - const char* name; - }; - inline static const std::vector status_table = { {RMDMotorStatus::BRAKE_RELEASED, "Brake Released"}, {RMDMotorStatus::BRAKE_LOCKED, "Brake Locked"}, @@ -224,7 +251,71 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface {RMDMotorStatus::OVER_TEMPERATURE, "Over Temperature"}, {RMDMotorStatus::ENCODER_CALIBRATION_ERROR, "Encoder Calibration Error"} }; -}; -} // namespace rmd_hardware_interface -#endif // RMD_HARDWARE_INTERACE_HPP_ + inline DecodedCommand unpack_command_full(int32_t counts_in, int64_t payload_in) + { + uint32_t counts = static_cast(counts_in); + uint64_t payload = static_cast(payload_in); + + // Counts are packed in the top 24 bits of a 32-bit value + uint8_t u8_count = (counts >> 16) & 0xFF; + uint8_t i16_count = (counts >> 8) & 0xFF; + uint8_t i32_count = (counts >> 0) & 0xFF; + + int bit_offset = 64; + auto pop_bits = [&](int bits) -> uint64_t { + bit_offset -= bits; + uint64_t mask = (1ULL << bits) - 1; + 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(); + + // 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 297b93a4..9604ba39 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,6 +13,7 @@ #include #include #include +#include #include "hardware_interface/system_interface.hpp" #include "hardware_interface/lexical_casts.hpp" @@ -25,6 +26,8 @@ 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; @@ -96,44 +99,21 @@ void RMDHardwareInterface::logger_function(){ << " | Joint Command Velocity: " << joint.joint_command_velocity << "\n" << "Motor Status Request: " << joint.motor_status_req << " | Motor Maintenance Request Rate: " << joint.motor_maintenance_req - << " | Motor Maintenance Command ID: " << joint.maintenance_cmd_id << "\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 - << " | Motor Maintenance Command ID: " << joint.prev_maintenance_cmd_id << "\n" - << "-- State --\n" + << "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 << " | Joint Velocity: " << joint.joint_state_velocity << "\n" << "-- Telemetry --\n" << "Motor Temperature: " << joint.motor_temperature << " C" << " | Torque Current: " << joint.motor_torque_current << " A\n"; - - /* - // Interfaces - oss << "-- Interfaces --\n"; - oss << "State Interfaces: "; - for (size_t si = 0; si < joint.state_interface_names.size(); ++si) { - if (si) oss << ", "; - oss << joint.state_interface_names[si]; - } - oss << "\n"; - - oss << "Command Interfaces: "; - for (size_t ci = 0; ci < joint.command_interface_names.size(); ++ci) { - if (ci) oss << ", "; - oss << joint.command_interface_names[ci]; - } - oss << "\n"; - - // Parameters map - oss << "-- Parameters --\n"; - bool first_param = true; - for (const auto &p : joint.parameters) { - if (!first_param) oss << ", "; - oss << p.first << ": " << p.second; - first_param = false; - } - oss << "\n"; - */ } log_msg += oss.str(); @@ -190,6 +170,160 @@ bool RMDHardwareInterface::process_status(uint16_t status, const rclcpp::Logger 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 { @@ -245,11 +379,14 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_init( .joint_command_velocity = 0.0, .motor_status_req = 0.0, .motor_maintenance_req = 0.0, - .maintenance_cmd_id = std::numeric_limits::quiet_NaN(), + .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, - .prev_maintenance_cmd_id = std::numeric_limits::quiet_NaN(), .elapsed_status_request_time = 0.0, .elapsed_maintenance_request_time = 0.0, .motor_velocity = 0.0, @@ -333,8 +470,12 @@ RMDHardwareInterface::export_command_interfaces() val = &joint.motor_status_req; } else if (iface == "maintenance_request") { val = &joint.motor_maintenance_req; - } else if (iface == "maintenance_cmd_id") { - val = &joint.maintenance_cmd_id; + } 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; @@ -504,8 +645,6 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { uint8_t data[8] = {0x00}; - int32_t joint_angle = 0; - int32_t joint_velocity = 0; // Logger update elapsed_time+=period.seconds(); @@ -520,31 +659,25 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( // Status request handling for (auto & joint : RMDJoints_) { - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint.node_write_id; - can_tx_frame_.dlc = 8; - can_tx_frame_.data[0] = static_cast(StatusCommands::MOTOR_STATUS_1_CMD); - double curr_status_req = joint.motor_status_req; if (curr_status_req < 0 && joint.prev_status_req >= 0) // Send one shot status request { - canBus.send(can_tx_frame_); - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "One shot status request sent for joint '%s'", joint.name.c_str()); + for (auto & status_command : kStatusCommands) { + send_command(joint.node_write_id, static_cast(status_command)); + } + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "One-shot status request sent for joint '%s'.", joint.name.c_str()); } - else if (joint.motor_status_req > 0) // Send status request at specified frequency (Hz) + 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 / joint.motor_status_req; + 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; - canBus.send(can_tx_frame_); - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Periodic status request sent for joint '%s'", joint.name.c_str()); + for (auto & status_command : kStatusCommands) { + send_command(joint.node_write_id, static_cast(status_command)); + } } } - else - { - continue; - } joint.prev_status_req = curr_status_req; } @@ -554,26 +687,43 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( 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; - uint8_t maintenance_cmd_id = 0x00; - can_tx_frame_.data[0] = maintenance_cmd_id; + if (curr_maintenance_req < 0 && joint.prev_maintenance_req >= 0) // Send one shot maintenance request { - canBus.send(can_tx_frame_); + 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(joint.motor_maintenance_req > 0) // Send maintenance request at specified frequency (Hz) + 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_); + canBus.send(can_tx_frame_); } } - else - { - continue; - } joint.prev_maintenance_req = curr_maintenance_req; } @@ -590,51 +740,7 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( can_tx_frame_.id = joint.node_write_id; can_tx_frame_.dlc = 8; - if (joint.control_level == integration_level_t::POSITION && std::isfinite(joint.joint_command_position) && - joint.joint_command_position != joint.prev_joint_command_position) { - // CALCULATE DESIRED JOINT ANGLE - joint_angle = joint.orientation * calculate_motor_position_from_desired_joint_position( - joint.joint_command_position, joint.gear_ratio); - - // ENCODING CAN MESSAGE - data[0] = static_cast(ControlCommands::ABSOLUTE_POS_CONTROL_CMD); - data[1] = 0x00; - data[2] = joint.operating_velocity & 0xFF; - data[3] = (joint.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; - - 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) { - // CALCULATE DESIRED JOINT VELOCITY - joint_velocity = joint.orientation * calculate_motor_velocity_from_desired_joint_velocity( - joint.joint_command_velocity, joint.gear_ratio); - - // ENCODING CAN MESSAGE - data[0] = static_cast(ControlCommands::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; - - joint.prev_joint_command_velocity = joint.joint_command_velocity; - } else { - // No new control command to send; send status poll instead - data[0] = static_cast(StatusCommands::MOTOR_STATUS_2_CMD); - // RCLCPP_WARN(rclcpp::get_logger("RMDHardwareInterface"), "Joint command value not found or undefined command state. Sending Motor Status 2 command for joint '%s'", joint.name.c_str()); - } - - // 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_); } } diff --git a/src/msgs/srv/MaintenanceReq.srv b/src/msgs/srv/MaintenanceReq.srv index 201bae95..9658f79c 100644 --- a/src/msgs/srv/MaintenanceReq.srv +++ b/src/msgs/srv/MaintenanceReq.srv @@ -5,6 +5,11 @@ 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/subsystems/general/general_controllers/config/motor_status_controller.yaml b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml index 3045a06a..dc6f8a86 100644 --- a/src/subsystems/general/general_controllers/config/motor_status_controller.yaml +++ b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml @@ -6,7 +6,7 @@ motor_status_controller: } command_interfaces: { type: string_array, - default_value: ["status_request", "maintenance_request", "maintenance_cmd_id"], + 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." } state_interfaces: { 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 index 31653578..5df4ede7 100644 --- 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 @@ -77,8 +77,11 @@ class MotorStatusController : public controller_interface::ControllerInterface int status_request_rate; std::string maintenance_req_joint_name; int maintenance_request_rate; - uint8_t maintenance_cmd_id; - bool one_shot_sent; + 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_; @@ -89,8 +92,56 @@ class MotorStatusController : public controller_interface::ControllerInterface // Publish rate limiting rclcpp::Duration publish_period_{0, 0}; rclcpp::Time last_publish_time_{0, 0, RCL_CLOCK_UNINITIALIZED}; - rclcpp::Time one_shot_time{0, 0, RCL_CLOCK_UNINITIALIZED}; - rclcpp::Duration one_shot_delay{0, 500000000}; + 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}; + + 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, diff --git a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp index 59fc0f7a..b480436c 100644 --- a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp +++ b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp @@ -39,7 +39,7 @@ controller_interface::CallbackReturn MotorStatusController::on_init() status_request_rate = 0; maintenance_req_joint_name = ""; maintenance_request_rate = 0; - one_shot_sent = false; + status_one_shot_sent = false; return controller_interface::CallbackReturn::SUCCESS; } @@ -138,9 +138,42 @@ controller_interface::CallbackReturn MotorStatusController::on_configure( maintenance_req_joint_name = request->joint_name; maintenance_request_rate = request->request_rate; - maintenance_cmd_id = request->command_id; - std::string msg = "Received maintenance_request " + std::to_string(maintenance_cmd_id) + " for joint " + maintenance_req_joint_name + " at request_rate: " + std::to_string(maintenance_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()); @@ -211,28 +244,50 @@ controller_interface::return_type MotorStatusController::update( std::string key = joint + "/" + iface; auto it = command_interface_map_.find(key); if (it != command_interface_map_.end()) { - if (iface == "maintenance_cmd_id" && joint == maintenance_req_joint_name) { - command_interfaces_[it->second].set_value(maintenance_cmd_id); + if (iface == "maintenance_frame_high" && joint == maintenance_req_joint_name) { + command_interfaces_[it->second].set_value(maintenance_frame_high); + } + else if (iface == "maintenance_frame_low" && joint == maintenance_req_joint_name) { + command_interfaces_[it->second].set_value(maintenance_frame_low); + } + else if(iface == "maintenance_data_count" && joint == maintenance_req_joint_name) { + command_interfaces_[it->second].set_value(maintenance_data_count); } else if (iface == "status_request" && joint == status_req_joint_name) { - if (status_request_rate < 0 && one_shot_sent == false) { + // Controller must turn status request back to 0 if it's a one-shot request + 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(), "One shot sent: ."); - one_shot_sent = true; - one_shot_time = time; + 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 && one_shot_sent == true && (time - one_shot_time) >= one_shot_delay) { + 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); - one_shot_sent = false; - RCLCPP_WARN(get_node()->get_logger(), "One shot reset."); + 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); } } else if (iface == "maintenance_request" && joint == maintenance_req_joint_name) { - command_interfaces_[it->second].set_value(maintenance_request_rate); + // Controller must turn maintenance request back to 0 if it's a one-shot request + 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); + } } } else { RCLCPP_WARN(get_node()->get_logger(), "Command interface %s not found for joint %s", iface.c_str(), joint.c_str()); From b53bdbc8d9cb92c93cb7f96242a0ff40bfd1a987 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Thu, 23 Apr 2026 22:23:25 -0400 Subject: [PATCH 11/37] holy fucking shit it worksgit status Need to implement Torque control and then start doing doing this implementation on the rest of the HWIs --- .../rmd_hardware_interface.hpp | 7 + .../src/rmd_hardware_interface.cpp | 140 ++++++++++++------ src/subsystems/general/README.md | 10 ++ 3 files changed, 111 insertions(+), 46 deletions(-) create mode 100644 src/subsystems/general/README.md 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 a1d968c7..5326b74a 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 @@ -112,6 +112,13 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface 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; 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 9604ba39..7cd85344 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 @@ -77,47 +77,57 @@ void RMDHardwareInterface::logger_function(){ << "\n--- Joint Specific ---"; for (auto & joint : RMDJoints_) { - if(static_cast(joint.control_level) == 1) { - control_mode = "POSITION"; + if(joint.name == "propulsion_fl_joint"){ + if(static_cast(joint.control_level) == 1) { + control_mode = "POSITION"; + } + else if(static_cast(joint.control_level) == 2) { + control_mode = "VELOCITY"; + } + else { + control_mode = "UNDEFINED"; + } + 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: " << 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: " << joint.motor_temperature << " C" + << " | Torque Current: " << joint.motor_torque_current << " A" + << " | Motor Status: : " << joint.motor_status << "\n"; } - else if(static_cast(joint.control_level) == 2) { - control_mode = "VELOCITY"; - } - else { - control_mode = "UNDEFINED"; - } - oss << "\nJOINT: " << 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" - << "-- Commands --\n" - << "Control Mode: " << control_mode << "\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 - << " | 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 - << " | Joint Velocity: " << joint.joint_state_velocity << "\n" - << "-- Telemetry --\n" - << "Motor Temperature: " << joint.motor_temperature << " C" - << " | Torque Current: " << joint.motor_torque_current << " A\n"; - } - log_msg += oss.str(); - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), log_msg.c_str()); + 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) @@ -158,7 +168,7 @@ bool RMDHardwareInterface::process_status(uint16_t status, const rclcpp::Logger // WARN if (has_warning) { - RCLCPP_WARN(logger, "Brake Released (warning state)"); + // RCLCPP_WARN(logger, "Brake Released (warning state)"); } // ERROR @@ -374,6 +384,13 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_init( .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, @@ -538,8 +555,7 @@ void RMDHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { // VELOCITY // uint16 -> int16 -> double (for calcs) joint.motor_velocity = static_cast(static_cast((data[5] << 8) | data[4])); - } - else if(can_rx_frame_.id == joint.node_read_id && + } else if(can_rx_frame_.id == joint.node_read_id && can_rx_frame_.data[0] == static_cast(StatusCommands::MOTOR_STATUS_1_CMD) && can_rx_frame_.data[1] != 0x00) { @@ -563,7 +579,39 @@ void RMDHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { // 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) { // RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Reply not heard."); @@ -689,9 +737,9 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( 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); + uint64_t h = static_cast(high); + uint64_t l = static_cast(low); + return static_cast((h << 32) | l); }; // Call it and store the result diff --git a/src/subsystems/general/README.md b/src/subsystems/general/README.md new file mode 100644 index 00000000..931e2894 --- /dev/null +++ b/src/subsystems/general/README.md @@ -0,0 +1,10 @@ +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/maintenance_request msgs/srv/MaintenanceReq "{ + joint_name: propulsion_fl_joint, + request_rate: -1, + command_id: 0x78, + i32_data: [], + i16_data: [], + u8_data: [] +}" \ No newline at end of file From f3376a2c6cf9825c63d7bac37ea50c22a5346a44 Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Fri, 24 Apr 2026 11:38:24 -0400 Subject: [PATCH 12/37] saving smc progress --- .../rmd_hardware_interface.hpp | 5 - .../smc_hardware_interface.hpp | 232 +++++++++++++++--- 2 files changed, 192 insertions(+), 45 deletions(-) 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 5326b74a..e3998685 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 @@ -189,11 +189,6 @@ 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_; // CAN Commands 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..d78602fb 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 @@ -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 smc_ros2_control { class SMCHardwareInterface : public hardware_interface::SystemInterface // Inheriting from System Interface @@ -31,7 +37,7 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher public: RCLCPP_SHARED_PTR_DEFINITIONS(SMCHardwareInterface) - // Initialization, so reading parameters, initializing variables, checking if all the joint state and command interfaces are correct + // Initialization: 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; @@ -68,6 +74,76 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher 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. The value indicate the bit + // position in the status word where the flag is located. + enum class SMCMotorStatus : uint8_t { + LOW_VOLTAGE = 0, + OVER_TEMPERATURE = 3, + }; + + 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; // Motor temperature in °C + double motor_torque_current; // Motor torque current in A + 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 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); @@ -89,26 +165,6 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher // 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 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_; @@ -116,29 +172,125 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher 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_; + + // CAN Commands + // Maintenance commands: Commands specifically sent via maintenance_request interface + enum class MaintenanceCommands : uint8_t { + WRITE_SETTINGS_TO_RAM_CMD = 0x42, + WRITE_SETTINGS_TO_ROM_CMD = 0x44, + WRITE_ACCELERATION_CMD = 0x43, + MOTOR_SHUTDOWN_CMD = 0X80, + MOTOR_STOP_CMD = 0x81, + MOTOR_RUNNING_CMD = 0x88, + CLEAR_MOTOR_ANGLE_CMD = 0x95, + CLEAR_ERROR_CMD = 0x9B, + }; + + // Control commands: Commands that actuate the motor + enum class ControlCommands : uint8_t { + TORQUE_CONTROL_CMD = 0xA1, + SPEED_CONTROL_CMD = 0xA2, + ABSOLUTE_POS_WITH_VEL_CONTROL_CMD = 0xA4, + }; + + // Status commands: Commands sent to request specific status information from the motor + enum class StatusCommands : uint8_t { + READ_SETTINGS_CMD = 0x40, + READ_ENCODER_CMD = 0x90, + MOTOR_STATUS_1_CMD = 0x9A, + MOTOR_STATUS_2_CMD = 0X9C, + }; + + // Parameter table for various settings + 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, + } + + // 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 DecodedCommand unpack_command_full(int32_t counts_in, int64_t payload_in) + { + uint32_t counts = static_cast(counts_in); + uint64_t payload = static_cast(payload_in); + + // Counts are packed in the top 24 bits of a 32-bit value + uint8_t u8_count = (counts >> 16) & 0xFF; + uint8_t i16_count = (counts >> 8) & 0xFF; + uint8_t i32_count = (counts >> 0) & 0xFF; + + int bit_offset = 64; + auto pop_bits = [&](int bits) -> uint64_t { + bit_offset -= bits; + uint64_t mask = (1ULL << bits) - 1; + 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(); + + // 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) + ); + } - // 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; }; } // namespace smc_hardware_interface From 986d2436fcdbc3edc99a9e62fbb811005a204645 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Fri, 24 Apr 2026 19:32:57 -0400 Subject: [PATCH 13/37] im sorry to whoever is reading this --- .../arm/arm.servo.ros2_control.xacro | 3 + .../arm/arm.smc_2dof.ros2_control.xacro | 9 + .../arm/arm.talon_2dof.ros2_control.xacro | 4 + .../drive/drive.killswitch.ros2_control.xacro | 2 + .../drive/drive.led.ros2_control.xacro | 2 + .../science/science.laser.ros2_control.xacro | 3 +- .../science/science.servo.ros2_control.xacro | 21 + .../science.stepper.ros2_control.xacro | 14 + .../science/science.talon.ros2_control.xacro | 4 + .../killswitch_hardware_interface.hpp | 94 +- .../src/killswitch_hardware_interface.cpp | 268 ++--- .../laser_hardware_interface.hpp | 87 +- .../src/laser_hardware_interface.cpp | 254 ++--- .../led_hardware_interface.hpp | 76 +- .../src/led_hardware_interface.cpp | 250 ++--- .../src/rmd_hardware_interface.cpp | 152 +-- .../servo_hardware_interface.hpp | 267 +++-- .../src/servo_hardware_interface.cpp | 944 +++++++++--------- .../smc_hardware_interface.hpp | 198 ++-- .../src/smc_hardware_interface.cpp | 827 ++++++++------- .../stepper_hardware_interface.hpp | 213 ++-- .../src/stepper_hardware_interface.cpp | 830 +++++++-------- .../talon_hardware_interface.hpp | 114 +-- .../src/talon_hardware_interface.cpp | 488 +++++---- 24 files changed, 2393 insertions(+), 2731 deletions(-) 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 4dce02ad..0a1fbb7f 100644 --- a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro @@ -22,6 +22,9 @@ + + + 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 45708546..4064c89e 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 @@ -19,6 +19,9 @@ + + + @@ -35,6 +38,9 @@ + + + @@ -51,6 +57,9 @@ + + + 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..b1b1bb7d 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 @@ -32,8 +32,12 @@ 0.0 + + + + diff --git a/src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro b/src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro index 2ee52595..fca01325 100644 --- a/src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.killswitch.ros2_control.xacro @@ -15,9 +15,11 @@ + + 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..09f0d44a 100644 --- a/src/description/ros2_control/drive/drive.led.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.led.ros2_control.xacro @@ -13,7 +13,9 @@ + + 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..4f101acc 100644 --- a/src/description/ros2_control/science/science.laser.ros2_control.xacro +++ b/src/description/ros2_control/science/science.laser.ros2_control.xacro @@ -15,7 +15,9 @@ + + @@ -23,4 +25,3 @@ - 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 e3ae23e9..700361b7 100644 --- a/src/description/ros2_control/science/science.servo.ros2_control.xacro +++ b/src/description/ros2_control/science/science.servo.ros2_control.xacro @@ -22,6 +22,9 @@ + + + @@ -38,6 +41,9 @@ + + + @@ -53,6 +59,9 @@ + + + @@ -68,6 +77,9 @@ + + + @@ -84,6 +96,9 @@ + + + @@ -100,6 +115,9 @@ + + + @@ -116,6 +134,9 @@ + + + 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..4c65551c 100644 --- a/src/description/ros2_control/science/science.stepper.ros2_control.xacro +++ b/src/description/ros2_control/science/science.stepper.ros2_control.xacro @@ -12,11 +12,18 @@ + + + + + 0.0 + + 0x151 1 @@ -26,11 +33,18 @@ + + + + + 0.0 + + 0x152 1 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/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 index 4636862b..690cf49c 100644 --- 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 @@ -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 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" @@ -29,38 +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 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: @@ -68,59 +25,52 @@ class KillswitchHardwareInterface : 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; private: - // CAN message handler - void onCanMessage(const CANLib::CanFrame& frame); + struct KillswitchJoint + { + std::string name; + uint32_t can_id; + double kill_all_sent; + double kill_main_sent; + double kill_jetson_sent; + double is_connected; + double status; + double cmd_kill_all; + double cmd_kill_main; + double cmd_kill_jetson; + 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 KILLSWITCHJoints_; - // 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; 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 index 3d335e8b..8d85cb55 100644 --- a/src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp +++ b/src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp @@ -1,21 +1,6 @@ -// 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 "pluginlib/class_list_macros.hpp" #include "rclcpp/rclcpp.hpp" namespace killswitch_ros2_control @@ -24,263 +9,186 @@ 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) + 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_interface_ = info_.hardware_parameters.count("can_interface") ? + info_.hardware_parameters.at("can_interface") : "can0"; + const uint32_t can_id = info_.hardware_parameters.count("can_id") ? + static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)) : 0x100; + + KILLSWITCHJoints_.clear(); + KILLSWITCHJoints_.push_back(KillswitchJoint{ + info_.gpios[0].name, + can_id, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + }); 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*/) + const rclcpp_lifecycle::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))) + 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"); - + auto & joint = KILLSWITCHJoints_.front(); + joint.is_connected = can_connected_ ? 1.0 : 0.0; + joint.status = joint.is_connected; return hardware_interface::CallbackReturn::SUCCESS; } -void KillswitchHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) +void KillswitchHardwareInterface::onCanMessage(const CANLib::CanFrame &) { - // Handle incoming CAN messages from voltage monitoring board if needed - (void)frame; // Currently not expecting responses } -std::vector -KillswitchHardwareInterface::export_state_interfaces() +std::vector KillswitchHardwareInterface::export_state_interfaces() { + auto & joint = KILLSWITCHJoints_.front(); 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()); - + state_interfaces.emplace_back(joint.name, "kill_all_sent", &joint.kill_all_sent); + state_interfaces.emplace_back(joint.name, "kill_main_sent", &joint.kill_main_sent); + state_interfaces.emplace_back(joint.name, "kill_jetson_sent", &joint.kill_jetson_sent); + 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 -KillswitchHardwareInterface::export_command_interfaces() +std::vector KillswitchHardwareInterface::export_command_interfaces() { + auto & joint = KILLSWITCHJoints_.front(); 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()); - + command_interfaces.emplace_back(joint.name, "kill_all", &joint.cmd_kill_all); + command_interfaces.emplace_back(joint.name, "kill_main", &joint.cmd_kill_main); + command_interfaces.emplace_back(joint.name, "kill_jetson", &joint.cmd_kill_jetson); + command_interfaces.emplace_back(joint.name, "status_request", &joint.status_request); return command_interfaces; } hardware_interface::CallbackReturn KillswitchHardwareInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::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; - + auto & joint = KILLSWITCHJoints_.front(); + joint.cmd_kill_all = 0.0; + joint.cmd_kill_main = 0.0; + joint.cmd_kill_jetson = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn KillswitchHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::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*/) + const rclcpp_lifecycle::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"); - + auto & joint = KILLSWITCHJoints_.front(); + joint.is_connected = 0.0; + joint.status = 0.0; 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*/) + const rclcpp::Time &, const rclcpp::Duration &) { - // 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*/) + const rclcpp::Time &, const rclcpp::Duration & period) { - // Check kill_all command (rising edge detection) - if (cmd_kill_all_ > 0.5 && kill_all_sent_ < 0.5) { + auto & joint = KILLSWITCHJoints_.front(); + + if (joint.cmd_kill_all > 0.5 && joint.kill_all_sent < 0.5) { 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_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 + joint.kill_all_sent = 1.0; + joint.status = 1.0; + } else if (joint.cmd_kill_all < 0.5) { + joint.kill_all_sent = 0.0; } - // Check kill_main command (rising edge detection) - if (cmd_kill_main_ > 0.5 && kill_main_sent_ < 0.5) { + if (joint.cmd_kill_main > 0.5 && joint.kill_main_sent < 0.5) { 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_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 + joint.kill_main_sent = 1.0; + joint.status = 1.0; + } else if (joint.cmd_kill_main < 0.5) { + joint.kill_main_sent = 0.0; } - // Check kill_jetson command (rising edge detection) - if (cmd_kill_jetson_ > 0.5 && kill_jetson_sent_ < 0.5) { + if (joint.cmd_kill_jetson > 0.5 && joint.kill_jetson_sent < 0.5) { 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_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 + joint.kill_jetson_sent = 1.0; + joint.status = 1.0; + } else if (joint.cmd_kill_jetson < 0.5) { + joint.kill_jetson_sent = 0.0; } + if (joint.status_request < 0.0 && joint.prev_status_request >= 0.0) { + joint.status = can_connected_ ? 1.0 : 0.0; + } 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; + joint.status = can_connected_ ? 1.0 : 0.0; + } + } + joint.prev_status_request = joint.status_request; + 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..a3449ab7 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,52 @@ 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; 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_; - // 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..cdcc97dd 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,21 +1,6 @@ -// 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" namespace laser_ros2_control @@ -24,250 +9,177 @@ namespace laser_ros2_control 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"; + 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_); - 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(); + 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/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..973468c2 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,18 +1,3 @@ -// Copyright (c) 2024 UMD Loop -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// - #ifndef LED_ROS2_CONTROL__LED_HARDWARE_INTERFACE_HPP_ #define LED_ROS2_CONTROL__LED_HARDWARE_INTERFACE_HPP_ @@ -31,30 +16,13 @@ namespace led_ros2_control { -// GPIO utility functions (Linux sysfs) namespace gpio_utils { - int setup_gpio_output(int pin); - void cleanup_gpio(int pin, int fd); - bool write_gpio(int fd, bool value); +int setup_gpio_output(int pin); +void cleanup_gpio(int pin, int fd); +bool write_gpio(int fd, bool value); } -/** - * @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") - */ class LEDHardwareInterface : public hardware_interface::SystemInterface { public: @@ -62,52 +30,46 @@ 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; + 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 + struct LEDJoint + { + std::string name; + int gpio_pin; + bool default_state; + double led_state; + double is_connected; + double status; + double led_command; + double status_request; + double prev_status_request; + double elapsed_status_request_time; + }; - // Command variables (ros2_control → hardware) - double led_command_; // Commanded state - - // Hardware interface int gpio_fd_; bool hw_connected_; + std::vector LEDJoints_; }; } // namespace led_ros2_control #endif // LED_ROS2_CONTROL__LED_HARDWARE_INTERFACE_HPP_ - 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..a4741bc2 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,46 +1,28 @@ -// 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" 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); + const 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 + usleep(100000); } - // 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); @@ -50,12 +32,9 @@ int setup_gpio_output(int pin) ::write(fd, "out", 3); ::close(fd); - // 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; + return ::open(value_path.str().c_str(), O_RDWR); } void cleanup_gpio(int pin, int fd) @@ -64,10 +43,9 @@ void cleanup_gpio(int pin, int fd) ::close(fd); } - // Unexport GPIO int export_fd = ::open("/sys/class/gpio/unexport", O_WRONLY); if (export_fd >= 0) { - std::string pin_str = std::to_string(pin); + const std::string pin_str = std::to_string(pin); ::write(export_fd, pin_str.c_str(), pin_str.length()); ::close(export_fd); } @@ -75,241 +53,159 @@ void cleanup_gpio(int pin, int fd) bool write_gpio(int fd, bool value) { - if (fd < 0) return false; - - char val = value ? '1' : '0'; + if (fd < 0) { + return false; + } + const 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"); + 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"); - } else { - default_state_ = false; // OFF by default - } - - // Initialize state variables - led_state_ = default_state_ ? 1.0 : 0.0; - is_connected_ = 0.0; - - // Initialize command variables - led_command_ = default_state_ ? 1.0 : 0.0; + const int gpio_pin = std::stoi(info_.hardware_parameters.at("gpio_pin")); + const bool default_state = info_.hardware_parameters.count("default_state") && + info_.hardware_parameters.at("default_state") == "on"; + + LEDJoints_.clear(); + LEDJoints_.push_back(LEDJoint{ + info_.gpios[0].name, + gpio_pin, + default_state, + default_state ? 1.0 : 0.0, + 0.0, + 0.0, + default_state ? 1.0 : 0.0, + 0.0, + 0.0, + 0.0 + }); gpio_fd_ = -1; hw_connected_ = false; - - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "Initialized LED on GPIO pin %d (default: %s)", - gpio_pin_, default_state_ ? "ON" : "OFF"); - 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)"); + auto & joint = LEDJoints_.front(); + gpio_fd_ = gpio_utils::setup_gpio_output(joint.gpio_pin); + hw_connected_ = gpio_fd_ >= 0; + if (hw_connected_) { + gpio_utils::write_gpio(gpio_fd_, joint.default_state); } - - is_connected_ = hw_connected_ ? 1.0 : 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "LED hardware configured (%s)", hw_connected_ ? "REAL GPIO" : "SIMULATION"); - + joint.is_connected = hw_connected_ ? 1.0 : 0.0; + joint.status = joint.is_connected; return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -LEDHardwareInterface::export_state_interfaces() +std::vector LEDHardwareInterface::export_state_interfaces() { + auto & joint = LEDJoints_.front(); 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()); - + state_interfaces.emplace_back(joint.name, "led_state", &joint.led_state); + 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 -LEDHardwareInterface::export_command_interfaces() +std::vector LEDHardwareInterface::export_command_interfaces() { + auto & joint = LEDJoints_.front(); 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()); - + command_interfaces.emplace_back(joint.name, "led_command", &joint.led_command); + command_interfaces.emplace_back(joint.name, "status_request", &joint.status_request); 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) + auto & joint = LEDJoints_.front(); 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)"); } - + joint.led_state = 0.0; 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_utils::cleanup_gpio(LEDJoints_.front().gpio_pin, gpio_fd_); gpio_fd_ = -1; } - hw_connected_ = false; - is_connected_ = 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("LEDHardwareInterface"), - "LED hardware cleanup complete"); - + auto & joint = LEDJoints_.front(); + joint.is_connected = 0.0; + joint.status = 0.0; 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); + auto & joint = LEDJoints_.front(); + const bool commanded_on = joint.led_command > 0.5; + const bool currently_on = joint.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); } - - 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)"); + joint.led_state = commanded_on ? 1.0 : 0.0; + } + + if (joint.status_request < 0.0 && joint.prev_status_request >= 0.0) { + joint.status = hw_connected_ ? 1.0 : 0.0; + } 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; + joint.status = hw_connected_ ? 1.0 : 0.0; + } } + joint.prev_status_request = joint.status_request; return hardware_interface::return_type::OK; } } // 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/rmd_ros2_control/src/rmd_hardware_interface.cpp b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp index 7cd85344..5e3a53fc 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 @@ -60,7 +60,7 @@ 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"; @@ -77,53 +77,51 @@ void RMDHardwareInterface::logger_function(){ << "\n--- Joint Specific ---"; for (auto & joint : RMDJoints_) { - if(joint.name == "propulsion_fl_joint"){ - if(static_cast(joint.control_level) == 1) { - control_mode = "POSITION"; - } - else if(static_cast(joint.control_level) == 2) { - control_mode = "VELOCITY"; - } - else { - control_mode = "UNDEFINED"; - } - 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: " << 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: " << joint.motor_temperature << " C" - << " | Torque Current: " << joint.motor_torque_current << " A" - << " | Motor Status: : " << joint.motor_status << "\n"; + if(static_cast(joint.control_level) == 1) { + control_mode = "POSITION"; } + else if(static_cast(joint.control_level) == 2) { + control_mode = "VELOCITY"; + } + else { + control_mode = "UNDEFINED"; + } + 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: " << 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: " << 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()); @@ -344,9 +342,22 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_init( // Setting Parameters RMDJoints_.clear(); + + num_joints = static_cast(RMDJoints_.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"); + + // 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); // Collect state interface names std::vector state_if_names; @@ -366,18 +377,15 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_init( params_map.emplace(p.first, p.second); } + // 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 = std::stoi(joint.parameters.at("joint_orientation")) == -1 ? -1 : 1, - .operating_velocity = static_cast(std::clamp( - std::stoi(joint.parameters.at("operating_velocity")), - 0, - static_cast(3.5 * 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, @@ -417,12 +425,7 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_init( ); } - num_joints = static_cast(RMDJoints_.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"); - + // Initialize timing tools elapsed_update_time = 0.0; elapsed_time = 0.0; elapsed_logger_time = 0.0; @@ -526,14 +529,13 @@ void RMDHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { // 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)) && - can_rx_frame_.data[1] != 0x00) { + 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 @@ -556,17 +558,17 @@ void RMDHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { // uint16 -> int16 -> double (for calcs) 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) && - can_rx_frame_.data[1] != 0x00) { + can_rx_frame_.data[0] == static_cast(StatusCommands::MOTOR_STATUS_1_CMD)) { // 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 byted + data[7] = can_rx_frame_.data[7]; // Error state high byte // TEMPERATURE (degrees C) joint.motor_temperature = static_cast(data[1]); @@ -811,14 +813,15 @@ hardware_interface::return_type RMDHardwareInterface::perform_command_mode_switc // 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; } } @@ -826,9 +829,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) { @@ -839,7 +843,7 @@ hardware_interface::return_type RMDHardwareInterface::perform_command_mode_switc // 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 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 d1fd714e..23661d73 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,169 +18,226 @@ #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; + }; - // -- Helper Functions -- - void on_can_message(const CANLib::CanFrame& frame); + struct DecodedCommand + { + uint8_t command_id; + std::vector u8_data; + std::vector i16_data; + std::vector i32_data; + }; + + 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; - - // Modes for control mode - enum class integration_level_t : std::uint8_t - { - UNDEFINED = 0, - POSITION = 1, - VELOCITY = 2, - }; + std::vector SERVOJoints_; - // Active control mode for each actuator - std::vector control_level_; + static constexpr uint8_t PCB_HEARTBEAT_CMD = 0x10; + static constexpr uint8_t ABSOLUTE_POS_CONTROL_CMD = 0x20; + static constexpr uint8_t VELOCITY_CONTROL_CMD = 0x30; + static constexpr uint8_t MOTOR_STATE_CMD = 0x40; + static constexpr uint8_t MOTOR_STATUS_CMD = 0x50; + static constexpr uint8_t MAINTENANCE_CMD = 0x60; + static constexpr uint8_t SERVO_SPECS_CMD = 0x70; - enum class servo_type_t : std::uint8_t + enum class MaintenanceCommands : uint8_t { - STANDARD = 0, - CONTINUOUS = 1, + MOTOR_STOP_CMD = 0x01, + MOTOR_SHUTDOWN_CMD = 0x02, }; - // Type of servo for each actuator - std::vector servo_type_; - - // Types of joints - enum class joint_type_t : std::uint8_t + enum class StatusCommands : uint8_t { - REVOLUTE = 0, - PRISMATIC = 1, + MOTOR_STATE = 0x01, + MOTOR_STATUS = 0x02, + SERVO_SPECS = 0x03, }; - // Type of joint for each actuator - std::vector joint_type_; - - // Multiplexor values - static constexpr uint8_t PCB_HEARTBEAT_CMD = 0X10; - static constexpr uint8_t LED_STATUS_CMD = 0x11; - static constexpr uint8_t ABSOLUTE_POS_CONTROL_CMD = 0x20; - static constexpr uint8_t VELOCITY_CONTROL_CMD = 0x30; - static constexpr uint8_t MOTOR_STATE_CMD = 0x40; - static constexpr uint8_t MOTOR_STATUS_CMD = 0x50; - static constexpr uint8_t MAINTENANCE_CMD = 0x60; - static constexpr uint8_t SERVO_SPECS_CMD = 0x70; + static constexpr std::array kStatusCommands = { + StatusCommands::MOTOR_STATE, + StatusCommands::MOTOR_STATUS, + StatusCommands::SERVO_SPECS, + }; - // Multiplexor Nibbles - int8_t motor_state_nibble; - int8_t motor_status_nibble; - int8_t maintenance_nibble; - int8_t servo_specs_nibble; + 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 1b2a6a38..c7862860 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,608 +1,588 @@ #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.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(ABSOLUTE_POS_CONTROL_CMD + joint.node_id); + frame.data[1] = static_cast(joint_angle & 0xFF); + frame.data[2] = static_cast((joint_angle >> 8) & 0xFF); + joint.prev_joint_command_position = joint.joint_command_position; + return; + } + + 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.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(VELOCITY_CONTROL_CMD + joint.node_id); + frame.data[1] = static_cast(joint_velocity & 0xFF); + frame.data[2] = static_cast((joint_velocity >> 8) & 0xFF); + joint.prev_joint_command_velocity = joint.joint_command_velocity; + return; + } + + frame.dlc = 1; + frame.data[0] = static_cast(MOTOR_STATE_CMD + joint.node_id); +} + +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 = 1; + switch (static_cast(command_id)) { + case StatusCommands::MOTOR_STATE: + frame.data[0] = static_cast(MOTOR_STATE_CMD + node_id); + return true; + case StatusCommands::MOTOR_STATUS: + frame.data[0] = static_cast(MOTOR_STATUS_CMD + node_id); + return true; + case StatusCommands::SERVO_SPECS: + frame.data[0] = static_cast(SERVO_SPECS_CMD + node_id); + 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(MAINTENANCE_CMD + node_id); + frame.data[1] = decoded_cmd.command_id; + + switch (static_cast(decoded_cmd.command_id)) { + case MaintenanceCommands::MOTOR_STOP_CMD: + case MaintenanceCommands::MOTOR_SHUTDOWN_CMD: + 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; - } - } - - // Command Nibble of Multiplexor ID - motor_state_nibble = (MOTOR_STATE_CMD >> 4) & 0x0F; - motor_status_nibble = (MOTOR_STATUS_CMD >> 4) & 0x0F; - maintenance_nibble = (MAINTENANCE_CMD >> 4) & 0x0F; - 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_nibble && 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()); - } - } - else if(can_rx_frame_.id == can_response_id && command_nibble == motor_status_nibble && device_id_nibble == joint_node_ids[i]){ - - // Populate device status - device_status[i] = can_rx_frame_.data[1]; +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; } - else{ - if(logger_state == 1) { - // RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Reply not heard."); + + if (command_nibble == (MOTOR_STATE_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; } - } + if (command_nibble == (MOTOR_STATUS_CMD >> 4)) { + joint.motor_status = static_cast(frame.data[1]); + 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_nibble; - 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::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_nibble; - 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::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; - elapsed_time+=period.seconds(); - - int data[3] = {0x00}; - int8_t command_nibble; - int8_t device_id_nibble; - int16_t joint_angle = 0; - int16_t joint_velocity = 0; - - // 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_nibble; // 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) { @@ -611,54 +591,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 d78602fb..476ddb6a 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,12 +2,14 @@ #define SMC_HARDWARE_INTERACE_HPP_ #include + +#include +#include +#include #include #include -#include -#include #include - +#include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -17,64 +19,48 @@ #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 +namespace CANLib { - struct CanFrame; +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: 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 Functions -- 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; - - // Modes for control mode enum class integration_level_t : std::uint8_t { UNDEFINED = 0, @@ -82,11 +68,11 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher VELOCITY = 2, }; - // Specific motor status flags. The value indicate the bit - // position in the status word where the flag is located. - enum class SMCMotorStatus : uint8_t { - LOW_VOLTAGE = 0, - OVER_TEMPERATURE = 3, + enum class SMCMotorStatus : std::uint8_t + { + OK = 0x00, + LOW_VOLTAGE = 0x01, + OVER_TEMPERATURE = 0x08, }; struct SMCJoint @@ -100,15 +86,18 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher double joint_state_position; double joint_state_velocity; - double motor_temperature; // Motor temperature in °C - double motor_torque_current; // Motor torque current in A + 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 joint_command_position; @@ -144,66 +133,65 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher 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 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 + double elapsed_update_time; + double elapsed_time; + double elapsed_logger_time; + int logger_rate; + int logger_state; int num_joints; - // 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 SMCJoints_; - // CAN Commands - // Maintenance commands: Commands specifically sent via maintenance_request interface - enum class MaintenanceCommands : uint8_t { + enum class MaintenanceCommands : uint8_t + { + WRITE_ACCELERATION_CMD = 0x34, WRITE_SETTINGS_TO_RAM_CMD = 0x42, WRITE_SETTINGS_TO_ROM_CMD = 0x44, - WRITE_ACCELERATION_CMD = 0x43, - MOTOR_SHUTDOWN_CMD = 0X80, + MOTOR_SHUTDOWN_CMD = 0x80, MOTOR_STOP_CMD = 0x81, MOTOR_RUNNING_CMD = 0x88, CLEAR_MOTOR_ANGLE_CMD = 0x95, CLEAR_ERROR_CMD = 0x9B, }; - // Control commands: Commands that actuate the motor - enum class ControlCommands : uint8_t { + enum class ControlCommands : uint8_t + { TORQUE_CONTROL_CMD = 0xA1, SPEED_CONTROL_CMD = 0xA2, ABSOLUTE_POS_WITH_VEL_CONTROL_CMD = 0xA4, }; - // Status commands: Commands sent to request specific status information from the motor - enum class StatusCommands : uint8_t { + enum class StatusCommands : uint8_t + { + READ_ACCELERATION_CMD = 0x33, READ_SETTINGS_CMD = 0x40, READ_ENCODER_CMD = 0x90, + READ_ABS_ANGLE_CMD = 0x92, MOTOR_STATUS_1_CMD = 0x9A, - MOTOR_STATUS_2_CMD = 0X9C, + MOTOR_STATUS_2_CMD = 0x9C, }; - // Parameter table for various settings - enum class ParamID : uint8_t { + enum class ParamID : uint8_t + { ANGLE_PID = 0x96, SPEED_PID = 0x97, CURRENT_PID = 0x98, @@ -213,86 +201,72 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher MAX_ANGLE_B = 0x9C, CURRENT_RAMP = 0x9D, SPEED_RAMP = 0x9E, - } + }; - // General motor status (can be interpreted by controller) - enum class MotorStatus : uint8_t - { - UNKNOWN = 0, - IDLE = 1, - ACTIVE = 2, - WARNING = 3, - ERROR = 4, - DISABLED = 5 + static constexpr std::array kStatusCommands = { + StatusCommands::READ_ACCELERATION_CMD, + StatusCommands::READ_SETTINGS_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) { - uint32_t counts = static_cast(counts_in); - uint64_t payload = static_cast(payload_in); + const uint32_t counts = static_cast(counts_in); + const uint64_t payload = static_cast(payload_in); - // Counts are packed in the top 24 bits of a 32-bit value - uint8_t u8_count = (counts >> 16) & 0xFF; - uint8_t i16_count = (counts >> 8) & 0xFF; - uint8_t i32_count = (counts >> 0) & 0xFF; + 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; - uint64_t mask = (1ULL << bits) - 1; - return (payload >> bit_offset) & mask; + bit_offset -= bits; + const uint64_t mask = (bits == 64) ? std::numeric_limits::max() : ((1ULL << bits) - 1ULL); + return (payload >> bit_offset) & mask; }; - DecodedCommand result; + 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)))); - + 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( - auto & joint, - const DecodedCommand & decoded_maintenance_cmd) + JointT & joint, const DecodedCommand & decoded_maintenance_cmd) { joint.decoded_maintenance_frame.clear(); - - // 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.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) - ); - - // i32_data → raw bytes - const uint8_t* i32_ptr = reinterpret_cast( - decoded_maintenance_cmd.i32_data.data() - ); + 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) - ); + joint.decoded_maintenance_frame.end(), + i32_ptr, + i32_ptr + decoded_maintenance_cmd.i32_data.size() * sizeof(int32_t)); } - }; -} // namespace smc_hardware_interface +} // namespace smc_ros2_control -#endif // SMC_HARDWARE_INTERACE_HPP_ \ No newline at end of file +#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 aa010d20..b2a16b54 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,457 +1,617 @@ #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; +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() +{ + if (num_joints == 0) { + return; + } - // Building Message - std::string log_msg = "\033[2J\033[H \nSMC Logger"; - std::string control_mode = ""; - - // HWI Specific std::ostringstream oss; - - oss << "\n--- HWI Specific ---\n" + oss << "\033[2J\033[H \nSMC 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) { + for (const auto & joint : SMCJoints_) { + std::string control_mode = "UNDEFINED"; + 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 { - 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" + + oss << "\nJOINT: " << joint.name << "\n" + << "Parameters: CAN ID: 0x" << std::hex << std::uppercase << joint.node_id << std::dec + << " | Gear Ratio: " << joint.gear_ratio << "\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" + << "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" - << "-- Telemetry --\n" - << "Motor Temperature: " << motor_temperature_[i] << " C" - << " | Torque Current: " << motor_torque_current_[i] << " A\n"; + << "Joint Position: " << joint.joint_state_position + << " | Joint Velocity: " << joint.joint_state_velocity << "\n" + << "Motor Temperature: " << joint.motor_temperature + << " | Torque Current: " << joint.motor_torque_current + << " | Status: " << joint.motor_status << "\n"; } - log_msg += oss.str(); - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), log_msg.c_str()); + RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "%s", oss.str().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; + 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; + } + + 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; + } + + frame.data[0] = static_cast(StatusCommands::MOTOR_STATUS_2_CMD); +} + +bool SMCHardwareInterface::format_status_command(CANLib::CanFrame & frame, uint8_t command_id) +{ + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); + frame.data[0] = command_id; + switch (static_cast(command_id)) { + case StatusCommands::READ_ACCELERATION_CMD: + case StatusCommands::READ_SETTINGS_CMD: + case StatusCommands::READ_ENCODER_CMD: + case StatusCommands::READ_ABS_ANGLE_CMD: + case StatusCommands::MOTOR_STATUS_1_CMD: + case StatusCommands::MOTOR_STATUS_2_CMD: + return true; + default: + return false; + } +} + +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; + + switch (static_cast(decoded_cmd.command_id)) { + case MaintenanceCommands::WRITE_ACCELERATION_CMD: + case MaintenanceCommands::WRITE_SETTINGS_TO_RAM_CMD: + case MaintenanceCommands::WRITE_SETTINGS_TO_ROM_CMD: + case MaintenanceCommands::MOTOR_SHUTDOWN_CMD: + case MaintenanceCommands::MOTOR_STOP_CMD: + case MaintenanceCommands::MOTOR_RUNNING_CMD: + case MaintenanceCommands::CLEAR_MOTOR_ANGLE_CMD: + case MaintenanceCommands::CLEAR_ERROR_CMD: + break; + 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"); - 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); + 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_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); - joint_command_velocity_.assign(num_joints, 0); + std::vector command_if_names; + for (const auto & ci : joint.command_interfaces) { + command_if_names.push_back(ci.name); + } - encoder_position.assign(num_joints, 0.0); - motor_position.assign(num_joints, 0.0); - motor_velocity.assign(num_joints, 0.0); + std::unordered_map params_map; + for (const auto & p : joint.parameters) { + params_map.emplace(p.first, p.second); + } - motor_temperature_.assign(num_joints, 0.0); - motor_torque_current_.assign(num_joints, 0.0); + 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); + + SMCJoint smc_joint{}; + smc_joint.name = joint.name; + smc_joint.node_id = static_cast(node_id); + smc_joint.gear_ratio = gear_ratio; + smc_joint.orientation = orientation; + smc_joint.operating_velocity = static_cast(operating_velocity); + smc_joint.control_level = integration_level_t::POSITION; + smc_joint.joint_state_position = std::numeric_limits::quiet_NaN(); + smc_joint.joint_state_velocity = 0.0; + smc_joint.motor_temperature = std::numeric_limits::quiet_NaN(); + smc_joint.motor_torque_current = std::numeric_limits::quiet_NaN(); + smc_joint.motor_status = 0.0; + smc_joint.current_Kp = std::numeric_limits::quiet_NaN(); + smc_joint.current_Ki = std::numeric_limits::quiet_NaN(); + smc_joint.current_Kd = std::numeric_limits::quiet_NaN(); + smc_joint.speed_Kp = std::numeric_limits::quiet_NaN(); + smc_joint.speed_Ki = std::numeric_limits::quiet_NaN(); + smc_joint.speed_Kd = std::numeric_limits::quiet_NaN(); + smc_joint.position_Kp = std::numeric_limits::quiet_NaN(); + smc_joint.position_Ki = std::numeric_limits::quiet_NaN(); + smc_joint.position_Kd = std::numeric_limits::quiet_NaN(); + smc_joint.acceleration = std::numeric_limits::quiet_NaN(); + smc_joint.joint_command_position = std::numeric_limits::quiet_NaN(); + smc_joint.joint_command_velocity = 0.0; + smc_joint.motor_status_req = 0.0; + smc_joint.motor_maintenance_req = 0.0; + smc_joint.maintenance_frame_high = 0.0; + smc_joint.maintenance_frame_low = 0.0; + smc_joint.maintenance_frame = 0.0; + smc_joint.maintenance_data_count = 0.0; + smc_joint.prev_status_req = 0.0; + smc_joint.prev_maintenance_req = 0.0; + smc_joint.elapsed_status_request_time = 0.0; + smc_joint.elapsed_maintenance_request_time = 0.0; + smc_joint.motor_velocity = 0.0; + smc_joint.motor_position = 0.0; + smc_joint.encoder_position = 0.0; + smc_joint.prev_joint_command_position = std::numeric_limits::quiet_NaN(); + smc_joint.prev_joint_command_velocity = std::numeric_limits::quiet_NaN(); + smc_joint.state_interface_names = state_if_names; + smc_joint.command_interface_names = command_if_names; + smc_joint.parameters = params_map; + SMCJoints_.push_back(std::move(smc_joint)); + } - control_level_.resize(num_joints, integration_level_t::POSITION); + num_joints = static_cast(SMCJoints_.size()); + elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; 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; + 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; + } - // 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])); + if (value == nullptr) { + 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 (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; + } - 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) { + 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 +void SMCHardwareInterface::on_can_message(const CANLib::CanFrame & frame) +{ + can_rx_frame_ = frame; + for (auto & joint : SMCJoints_) { + if (frame.id != joint.node_id) { + continue; + } - // ENCODER POSITION - // uint16 -> int16 -> double (for calcs) - encoder_position[i] = static_cast(static_cast((data[7] << 8) | data[6])); + 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; + } - // SPEED - // uint16 -> int16 -> double (for calcs) - motor_velocity[i] = static_cast(static_cast((data[5] << 8) | data[4])); + 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; + } - // TEMPERATURE (1 byte, degrees C) - motor_temperature_[i] = static_cast(data[1]); + 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; + } - // 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::READ_SETTINGS_CMD)) { + const std::array data = frame.data; + interpret_settings_parameters(joint, data); + 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."); - } - } } - } 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) { - int data[8] = {0x00}; - int32_t joint_angle = 0; - int32_t joint_velocity = 0; - - // Logger update - elapsed_update_time+=period.seconds(); - double update_period = 1.0/update_rate; - elapsed_time+=period.seconds(); - 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; - 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; + 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) { + CANLib::CanFrame frame; + frame.id = joint.node_id; + frame.dlc = 8; + if (format_status_command(frame, static_cast(status_cmd))) { + canBus.send(frame); + } } - 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; + } 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 = joint.node_id; + frame.dlc = 8; + if (format_status_command(frame, static_cast(status_cmd))) { + canBus.send(frame); + } + } } + } + joint.prev_status_req = curr_status_req; + } + + 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)) { + joint.prev_maintenance_req = joint.motor_maintenance_req; + continue; + } - // 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]; + 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); } - canBus.send(can_tx_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; + 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) { @@ -460,53 +620,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/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..a9cdb3a8 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,147 +1,204 @@ -// 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 -// - #ifndef STEPPER_HARDWARE_INTERFACE_HPP_ #define STEPPER_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 "msgs/msg/cana.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" - -#include -#include -#include -#include "msgs/msg/cana.hpp" - 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 + enum class integration_level_t : std::uint8_t + { + UNDEFINED = 0, + POSITION = 1, + VELOCITY = 2, + }; + + struct StepperJoint + { + std::string name; + uint16_t node_id; + int gear_ratio; + int orientation; + double initial_position; + integration_level_t control_level; + + double joint_state_position; + double joint_state_velocity; + double motor_temperature; + double motor_torque_current; + double motor_status; + 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_position; + double motor_velocity; + 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; + }; + 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: + void format_control_command(msgs::msg::CANA & frame, StepperJoint & joint); + bool format_status_command(msgs::msg::CANA & frame, uint8_t command_id, uint16_t node_id); + bool format_maintenance_command( + msgs::msg::CANA & frame, uint16_t node_id, const DecodedCommand & decoded_cmd); +private: int num_joints; - - - // EXPERIMENTING - std::vector initial_position_; - - // 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 from CAN responses - std::vector motor_temperature_; // Motor temperature in °C - std::vector motor_torque_current_; // Motor torque current in A - - double encoder_position; - double motor_velocity; - double motor_position; - - std::vector joint_initialization_; + uint16_t current_iteration; rclcpp::Publisher::SharedPtr science_can_publisher_; rclcpp::Subscription::SharedPtr science_can_subscriber_; rclcpp::Node::SharedPtr node_; - uint16_t current_iteration; - - msgs::msg::CANA received_joint_data_; - std::vector joint_gear_ratios; - std::vector joint_orientation; + std::vector STEPPERJoints_; + enum class MaintenanceCommands : uint8_t + { + BRAKE_RELEASE_CMD = 0x77, + BRAKE_LOCK_CMD = 0x78, + MOTOR_SHUTDOWN_CMD = 0x80, + MOTOR_STOP_CMD = 0x81, + }; + enum class ControlCommands : uint8_t + { + SPEED_CONTROL_CMD = 0xA2, + ABSOLUTE_POS_CONTROL_CMD = 0xA4, + }; - enum integration_level_t : std::uint8_t + enum class StatusCommands : uint8_t { - UNDEFINED = 0, - POSITION = 1, - VELOCITY = 2, + READ_MULTI_TURN_ANGLE_CMD = 0x92, + MOTOR_STATUS_2_CMD = 0x9C, }; - // Active control mode for each actuator - std::vector control_level_; + static constexpr std::array kStatusCommands = { + StatusCommands::READ_MULTI_TURN_ANGLE_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); + + 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/src/stepper_hardware_interface.cpp b/src/hardware_interfaces/stepper_ros2_control/src/stepper_hardware_interface.cpp index d12c3d15..862a582f 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,463 @@ -// 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 * 0.01 * (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; +} + +int32_t STEPPERHardwareInterface::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 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) * 100.0) * gear_ratio)); +} + +void STEPPERHardwareInterface::format_control_command(msgs::msg::CANA & frame, StepperJoint & joint) +{ + frame.id = joint.node_id; + frame.data.assign(8, 0x00); + if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) + joint.control_level == integration_level_t::POSITION && + std::isfinite(joint.joint_command_position) && + joint.joint_command_position != joint.prev_joint_command_position) { - return hardware_interface::CallbackReturn::ERROR; + 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_CONTROL_CMD); + frame.data[2] = 200; + frame.data[3] = 0x00; + 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; } - - /* - 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"))); + + 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; } - num_joints = static_cast(info_.joints.size()); + frame.data[0] = static_cast(StatusCommands::MOTOR_STATUS_2_CMD); +} - 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]); +bool STEPPERHardwareInterface::format_status_command( + msgs::msg::CANA & frame, uint8_t command_id, uint16_t node_id) +{ + frame.id = node_id; + frame.data.assign(8, 0x00); + switch (static_cast(command_id)) { + case StatusCommands::READ_MULTI_TURN_ANGLE_CMD: + case StatusCommands::MOTOR_STATUS_2_CMD: + frame.data[0] = command_id; + return true; + default: + return false; } - - // 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_maintenance_command( + msgs::msg::CANA & frame, uint16_t node_id, const DecodedCommand & decoded_cmd) +{ + frame.id = node_id; + frame.data.assign(8, 0x00); + frame.data[0] = decoded_cmd.command_id; + switch (static_cast(decoded_cmd.command_id)) { + case MaintenanceCommands::BRAKE_RELEASE_CMD: + case MaintenanceCommands::BRAKE_LOCK_CMD: + case MaintenanceCommands::MOTOR_SHUTDOWN_CMD: + case MaintenanceCommands::MOTOR_STOP_CMD: + 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]); +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; } - motor_temperature_.assign(num_joints, 0.0); - motor_torque_current_.assign(num_joints, 0.0); + STEPPERJoints_.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); + } - encoder_position = 0.0; - motor_position = 0.0; - motor_velocity = 0.0; + std::vector command_if_names; + for (const auto & ci : joint.command_interfaces) { + command_if_names.push_back(ci.name); + } - joint_initialization_.assign(num_joints, false); + 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); + const uint16_t node_id = static_cast( + joint.parameters.count("node_id") ? std::stoi(joint.parameters.at("node_id"), nullptr, 0) : 0); + + StepperJoint stepper_joint{}; + stepper_joint.name = joint.name; + stepper_joint.node_id = node_id; + stepper_joint.gear_ratio = std::abs(std::stoi(joint.parameters.at("gear_ratio"))); + stepper_joint.orientation = joint.parameters.count("joint_orientation") && + std::stoi(joint.parameters.at("joint_orientation")) == -1 ? -1 : 1; + stepper_joint.initial_position = joint.parameters.count("initial_position") ? + std::stod(joint.parameters.at("initial_position")) : 0.0; + stepper_joint.control_level = integration_level_t::POSITION; + stepper_joint.joint_state_position = 0.0; + stepper_joint.joint_state_velocity = 0.0; + stepper_joint.motor_temperature = 0.0; + stepper_joint.motor_torque_current = 0.0; + stepper_joint.motor_status = 0.0; + stepper_joint.acceleration = std::numeric_limits::quiet_NaN(); + stepper_joint.joint_command_position = 0.0; + stepper_joint.joint_command_velocity = 0.0; + stepper_joint.motor_status_req = 0.0; + stepper_joint.motor_maintenance_req = 0.0; + stepper_joint.maintenance_frame_high = 0.0; + stepper_joint.maintenance_frame_low = 0.0; + stepper_joint.maintenance_frame = 0.0; + stepper_joint.maintenance_data_count = 0.0; + stepper_joint.prev_status_req = 0.0; + stepper_joint.prev_maintenance_req = 0.0; + stepper_joint.elapsed_status_request_time = 0.0; + stepper_joint.elapsed_maintenance_request_time = 0.0; + stepper_joint.motor_position = 0.0; + stepper_joint.motor_velocity = 0.0; + stepper_joint.encoder_position = 0.0; + stepper_joint.prev_joint_command_position = std::numeric_limits::quiet_NaN(); + stepper_joint.prev_joint_command_velocity = std::numeric_limits::quiet_NaN(); + stepper_joint.state_interface_names = state_if_names; + stepper_joint.command_interface_names = command_if_names; + stepper_joint.parameters = params_map; + STEPPERJoints_.push_back(std::move(stepper_joint)); + } + num_joints = static_cast(STEPPERJoints_.size()); current_iteration = 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 == "motor_temperature") { + value = &joint.motor_temperature; + } else if (iface == "torque_current") { + value = &joint.motor_torque_current; + } 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) { + 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) { + 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; - } - ); - + "can_rx", rclcpp::QoS(50).reliable(), + [this](const msgs::msg::CANA::SharedPtr message) { + received_joint_data_ = *message; + }); return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn STEPPERHardwareInterface::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++){ - 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); + for (const auto & joint : STEPPERJoints_) { + msgs::msg::CANA frame; + if (format_maintenance_command( + frame, joint.node_id, + DecodedCommand{static_cast(MaintenanceCommands::MOTOR_SHUTDOWN_CMD), {}, {}, {}})) + { + science_can_publisher_->publish(frame); + } + if (format_maintenance_command( + frame, joint.node_id, + DecodedCommand{static_cast(MaintenanceCommands::BRAKE_LOCK_CMD), {}, {}, {}})) + { + science_can_publisher_->publish(frame); + } } - // 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*/) + const rclcpp_lifecycle::State &) { - 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); - } - - 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]); + for (auto & joint : STEPPERJoints_) { + msgs::msg::CANA frame; + if (format_maintenance_command( + frame, joint.node_id, + DecodedCommand{static_cast(MaintenanceCommands::BRAKE_RELEASE_CMD), {}, {}, {}})) + { + science_can_publisher_->publish(frame); + } + joint.joint_command_position = joint.initial_position; + joint.joint_command_velocity = 0.0; } - - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } - hardware_interface::CallbackReturn STEPPERHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::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); + for (const auto & joint : STEPPERJoints_) { + msgs::msg::CANA frame; + if (format_maintenance_command( + frame, joint.node_id, + DecodedCommand{static_cast(MaintenanceCommands::MOTOR_STOP_CMD), {}, {}, {}})) + { + science_can_publisher_->publish(frame); + } } - - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Successfully deactivated all STEPPER motors!"); - return hardware_interface::CallbackReturn::SUCCESS; } -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; -} - -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; -} - -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)); -} - - - hardware_interface::return_type STEPPERHardwareInterface::read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time &, const rclcpp::Duration &) { - 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; } - if (rclcpp::ok()) - { + if (rclcpp::ok()) { rclcpp::spin_some(node_); } - 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}; - } - else if(current_iteration%2 == 1){ - // Command to read motor status 2 - - joint_tx.data = {0x9C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - } - science_can_publisher_->publish(joint_tx); - } - - // 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()); - } 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]); - } - 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."); - } + for (auto & joint : STEPPERJoints_) { + if (received_joint_data_.id != joint.node_id) { + continue; } - 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])); + if (received_joint_data_.data[0] == static_cast(StatusCommands::MOTOR_STATUS_2_CMD)) { + joint.encoder_position = static_cast( + static_cast((received_joint_data_.data[7] << 8) | received_joint_data_.data[6])); + joint.motor_velocity = static_cast( + static_cast((received_joint_data_.data[5] << 8) | received_joint_data_.data[4])); + joint.motor_temperature = static_cast(received_joint_data_.data[1]); + joint.motor_torque_current = static_cast( + static_cast((received_joint_data_.data[3] << 8) | received_joint_data_.data[2])) * 0.01; + joint.motor_status = 1.0; + } else if ( + received_joint_data_.data[0] == static_cast(StatusCommands::READ_MULTI_TURN_ANGLE_CMD)) + { + joint.motor_position = static_cast( + static_cast( + (received_joint_data_.data[7] << 24) | + (received_joint_data_.data[6] << 16) | + (received_joint_data_.data[5] << 8) | + received_joint_data_.data[4])); } + joint.joint_state_position = calculate_joint_position_from_motor_position( + joint.motor_position, joint.gear_ratio); + joint.joint_state_velocity = calculate_joint_velocity_from_motor_velocity( + joint.motor_velocity, joint.gear_ratio); } + return hardware_interface::return_type::OK; } - -hardware_interface::return_type stepper_ros2_control::STEPPERHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +hardware_interface::return_type STEPPERHardwareInterface::write( + const rclcpp::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]); + (void)period; + + 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) { + msgs::msg::CANA frame; + if (format_status_command(frame, static_cast(status_cmd), joint.node_id)) { + science_can_publisher_->publish(frame); + } } - - // 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; - - } - 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]); + } 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) { + msgs::msg::CANA frame; + if (format_status_command(frame, static_cast(status_cmd), joint.node_id)) { + science_can_publisher_->publish(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_status_req = curr_status_req; + } + + 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); + + msgs::msg::CANA frame; + if (!format_maintenance_command(frame, joint.node_id, decoded_maintenance_cmd)) { + joint.prev_maintenance_req = joint.motor_maintenance_req; + continue; } - for(int j = 0; j < 8; j++){ - joint_tx.data.push_back(static_cast(data[j])); + const double curr_maintenance_req = joint.motor_maintenance_req; + if (curr_maintenance_req < 0.0 && joint.prev_maintenance_req >= 0.0) { + science_can_publisher_->publish(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; + science_can_publisher_->publish(frame); + } } - - science_can_publisher_->publish(joint_tx); + joint.prev_maintenance_req = curr_maintenance_req; } - + for (auto & joint : STEPPERJoints_) { + msgs::msg::CANA frame; + format_control_command(frame, joint); + science_can_publisher_->publish(frame); + } 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 +466,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 + stepper_ros2_control::STEPPERHardwareInterface, hardware_interface::SystemInterface) 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) From 87a924e5d878144a66a0778673a9fb23f3626199 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Sat, 25 Apr 2026 17:18:57 -0400 Subject: [PATCH 14/37] saving some progress --- src/description/ros2_control/arm/arm.rmd.ros2_control.xacro | 6 ++++++ .../arm/arm_bringup/config/athena_arm_controllers.yaml | 3 +++ 2 files changed, 9 insertions(+) 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 dd2c900d..c529abb0 100644 --- a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro @@ -19,6 +19,9 @@ + + + @@ -35,6 +38,9 @@ + + + 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 5570d90e..0c6db7b5 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -202,6 +202,9 @@ motor_status_controller: - base_yaw - shoulder_pitch - elbow_pitch + - wrist_pitch + - wrist_roll + - actuator interfaces: - motor_temperature - torque_current From 0bfbf9ff4d1af1570b724e712cb2d65b7c8d26cc Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Sun, 26 Apr 2026 13:42:48 -0400 Subject: [PATCH 15/37] updated smc hwi, issue with servo hwi --- .../rmd_hardware_interface.hpp | 32 ++- .../src/rmd_hardware_interface.cpp | 26 +- .../smc_hardware_interface.hpp | 26 +- .../src/smc_hardware_interface.cpp | 259 +++++++++++------- .../config/athena_arm_controllers.yaml | 3 +- .../src/motor_status_controller.cpp | 89 ++++-- 6 files changed, 286 insertions(+), 149 deletions(-) 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 e3998685..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 @@ -256,27 +256,31 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface inline DecodedCommand unpack_command_full(int32_t counts_in, int64_t payload_in) { - uint32_t counts = static_cast(counts_in); - uint64_t payload = static_cast(payload_in); + const uint32_t counts = static_cast(counts_in); + const uint64_t payload = static_cast(payload_in); - // Counts are packed in the top 24 bits of a 32-bit value - uint8_t u8_count = (counts >> 16) & 0xFF; - uint8_t i16_count = (counts >> 8) & 0xFF; - uint8_t i32_count = (counts >> 0) & 0xFF; + 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; - uint64_t mask = (1ULL << bits) - 1; - return (payload >> bit_offset) & mask; + bit_offset -= bits; + const uint64_t mask = (bits == 64) ? std::numeric_limits::max() : ((1ULL << bits) - 1ULL); + return (payload >> bit_offset) & mask; }; - DecodedCommand result; + 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)))); - + 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; } 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 5e3a53fc..c1dde012 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 @@ -20,13 +20,10 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" - -using std::placeholders::_1; - namespace rmd_ros2_control { - // -- HELPER FUNCTIONS -- // +// -- 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 @@ -66,9 +63,7 @@ void RMDHardwareInterface::logger_function(){ 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 @@ -462,7 +457,10 @@ std::vector RMDHardwareInterface::export_sta } 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()); + RCLCPP_WARN( + rclcpp::get_logger("RMDHardwareInterface"), + "Unknown state interface '%s' for joint '%s'", + iface.c_str(), joint.name.c_str()); continue; } @@ -497,7 +495,10 @@ RMDHardwareInterface::export_command_interfaces() } 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()); + RCLCPP_WARN( + rclcpp::get_logger("RMDHardwareInterface"), + "Unknown command interface '%s' for joint '%s'", + iface.c_str(), joint.name.c_str()); continue; } @@ -712,8 +713,8 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( 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_command : kStatusCommands) { - send_command(joint.node_write_id, static_cast(status_command)); + 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()); } @@ -723,8 +724,8 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( 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_command : kStatusCommands) { - send_command(joint.node_write_id, static_cast(status_command)); + for (auto & status_cmd : kStatusCommands) { + send_command(joint.node_write_id, static_cast(status_cmd)); } } } @@ -755,6 +756,7 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( 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()); + joint.prev_maintenance_req = joint.motor_maintenance_req; continue; } 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 476ddb6a..91342eb8 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 @@ -35,20 +35,25 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface public: RCLCPP_SHARED_PTR_DEFINITIONS(SMCHardwareInterface) + // -- Lifecycle Functions -- 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; @@ -99,6 +104,11 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface 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; @@ -146,12 +156,14 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface bool format_maintenance_command(CANLib::CanFrame & frame, 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; + 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 + int num_joints; CANLib::SocketCanBus canBus; @@ -266,7 +278,5 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface 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 b2a16b54..30c2ce3e 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 @@ -13,6 +13,8 @@ namespace smc_ros2_control { +// -- HELPER FUNCTIONS -- // + double SMCHardwareInterface::calculate_joint_position_from_motor_position( double motor_position, int gear_ratio) { @@ -112,6 +114,22 @@ bool SMCHardwareInterface::interpret_settings_parameters( 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; } @@ -162,39 +180,79 @@ void SMCHardwareInterface::format_control_command(CANLib::CanFrame & frame, SMCJ frame.data[0] = static_cast(StatusCommands::MOTOR_STATUS_2_CMD); } -bool SMCHardwareInterface::format_status_command(CANLib::CanFrame & frame, uint8_t command_id) -{ - std::fill(std::begin(frame.data), std::end(frame.data), 0x00); - frame.data[0] = command_id; - switch (static_cast(command_id)) { - case StatusCommands::READ_ACCELERATION_CMD: - case StatusCommands::READ_SETTINGS_CMD: - case StatusCommands::READ_ENCODER_CMD: - case StatusCommands::READ_ABS_ANGLE_CMD: - case StatusCommands::MOTOR_STATUS_1_CMD: - case StatusCommands::MOTOR_STATUS_2_CMD: - return true; - default: - return false; - } -} - 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; - + 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::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: - break; + 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; } @@ -240,72 +298,89 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_init( 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); } - 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); - + // Populate each SMCJoint object SMCJoint smc_joint{}; - smc_joint.name = joint.name; - smc_joint.node_id = static_cast(node_id); - smc_joint.gear_ratio = gear_ratio; - smc_joint.orientation = orientation; - smc_joint.operating_velocity = static_cast(operating_velocity); - smc_joint.control_level = integration_level_t::POSITION; - smc_joint.joint_state_position = std::numeric_limits::quiet_NaN(); - smc_joint.joint_state_velocity = 0.0; - smc_joint.motor_temperature = std::numeric_limits::quiet_NaN(); - smc_joint.motor_torque_current = std::numeric_limits::quiet_NaN(); - smc_joint.motor_status = 0.0; - smc_joint.current_Kp = std::numeric_limits::quiet_NaN(); - smc_joint.current_Ki = std::numeric_limits::quiet_NaN(); - smc_joint.current_Kd = std::numeric_limits::quiet_NaN(); - smc_joint.speed_Kp = std::numeric_limits::quiet_NaN(); - smc_joint.speed_Ki = std::numeric_limits::quiet_NaN(); - smc_joint.speed_Kd = std::numeric_limits::quiet_NaN(); - smc_joint.position_Kp = std::numeric_limits::quiet_NaN(); - smc_joint.position_Ki = std::numeric_limits::quiet_NaN(); - smc_joint.position_Kd = std::numeric_limits::quiet_NaN(); - smc_joint.acceleration = std::numeric_limits::quiet_NaN(); - smc_joint.joint_command_position = std::numeric_limits::quiet_NaN(); - smc_joint.joint_command_velocity = 0.0; - smc_joint.motor_status_req = 0.0; - smc_joint.motor_maintenance_req = 0.0; - smc_joint.maintenance_frame_high = 0.0; - smc_joint.maintenance_frame_low = 0.0; - smc_joint.maintenance_frame = 0.0; - smc_joint.maintenance_data_count = 0.0; - smc_joint.prev_status_req = 0.0; - smc_joint.prev_maintenance_req = 0.0; - smc_joint.elapsed_status_request_time = 0.0; - smc_joint.elapsed_maintenance_request_time = 0.0; - smc_joint.motor_velocity = 0.0; - smc_joint.motor_position = 0.0; - smc_joint.encoder_position = 0.0; - smc_joint.prev_joint_command_position = std::numeric_limits::quiet_NaN(); - smc_joint.prev_joint_command_velocity = std::numeric_limits::quiet_NaN(); - smc_joint.state_interface_names = state_if_names; - smc_joint.command_interface_names = command_if_names; - smc_joint.parameters = params_map; - SMCJoints_.push_back(std::move(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()); @@ -338,9 +413,7 @@ std::vector SMCHardwareInterface::export_sta value = &joint.motor_torque_current; } else if (iface == "status") { value = &joint.motor_status; - } - - if (value == nullptr) { + } else { RCLCPP_WARN( rclcpp::get_logger("SMCHardwareInterface"), "Unknown state interface '%s' for joint '%s'", @@ -373,9 +446,7 @@ std::vector SMCHardwareInterface::export_c value = &joint.maintenance_frame_low; } else if (iface == "maintenance_data_count") { value = &joint.maintenance_data_count; - } - - if (value == nullptr) { + } else { RCLCPP_WARN( rclcpp::get_logger("SMCHardwareInterface"), "Unknown command interface '%s' for joint '%s'", @@ -453,6 +524,12 @@ void SMCHardwareInterface::on_can_message(const CANLib::CanFrame & frame) joint.acceleration = static_cast( (frame.data[7] << 24) | (frame.data[6] << 16) | (frame.data[5] << 8) | frame.data[4]); } + + if(frame.data[0] == static_cast(StatusCommands::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()); + } + } } } @@ -513,26 +590,21 @@ hardware_interface::return_type SMCHardwareInterface::write( 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) { + if (curr_status_req < 0.0 && joint.prev_status_req >= 0.0) + { for (auto status_cmd : kStatusCommands) { - CANLib::CanFrame frame; - frame.id = joint.node_id; - frame.dlc = 8; - if (format_status_command(frame, static_cast(status_cmd))) { - canBus.send(frame); - } + send_command(joint.node_id, static_cast(status_cmd)); } - } else if (curr_status_req > 0.0) { + 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(); - if (joint.elapsed_status_request_time > (1.0 / curr_status_req)) { + 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) { - CANLib::CanFrame frame; - frame.id = joint.node_id; - frame.dlc = 8; - if (format_status_command(frame, static_cast(status_cmd))) { - canBus.send(frame); - } + send_command(joint.node_id, static_cast(status_cmd)); } } } @@ -566,7 +638,8 @@ hardware_interface::return_type SMCHardwareInterface::write( 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)) { + 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); } 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 0c6db7b5..5152b611 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -205,7 +205,8 @@ motor_status_controller: - wrist_pitch - wrist_roll - actuator - interfaces: + state_interfaces: - motor_temperature - torque_current + - status publish_rate: 10.0 diff --git a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp index b480436c..70339ef9 100644 --- a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp +++ b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp @@ -14,6 +14,7 @@ #include "general_controllers/motor_status_controller.hpp" +#include #include #include @@ -48,16 +49,7 @@ controller_interface::InterfaceConfiguration MotorStatusController::command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; - command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - - command_interfaces_config.names.reserve(params_.joints.size() * params_.command_interfaces.size()); - for (const auto & joint : params_.joints) - { - for (const auto & iface : params_.command_interfaces) { - command_interfaces_config.names.push_back(joint + "/" + iface); - } - } - + command_interfaces_config.type = controller_interface::interface_configuration_type::ALL; return command_interfaces_config; } @@ -65,14 +57,7 @@ controller_interface::InterfaceConfiguration MotorStatusController::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_.state_interfaces) { - config.names.push_back(joint + "/" + iface); - } - } - + config.type = controller_interface::interface_configuration_type::ALL; return config; } @@ -197,6 +182,38 @@ controller_interface::CallbackReturn MotorStatusController::on_activate( state_interfaces_[i].get_interface_name()] = i; } + for (const auto & joint : params_.joints) { + std::vector available_interfaces; + for (const auto & iface : params_.state_interfaces) { + const std::string key = joint + "/" + iface; + if (state_interface_map_.find(key) != state_interface_map_.end()) { + available_interfaces.push_back(iface); + } + } + + if (available_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_interfaces.size(); ++i) { + if (i > 0) { + interfaces_stream << ", "; + } + interfaces_stream << available_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()); + } + // Build lookup map from "joint/interface" -> command_interfaces_ index command_interface_map_.clear(); for (size_t i = 0; i < command_interfaces_.size(); ++i) { @@ -204,6 +221,38 @@ controller_interface::CallbackReturn MotorStatusController::on_activate( command_interfaces_[i].get_interface_name()] = i; } + for (const auto & joint : params_.joints) { + std::vector available_command_interfaces; + 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); + } + } + + if (available_command_interfaces.empty()) { + RCLCPP_INFO( + get_node()->get_logger(), + "Joint '%s' has no motor status command interfaces available.", + joint.c_str()); + continue; + } + + std::ostringstream interfaces_stream; + 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()); + } + last_publish_time_ = get_node()->now(); RCLCPP_INFO( @@ -289,8 +338,6 @@ controller_interface::return_type MotorStatusController::update( command_interfaces_[it->second].set_value(maintenance_request_rate); } } - } else { - RCLCPP_WARN(get_node()->get_logger(), "Command interface %s not found for joint %s", iface.c_str(), joint.c_str()); } } @@ -310,7 +357,7 @@ controller_interface::return_type MotorStatusController::update( if (it != state_interface_map_.end()) { double value = state_interfaces_[it->second].get_value(); - if (iface == "temperature") { + if (iface == "motor_temperature") { status.temperature = static_cast(value); } else if (iface == "torque_current") { status.torque_current = value; From f1fe0d07117130818016d8539237a57c0e678ee9 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Sun, 26 Apr 2026 19:38:55 -0400 Subject: [PATCH 16/37] fixed state issue, accepts all state interfaces and filters accordingly --- .../src/rmd_hardware_interface.cpp | 14 +- .../motor_status_controller.hpp | 6 + .../src/motor_status_controller.cpp | 146 ++++++++++-------- 3 files changed, 98 insertions(+), 68 deletions(-) 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 c1dde012..d2f3d035 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 @@ -805,13 +805,13 @@ 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. 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 index 5df4ede7..8edd210c 100644 --- 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 @@ -63,6 +63,9 @@ class MotorStatusController : public controller_interface::ControllerInterface 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_; @@ -96,6 +99,9 @@ class MotorStatusController : public controller_interface::ControllerInterface 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) diff --git a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp index 70339ef9..2fcdfbe1 100644 --- a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp +++ b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp @@ -20,11 +20,70 @@ #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 { @@ -49,16 +108,28 @@ controller_interface::InterfaceConfiguration MotorStatusController::command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; - command_interfaces_config.type = controller_interface::interface_configuration_type::ALL; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(params_.joints.size() * params_.command_interfaces.size()); + for (const auto & joint : params_.joints) + { + for (const auto & iface : params_.command_interfaces) { + command_interfaces_config.names.push_back(joint + "/" + iface); + } + } return command_interfaces_config; } controller_interface::InterfaceConfiguration MotorStatusController::state_interface_configuration() const { - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::ALL; - return config; + controller_interface::InterfaceConfiguration state_interfaces_config; + + // Request every available state interface + state_interfaces_config.type = + controller_interface::interface_configuration_type::ALL; + + return state_interfaces_config; } controller_interface::CallbackReturn MotorStatusController::on_configure( @@ -166,7 +237,7 @@ controller_interface::CallbackReturn MotorStatusController::on_configure( RCLCPP_INFO( get_node()->get_logger(), - "Configured motor_status_controller for %zu joints, %zu interfaces, publish rate: %.1f Hz", + "Configured motor_status_controller for %zu joints, %zu state interfaces each, publish rate: %.1f Hz", params_.joints.size(), params_.state_interfaces.size(), params_.publish_rate); return controller_interface::CallbackReturn::SUCCESS; @@ -183,35 +254,12 @@ controller_interface::CallbackReturn MotorStatusController::on_activate( } for (const auto & joint : params_.joints) { - std::vector available_interfaces; for (const auto & iface : params_.state_interfaces) { const std::string key = joint + "/" + iface; if (state_interface_map_.find(key) != state_interface_map_.end()) { - available_interfaces.push_back(iface); + available_state_interfaces.push_back(iface); } } - - if (available_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_interfaces.size(); ++i) { - if (i > 0) { - interfaces_stream << ", "; - } - interfaces_stream << available_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()); } // Build lookup map from "joint/interface" -> command_interfaces_ index @@ -222,44 +270,16 @@ controller_interface::CallbackReturn MotorStatusController::on_activate( } for (const auto & joint : params_.joints) { - std::vector available_command_interfaces; 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); } } - - if (available_command_interfaces.empty()) { - RCLCPP_INFO( - get_node()->get_logger(), - "Joint '%s' has no motor status command interfaces available.", - joint.c_str()); - continue; - } - - std::ostringstream interfaces_stream; - 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()); } last_publish_time_ = get_node()->now(); - RCLCPP_INFO( - get_node()->get_logger(), - "MotorStatusController activated with %zu state interfaces", - state_interfaces_.size()); - return controller_interface::CallbackReturn::SUCCESS; } @@ -296,13 +316,16 @@ controller_interface::return_type MotorStatusController::update( if (iface == "maintenance_frame_high" && joint == maintenance_req_joint_name) { command_interfaces_[it->second].set_value(maintenance_frame_high); } - else if (iface == "maintenance_frame_low" && joint == maintenance_req_joint_name) { + + if (iface == "maintenance_frame_low" && joint == maintenance_req_joint_name) { command_interfaces_[it->second].set_value(maintenance_frame_low); } - else if(iface == "maintenance_data_count" && joint == maintenance_req_joint_name) { + + if(iface == "maintenance_data_count" && joint == maintenance_req_joint_name) { command_interfaces_[it->second].set_value(maintenance_data_count); } - else if (iface == "status_request" && joint == status_req_joint_name) { + + if (iface == "status_request" && joint == status_req_joint_name) { // Controller must turn status request back to 0 if it's a one-shot request if (status_request_rate < 0 && status_one_shot_sent == false) { command_interfaces_[it->second].set_value(status_request_rate); @@ -320,7 +343,8 @@ controller_interface::return_type MotorStatusController::update( command_interfaces_[it->second].set_value(status_request_rate); } } - else if (iface == "maintenance_request" && joint == maintenance_req_joint_name) { + + if (iface == "maintenance_request" && joint == maintenance_req_joint_name) { // Controller must turn maintenance request back to 0 if it's a one-shot request if (maintenance_request_rate < 0 && maintenance_one_shot_sent == false) { command_interfaces_[it->second].set_value(maintenance_request_rate); From ff1fea52bf820540e27212a63b5e1b5f8a02c6ec Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Mon, 27 Apr 2026 00:39:01 -0400 Subject: [PATCH 17/37] saving progress, killswitch seems to not be sending can messages for some reason --- .../drive/drive.led.ros2_control.xacro | 3 + ... => drive.power_module.ros2_control.xacro} | 7 +- .../science/science.laser.ros2_control.xacro | 3 + src/description/urdf/athena_drive.urdf.xacro | 8 +- src/hardware_interfaces/README.md | 2 +- .../killswitch_hardware_interface.xml | 10 -- .../laser_hardware_interface.hpp | 7 ++ .../src/laser_hardware_interface.cpp | 48 ++++++++ .../led_hardware_interface.hpp | 7 ++ .../src/led_hardware_interface.cpp | 45 +++++++ .../CMakeLists.txt | 26 ++-- .../power_module_hardware_interface.hpp} | 35 ++++-- .../package.xml | 4 +- .../power_module_hardware_interface.xml | 10 ++ .../src/power_module_hardware_interface.cpp} | 114 ++++++++++++++---- src/subsystems/drive/README.md | 9 +- .../config/athena_drive_controllers.yaml | 14 +-- .../launch/athena_drive.launch.py | 2 +- 18 files changed, 278 insertions(+), 76 deletions(-) rename src/description/ros2_control/drive/{drive.killswitch.ros2_control.xacro => drive.power_module.ros2_control.xacro} (69%) delete mode 100644 src/hardware_interfaces/killswitch_ros2_control/killswitch_hardware_interface.xml rename src/hardware_interfaces/{killswitch_ros2_control => power_module_ros2_control}/CMakeLists.txt (63%) rename src/hardware_interfaces/{killswitch_ros2_control/include/killswitch_ros2_control/killswitch_hardware_interface.hpp => power_module_ros2_control/include/power_module_ros2_control/power_module_hardware_interface.hpp} (69%) rename src/hardware_interfaces/{killswitch_ros2_control => power_module_ros2_control}/package.xml (85%) create mode 100644 src/hardware_interfaces/power_module_ros2_control/power_module_hardware_interface.xml rename src/hardware_interfaces/{killswitch_ros2_control/src/killswitch_hardware_interface.cpp => power_module_ros2_control/src/power_module_hardware_interface.cpp} (52%) 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 09f0d44a..66f1df4f 100644 --- a/src/description/ros2_control/drive/drive.led.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.led.ros2_control.xacro @@ -8,6 +8,9 @@ led_ros2_control/LEDHardwareInterface ${gpio_pin} ${default_state} + 10 + 5 + 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 69% 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 fca01325..c95e587b 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,13 +1,16 @@ - + - killswitch_ros2_control/KillswitchHardwareInterface + power_module_ros2_control/PowerModuleHardwareInterface ${can_interface} 0x100 + 10 + 5 + 0 diff --git a/src/description/ros2_control/science/science.laser.ros2_control.xacro b/src/description/ros2_control/science/science.laser.ros2_control.xacro index 4f101acc..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,6 +9,9 @@ laser_ros2_control/LaserHardwareInterface ${can_interface} 0x130 + 10 + 5 + 0 diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index bd7dd22e..4be91a91 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -39,8 +39,8 @@ - - + + @@ -80,8 +80,8 @@ - - + + 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/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/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 a3449ab7..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 @@ -45,6 +45,8 @@ class LaserHardwareInterface : public hardware_interface::SystemInterface hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + void logger_function(); + private: struct LaserJoint { @@ -67,6 +69,11 @@ class LaserHardwareInterface : public hardware_interface::SystemInterface 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_; static constexpr uint8_t CMD_LASER_ON = 0x60; static constexpr uint8_t CMD_LASER_OFF = 0x80; 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 cdcc97dd..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 @@ -3,9 +3,40 @@ #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) { @@ -17,6 +48,12 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_init( 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; @@ -35,6 +72,8 @@ hardware_interface::CallbackReturn LaserHardwareInterface::on_init( }); can_connected_ = false; + elapsed_time_ = 0.0; + elapsed_logger_time_ = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } @@ -140,6 +179,15 @@ hardware_interface::return_type LaserHardwareInterface::write( { 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) { 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 973468c2..6db385d5 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 @@ -50,6 +50,8 @@ class LEDHardwareInterface : public hardware_interface::SystemInterface hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + void logger_function(); + private: struct LEDJoint { @@ -68,6 +70,11 @@ class LEDHardwareInterface : public hardware_interface::SystemInterface int gpio_fd_; bool hw_connected_; std::vector LEDJoints_; + int update_rate_; + int logger_rate_; + int logger_state_; + double elapsed_time_; + double elapsed_logger_time_; }; } // namespace led_ros2_control 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 a4741bc2..a6da9365 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 @@ -63,6 +63,34 @@ bool write_gpio(int fd, bool value) } // namespace gpio_utils +void LEDHardwareInterface::logger_function() +{ + if (LEDJoints_.empty()) { + return; + } + + const auto & joint = LEDJoints_.front(); + std::ostringstream oss; + oss << "\033[2J\033[H \nLED Logger" + << "\n--- HWI Specific ---\n" + << "GPIO Pin: " << joint.gpio_pin + << " | 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: Default State: " << (joint.default_state ? "ON" : "OFF") << "\n" + << "-- Commands --\n" + << "LED Command: " << joint.led_command + << " | Status Request: " << joint.status_request << "\n" + << "-- State --\n" + << "LED State: " << joint.led_state + << " | Is Connected: " << joint.is_connected + << " | Status: " << joint.status << "\n"; + + RCLCPP_INFO(rclcpp::get_logger("LEDHardwareInterface"), "%s", oss.str().c_str()); +} + hardware_interface::CallbackReturn LEDHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) { @@ -80,6 +108,12 @@ hardware_interface::CallbackReturn LEDHardwareInterface::on_init( const int gpio_pin = std::stoi(info_.hardware_parameters.at("gpio_pin")); const bool default_state = info_.hardware_parameters.count("default_state") && info_.hardware_parameters.at("default_state") == "on"; + 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; LEDJoints_.clear(); LEDJoints_.push_back(LEDJoint{ @@ -97,6 +131,8 @@ hardware_interface::CallbackReturn LEDHardwareInterface::on_init( gpio_fd_ = -1; hw_connected_ = false; + elapsed_time_ = 0.0; + elapsed_logger_time_ = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } @@ -182,6 +218,15 @@ hardware_interface::return_type LEDHardwareInterface::write( { auto & joint = LEDJoints_.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.led_command > 0.5; const bool currently_on = joint.led_state > 0.5; if (commanded_on != currently_on) { 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/killswitch_ros2_control/include/killswitch_ros2_control/killswitch_hardware_interface.hpp b/src/hardware_interfaces/power_module_ros2_control/include/power_module_ros2_control/power_module_hardware_interface.hpp similarity index 69% rename from src/hardware_interfaces/killswitch_ros2_control/include/killswitch_ros2_control/killswitch_hardware_interface.hpp rename to src/hardware_interfaces/power_module_ros2_control/include/power_module_ros2_control/power_module_hardware_interface.hpp index 690cf49c..31b53133 100644 --- a/src/hardware_interfaces/killswitch_ros2_control/include/killswitch_ros2_control/killswitch_hardware_interface.hpp +++ b/src/hardware_interfaces/power_module_ros2_control/include/power_module_ros2_control/power_module_hardware_interface.hpp @@ -1,5 +1,5 @@ -#ifndef KILLSWITCH_ROS2_CONTROL__KILLSWITCH_HARDWARE_INTERFACE_HPP_ -#define KILLSWITCH_ROS2_CONTROL__KILLSWITCH_HARDWARE_INTERFACE_HPP_ +#ifndef POWER_MODULE_ROS2_CONTROL__POWER_MODULE_HARDWARE_INTERFACE_HPP_ +#define POWER_MODULE_ROS2_CONTROL__POWER_MODULE_HARDWARE_INTERFACE_HPP_ #include #include @@ -15,13 +15,13 @@ #include "umdloop_can_library/CanFrame.hpp" #include "umdloop_can_library/SocketCanBus.hpp" -namespace killswitch_ros2_control +namespace power_module_ros2_control { -class KillswitchHardwareInterface : public hardware_interface::SystemInterface +class PowerModuleHardwareInterface : public hardware_interface::SystemInterface { public: - RCLCPP_SHARED_PTR_DEFINITIONS(KillswitchHardwareInterface) + RCLCPP_SHARED_PTR_DEFINITIONS(PowerModuleHardwareInterface) hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; @@ -45,8 +45,13 @@ class KillswitchHardwareInterface : public hardware_interface::SystemInterface 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 KillswitchJoint + struct PowerModuleJoint { std::string name; uint32_t can_id; @@ -63,19 +68,27 @@ class KillswitchHardwareInterface : public hardware_interface::SystemInterface double elapsed_status_request_time; }; - void onCanMessage(const CANLib::CanFrame & frame); - std::string can_interface_; CANLib::SocketCanBus canBus_; CANLib::CanFrame can_tx_frame_; bool can_connected_; - std::vector KILLSWITCHJoints_; + std::vector PowerModuleJoints_; + 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 = 0x01; static constexpr uint8_t CMD_KILL_MAIN = 0x03; static constexpr uint8_t CMD_KILL_JETSON = 0x05; + + // 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 killswitch_ros2_control +} // namespace power_module_ros2_control -#endif // KILLSWITCH_ROS2_CONTROL__KILLSWITCH_HARDWARE_INTERFACE_HPP_ +#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/killswitch_ros2_control/src/killswitch_hardware_interface.cpp b/src/hardware_interfaces/power_module_ros2_control/src/power_module_hardware_interface.cpp similarity index 52% rename from src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp rename to src/hardware_interfaces/power_module_ros2_control/src/power_module_hardware_interface.cpp index 8d85cb55..4d38b075 100644 --- a/src/hardware_interfaces/killswitch_ros2_control/src/killswitch_hardware_interface.cpp +++ b/src/hardware_interfaces/power_module_ros2_control/src/power_module_hardware_interface.cpp @@ -1,12 +1,56 @@ -#include "killswitch_ros2_control/killswitch_hardware_interface.hpp" +#include "power_module_ros2_control/power_module_hardware_interface.hpp" #include "pluginlib/class_list_macros.hpp" #include "rclcpp/rclcpp.hpp" -namespace killswitch_ros2_control +#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 (PowerModuleJoints_.empty()) { + return; + } -hardware_interface::CallbackReturn KillswitchHardwareInterface::on_init( + const auto & joint = PowerModuleJoints_.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--- Joint Specific ---\n" + << "JOINT: " << joint.name << "\n" + << "Parameters: CAN ID: 0x" << std::hex << std::uppercase << joint.can_id << std::dec << "\n" + << "-- Commands --\n" + << "Kill All: " << joint.cmd_kill_all + << " | Kill Main: " << joint.cmd_kill_main + << " | Kill Jetson: " << joint.cmd_kill_jetson << "\n" + << "Status Request: " << joint.status_request << "\n" + << "-- State --\n" + << "Kill All Sent: " << joint.kill_all_sent + << " | Kill Main Sent: " << joint.kill_main_sent + << " | Kill Jetson Sent: " << joint.kill_jetson_sent << "\n" + << "Is Connected: " << joint.is_connected + << " | Status: " << joint.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) != @@ -17,11 +61,17 @@ hardware_interface::CallbackReturn KillswitchHardwareInterface::on_init( 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)) : 0x100; - KILLSWITCHJoints_.clear(); - KILLSWITCHJoints_.push_back(KillswitchJoint{ + PowerModuleJoints_.clear(); + PowerModuleJoints_.push_back(PowerModuleJoint{ info_.gpios[0].name, can_id, 0.0, @@ -38,33 +88,35 @@ hardware_interface::CallbackReturn KillswitchHardwareInterface::on_init( }); can_connected_ = false; + elapsed_time_ = 0.0; + elapsed_logger_time_ = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn KillswitchHardwareInterface::on_configure( +hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_configure( const rclcpp_lifecycle::State &) { if (!canBus_.open( can_interface_, - std::bind(&KillswitchHardwareInterface::onCanMessage, this, std::placeholders::_1))) + std::bind(&PowerModuleHardwareInterface::on_can_message, this, std::placeholders::_1))) { can_connected_ = false; } else { can_connected_ = true; } - auto & joint = KILLSWITCHJoints_.front(); + auto & joint = PowerModuleJoints_.front(); joint.is_connected = can_connected_ ? 1.0 : 0.0; joint.status = joint.is_connected; return hardware_interface::CallbackReturn::SUCCESS; } -void KillswitchHardwareInterface::onCanMessage(const CANLib::CanFrame &) +void PowerModuleHardwareInterface::on_can_message(const CANLib::CanFrame &) { } -std::vector KillswitchHardwareInterface::export_state_interfaces() +std::vector PowerModuleHardwareInterface::export_state_interfaces() { - auto & joint = KILLSWITCHJoints_.front(); + auto & joint = PowerModuleJoints_.front(); std::vector state_interfaces; state_interfaces.emplace_back(joint.name, "kill_all_sent", &joint.kill_all_sent); state_interfaces.emplace_back(joint.name, "kill_main_sent", &joint.kill_main_sent); @@ -74,9 +126,9 @@ std::vector KillswitchHardwareInterface::exp return state_interfaces; } -std::vector KillswitchHardwareInterface::export_command_interfaces() +std::vector PowerModuleHardwareInterface::export_command_interfaces() { - auto & joint = KILLSWITCHJoints_.front(); + auto & joint = PowerModuleJoints_.front(); std::vector command_interfaces; command_interfaces.emplace_back(joint.name, "kill_all", &joint.cmd_kill_all); command_interfaces.emplace_back(joint.name, "kill_main", &joint.cmd_kill_main); @@ -85,51 +137,60 @@ std::vector KillswitchHardwareInterface::e return command_interfaces; } -hardware_interface::CallbackReturn KillswitchHardwareInterface::on_activate( +hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_activate( const rclcpp_lifecycle::State &) { - auto & joint = KILLSWITCHJoints_.front(); + auto & joint = PowerModuleJoints_.front(); joint.cmd_kill_all = 0.0; joint.cmd_kill_main = 0.0; joint.cmd_kill_jetson = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn KillswitchHardwareInterface::on_deactivate( +hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_deactivate( const rclcpp_lifecycle::State &) { return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn KillswitchHardwareInterface::on_cleanup( +hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_cleanup( const rclcpp_lifecycle::State &) { if (can_connected_) { canBus_.close(); } can_connected_ = false; - auto & joint = KILLSWITCHJoints_.front(); + auto & joint = PowerModuleJoints_.front(); joint.is_connected = 0.0; joint.status = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn KillswitchHardwareInterface::on_shutdown( +hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_shutdown( const rclcpp_lifecycle::State & previous_state) { return on_cleanup(previous_state); } -hardware_interface::return_type KillswitchHardwareInterface::read( +hardware_interface::return_type PowerModuleHardwareInterface::read( const rclcpp::Time &, const rclcpp::Duration &) { return hardware_interface::return_type::OK; } -hardware_interface::return_type KillswitchHardwareInterface::write( +hardware_interface::return_type PowerModuleHardwareInterface::write( const rclcpp::Time &, const rclcpp::Duration & period) { - auto & joint = KILLSWITCHJoints_.front(); + auto & joint = PowerModuleJoints_.front(); + + elapsed_time_ += period.seconds(); + elapsed_logger_time_ += period.seconds(); + if (logger_rate_ > 0 && elapsed_logger_time_ > (1.0 / static_cast(logger_rate_))) { + elapsed_logger_time_ = 0.0; + if (logger_state_ == 1) { + logger_function(); + } + } if (joint.cmd_kill_all > 0.5 && joint.kill_all_sent < 0.5) { if (can_connected_) { @@ -138,7 +199,9 @@ hardware_interface::return_type KillswitchHardwareInterface::write( can_tx_frame_.dlc = 1; can_tx_frame_.data[0] = CMD_KILL_ALL; canBus_.send(can_tx_frame_); + RCLCPP_INFO(rclcpp::get_logger("PowerModuleHardwareInterface"), "Sent Kill All Command to CAN ID 0x%X", joint.can_id); } + joint.kill_all_sent = 1.0; joint.status = 1.0; } else if (joint.cmd_kill_all < 0.5) { @@ -152,6 +215,7 @@ hardware_interface::return_type KillswitchHardwareInterface::write( can_tx_frame_.dlc = 1; can_tx_frame_.data[0] = CMD_KILL_MAIN; canBus_.send(can_tx_frame_); + RCLCPP_INFO(rclcpp::get_logger("PowerModuleHardwareInterface"), "Sent Kill Main Command to CAN ID 0x%X", joint.can_id); } joint.kill_main_sent = 1.0; joint.status = 1.0; @@ -166,7 +230,9 @@ hardware_interface::return_type KillswitchHardwareInterface::write( can_tx_frame_.dlc = 1; can_tx_frame_.data[0] = CMD_KILL_JETSON; canBus_.send(can_tx_frame_); + RCLCPP_INFO(rclcpp::get_logger("PowerModuleHardwareInterface"), "Sent Kill Jetson Command to CAN ID 0x%X", joint.can_id); } + joint.kill_jetson_sent = 1.0; joint.status = 1.0; } else if (joint.cmd_kill_jetson < 0.5) { @@ -187,8 +253,8 @@ hardware_interface::return_type KillswitchHardwareInterface::write( return hardware_interface::return_type::OK; } -} // namespace killswitch_ros2_control +} // namespace power_module_ros2_control PLUGINLIB_EXPORT_CLASS( - killswitch_ros2_control::KillswitchHardwareInterface, + power_module_ros2_control::PowerModuleHardwareInterface, hardware_interface::SystemInterface) 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 1e705c36..2fb25d69 100644 --- a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml +++ b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml @@ -45,8 +45,8 @@ controller_manager: 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_controller: @@ -126,19 +126,19 @@ led_gpio_controller: - led_state - is_connected -# 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 state_interfaces: - drive_killswitch: + drive_power_module: interfaces: - kill_all_sent - kill_main_sent 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 a6650637..12ec7d5d 100644 --- a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py +++ b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py @@ -256,7 +256,7 @@ def launch_setup(context, *args, **kwargs): ) ] - gpio_controller_names = ["led_gpio_controller", "killswitch_gpio_controller"] + gpio_controller_names = ["led_gpio_controller", "power_module_gpio_controller"] gpio_controller_spawners = [] for controller in gpio_controller_names: gpio_controller_spawners += [ From 83bca6cb8fec719c5a44a709c9b66ed8fd33f444 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Tue, 28 Apr 2026 23:31:40 -0400 Subject: [PATCH 18/37] saving changes, for some reason I cant see power module can commands being sent --- .../drive/drive.led.ros2_control.xacro | 14 +- .../drive.power_module.ros2_control.xacro | 7 +- src/description/urdf/athena_drive.urdf.xacro | 4 +- .../led_ros2_control/CMakeLists.txt | 5 + .../led_hardware_interface.hpp | 38 ++- .../led_ros2_control/package.xml | 4 + .../src/led_hardware_interface.cpp | 258 ++++++++-------- .../power_module_hardware_interface.hpp | 32 +- .../src/power_module_hardware_interface.cpp | 289 +++++++++++------- .../src/rmd_hardware_interface.cpp | 4 +- .../config/athena_drive_controllers.yaml | 33 +- src/subsystems/general/README.md | 14 +- 12 files changed, 394 insertions(+), 308 deletions(-) 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 66f1df4f..619ad55b 100644 --- a/src/description/ros2_control/drive/drive.led.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.led.ros2_control.xacro @@ -1,23 +1,25 @@ - + led_ros2_control/LEDHardwareInterface - ${gpio_pin} - ${default_state} + ${can_interface} + 0x120 10 5 0 - - + true - + + + + diff --git a/src/description/ros2_control/drive/drive.power_module.ros2_control.xacro b/src/description/ros2_control/drive/drive.power_module.ros2_control.xacro index c95e587b..8f6c90bc 100644 --- a/src/description/ros2_control/drive/drive.power_module.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.power_module.ros2_control.xacro @@ -14,14 +14,15 @@ + - - - + + + diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index 4be91a91..80c66f07 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -78,10 +78,10 @@ - + - + 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 6db385d5..094256b6 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 @@ -13,16 +13,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" -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; } +namespace led_ros2_control +{ + class LEDHardwareInterface : public hardware_interface::SystemInterface { public: @@ -50,26 +51,33 @@ class LEDHardwareInterface : public hardware_interface::SystemInterface hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + // -- Helper Functions -- void logger_function(); private: - struct LEDJoint + struct LEDGPIO { std::string name; - int gpio_pin; - bool default_state; - double led_state; - double is_connected; + bool is_rgb; double status; - double led_command; + double intensity; + double red_command; + double green_command; + double blue_command; 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; }; - int gpio_fd_; - bool hw_connected_; - std::vector LEDJoints_; + std::string can_interface_; + CANLib::SocketCanBus canBus_; + CANLib::CanFrame can_tx_frame_; + + std::vector LEDGPIOs_; int update_rate_; int logger_rate_; int logger_state_; 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 a6da9365..51df592a 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 @@ -10,83 +10,28 @@ namespace led_ros2_control { -namespace gpio_utils -{ - -int setup_gpio_output(int pin) -{ - int fd = ::open("/sys/class/gpio/export", O_WRONLY); - if (fd >= 0) { - const std::string pin_str = std::to_string(pin); - ::write(fd, pin_str.c_str(), pin_str.length()); - ::close(fd); - usleep(100000); - } - - 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); - - std::stringstream value_path; - value_path << "/sys/class/gpio/gpio" << pin << "/value"; - return ::open(value_path.str().c_str(), O_RDWR); -} - -void cleanup_gpio(int pin, int fd) -{ - if (fd >= 0) { - ::close(fd); - } - - int export_fd = ::open("/sys/class/gpio/unexport", O_WRONLY); - if (export_fd >= 0) { - const std::string pin_str = std::to_string(pin); - ::write(export_fd, pin_str.c_str(), pin_str.length()); - ::close(export_fd); - } -} - -bool write_gpio(int fd, bool value) -{ - if (fd < 0) { - return false; - } - const char val = value ? '1' : '0'; - lseek(fd, 0, SEEK_SET); - return (::write(fd, &val, 1) == 1); -} - -} // namespace gpio_utils - void LEDHardwareInterface::logger_function() { - if (LEDJoints_.empty()) { + if (LEDGPIOs_.empty()) { return; } - const auto & joint = LEDJoints_.front(); + const auto & gpio = LEDGPIOs_.front(); std::ostringstream oss; oss << "\033[2J\033[H \nLED Logger" << "\n--- HWI Specific ---\n" - << "GPIO Pin: " << joint.gpio_pin - << " | HWI Update Rate: " << update_rate_ + << "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: Default State: " << (joint.default_state ? "ON" : "OFF") << "\n" + << "\n--- GPIO Specific ---\n" + << "GPIO: " << gpio.name << "\n" << "-- Commands --\n" - << "LED Command: " << joint.led_command - << " | Status Request: " << joint.status_request << "\n" + << "Red Command: " << gpio.red_command + << " | Green Command: " << gpio.green_command + << " | Blue Command: " << gpio.blue_command + << " | Status Request: " << gpio.status_request << "\n" << "-- State --\n" - << "LED State: " << joint.led_state - << " | Is Connected: " << joint.is_connected - << " | Status: " << joint.status << "\n"; + << "Status: " << gpio.status << "\n"; RCLCPP_INFO(rclcpp::get_logger("LEDHardwareInterface"), "%s", oss.str().c_str()); } @@ -99,38 +44,63 @@ hardware_interface::CallbackReturn LEDHardwareInterface::on_init( { return hardware_interface::CallbackReturn::ERROR; } - - if (!info_.hardware_parameters.count("gpio_pin")) { - RCLCPP_ERROR(rclcpp::get_logger("LEDHardwareInterface"), "gpio_pin parameter is required"); - return hardware_interface::CallbackReturn::ERROR; - } - - const int gpio_pin = std::stoi(info_.hardware_parameters.at("gpio_pin")); - const bool default_state = info_.hardware_parameters.count("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")) : 0; + 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")) { + const uint32_t can_id = static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); + } + else { + RCLCPP_ERROR(rclcpp::get_logger("PowerModuleHardwareInterface"), "CAN ID parameter 'can_id' is missing or invalid 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); + } - LEDJoints_.clear(); - LEDJoints_.push_back(LEDJoint{ - info_.gpios[0].name, - gpio_pin, - default_state, - default_state ? 1.0 : 0.0, - 0.0, - 0.0, - default_state ? 1.0 : 0.0, - 0.0, - 0.0, - 0.0 - }); + LEDGPIOs_.push_back( + LEDGPIO{ + .name = info_.gpios[0].name, + .is_rgb = false, + .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, + + .state_interface_names = state_if_names, + .command_interface_names = command_if_names, + .parameters = params_map + } + ); + } - gpio_fd_ = -1; - hw_connected_ = false; elapsed_time_ = 0.0; elapsed_logger_time_ = 0.0; return hardware_interface::CallbackReturn::SUCCESS; @@ -139,33 +109,70 @@ hardware_interface::CallbackReturn LEDHardwareInterface::on_init( hardware_interface::CallbackReturn LEDHardwareInterface::on_configure( const rclcpp_lifecycle::State &) { - auto & joint = LEDJoints_.front(); - gpio_fd_ = gpio_utils::setup_gpio_output(joint.gpio_pin); - hw_connected_ = gpio_fd_ >= 0; - if (hw_connected_) { - gpio_utils::write_gpio(gpio_fd_, joint.default_state); - } - joint.is_connected = hw_connected_ ? 1.0 : 0.0; - joint.status = joint.is_connected; return hardware_interface::CallbackReturn::SUCCESS; } std::vector LEDHardwareInterface::export_state_interfaces() { - auto & joint = LEDJoints_.front(); std::vector state_interfaces; - state_interfaces.emplace_back(joint.name, "led_state", &joint.led_state); - state_interfaces.emplace_back(joint.name, "is_connected", &joint.is_connected); - state_interfaces.emplace_back(joint.name, "status", &joint.status); + + 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() { - auto & joint = LEDJoints_.front(); std::vector command_interfaces; - command_interfaces.emplace_back(joint.name, "led_command", &joint.led_command); - command_interfaces.emplace_back(joint.name, "status_request", &joint.status_request); + + for (auto & gpio : LEDGPIOs_) { + for (auto & iface : gpio.command_interface_names) { + double * val = nullptr; + 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 if (iface == "intensity") { + if(gpio.is_rgb) { + RCLCPP_ERROR( + rclcpp::get_logger("LEDHardwareInterface"), + "Intensity command interface is not supported for RGB GPIO '%s'", + gpio.name.c_str()); + continue; + } + else { + val = &gpio.intensity; + } + } else { + RCLCPP_WARN( + rclcpp::get_logger("LEDHardwareInterface"), + "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; } @@ -178,26 +185,14 @@ hardware_interface::CallbackReturn LEDHardwareInterface::on_activate( hardware_interface::CallbackReturn LEDHardwareInterface::on_deactivate( const rclcpp_lifecycle::State &) { - auto & joint = LEDJoints_.front(); - if (hw_connected_ && gpio_fd_ >= 0) { - gpio_utils::write_gpio(gpio_fd_, false); - } - joint.led_state = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn LEDHardwareInterface::on_cleanup( const rclcpp_lifecycle::State &) { - if (gpio_fd_ >= 0) { - gpio_utils::write_gpio(gpio_fd_, false); - gpio_utils::cleanup_gpio(LEDJoints_.front().gpio_pin, gpio_fd_); - gpio_fd_ = -1; - } - hw_connected_ = false; - auto & joint = LEDJoints_.front(); - joint.is_connected = 0.0; - joint.status = 0.0; + auto & gpio = LEDGPIOs_.front(); + gpio.status = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } @@ -216,7 +211,7 @@ hardware_interface::return_type LEDHardwareInterface::read( hardware_interface::return_type LEDHardwareInterface::write( const rclcpp::Time &, const rclcpp::Duration & period) { - auto & joint = LEDJoints_.front(); + auto & gpio = LEDGPIOs_.front(); elapsed_time_ += period.seconds(); elapsed_logger_time_ += period.seconds(); @@ -227,25 +222,14 @@ hardware_interface::return_type LEDHardwareInterface::write( } } - const bool commanded_on = joint.led_command > 0.5; - const bool currently_on = joint.led_state > 0.5; - if (commanded_on != currently_on) { - if (hw_connected_ && gpio_fd_ >= 0) { - gpio_utils::write_gpio(gpio_fd_, commanded_on); - } - joint.led_state = commanded_on ? 1.0 : 0.0; - } - - if (joint.status_request < 0.0 && joint.prev_status_request >= 0.0) { - joint.status = hw_connected_ ? 1.0 : 0.0; - } 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; - joint.status = hw_connected_ ? 1.0 : 0.0; + 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; } } - joint.prev_status_request = joint.status_request; + gpio.prev_status_request = gpio.status_request; return hardware_interface::return_type::OK; } 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 index 31b53133..d5398e09 100644 --- 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 @@ -12,9 +12,15 @@ #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 { @@ -51,28 +57,33 @@ class PowerModuleHardwareInterface : public hardware_interface::SystemInterface void logger_function(); private: - struct PowerModuleJoint + struct PowerModuleGPIO { std::string name; uint32_t can_id; double kill_all_sent; - double kill_main_sent; double kill_jetson_sent; - double is_connected; + double kill_main_sent; + double kill_by_voltage_sent; double status; double cmd_kill_all; - double cmd_kill_main; 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_; - CANLib::SocketCanBus canBus_; + uint32_t can_id; + CANLib::SocketCanBus canBus; CANLib::CanFrame can_tx_frame_; - bool can_connected_; - std::vector PowerModuleJoints_; + std::vector PowerModuleGPIOs_; int update_rate_; int logger_rate_; int logger_state_; @@ -80,9 +91,10 @@ class PowerModuleHardwareInterface : public hardware_interface::SystemInterface double elapsed_logger_time_; // Multiplexor byte - static constexpr uint8_t CMD_KILL_ALL = 0x01; - static constexpr uint8_t CMD_KILL_MAIN = 0x03; - static constexpr uint8_t CMD_KILL_JETSON = 0x05; + 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; 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 index 4d38b075..e646cbe3 100644 --- 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 @@ -15,16 +15,30 @@ void PowerModuleHardwareInterface::send_command(int can_id, int cmd_id){ frame.data.fill(0); frame.data[0] = cmd_id; frame.data[1] = CONFIRM_SEND; - canBus_.send(frame); + + RCLCPP_INFO( + rclcpp::get_logger("can_logger"), + "Inputted can_id: 0x%X | cmd_id: 0x%X | CAN Frame | id: 0x%X | dlc: %u | ext: %s | rtr: %s | data: [%02X %02X %02X %02X %02X %02X %02X %02X]", + can_id, + cmd_id, + frame.id, + frame.dlc, + frame.is_extended ? "true" : "false", + frame.is_rtr ? "true" : "false", + frame.data[0], frame.data[1], frame.data[2], frame.data[3], + frame.data[4], frame.data[5], frame.data[6], frame.data[7] + ); + + canBus.send(frame); } void PowerModuleHardwareInterface::logger_function() { - if (PowerModuleJoints_.empty()) { + if (PowerModuleGPIOs_.empty()) { return; } - const auto & joint = PowerModuleJoints_.front(); + const auto & gpio = PowerModuleGPIOs_.front(); std::ostringstream oss; oss << "\033[2J\033[H \nPowerModule Logger" << "\n--- HWI Specific ---\n" @@ -32,20 +46,21 @@ void PowerModuleHardwareInterface::logger_function() << " | 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" + << "\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: " << joint.cmd_kill_all - << " | Kill Main: " << joint.cmd_kill_main - << " | Kill Jetson: " << joint.cmd_kill_jetson << "\n" - << "Status Request: " << joint.status_request << "\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: " << joint.kill_all_sent - << " | Kill Main Sent: " << joint.kill_main_sent - << " | Kill Jetson Sent: " << joint.kill_jetson_sent << "\n" - << "Is Connected: " << joint.is_connected - << " | Status: " << joint.status << "\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()); } @@ -59,35 +74,71 @@ hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_init( 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")) : 0; + 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; - const uint32_t can_id = info_.hardware_parameters.count("can_id") ? - static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)) : 0x100; + if(info_.hardware_parameters.count("can_id")) { + try { + can_id = static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); + } catch (const std::exception & e) { + RCLCPP_ERROR(rclcpp::get_logger("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 + } + ); + } - PowerModuleJoints_.clear(); - PowerModuleJoints_.push_back(PowerModuleJoint{ - info_.gpios[0].name, - can_id, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - }); - - can_connected_ = false; elapsed_time_ = 0.0; elapsed_logger_time_ = 0.0; return hardware_interface::CallbackReturn::SUCCESS; @@ -96,17 +147,13 @@ hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_init( hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_configure( const rclcpp_lifecycle::State &) { - if (!canBus_.open( + if (!canBus.open( can_interface_, std::bind(&PowerModuleHardwareInterface::on_can_message, this, std::placeholders::_1))) { - can_connected_ = false; - } else { - can_connected_ = true; + RCLCPP_ERROR(rclcpp::get_logger("PowerModuleHardwareInterface"), "Failed to open CAN interface"); + return hardware_interface::CallbackReturn::ERROR; } - auto & joint = PowerModuleJoints_.front(); - joint.is_connected = can_connected_ ? 1.0 : 0.0; - joint.status = joint.is_connected; return hardware_interface::CallbackReturn::SUCCESS; } @@ -116,34 +163,69 @@ void PowerModuleHardwareInterface::on_can_message(const CANLib::CanFrame &) std::vector PowerModuleHardwareInterface::export_state_interfaces() { - auto & joint = PowerModuleJoints_.front(); std::vector state_interfaces; - state_interfaces.emplace_back(joint.name, "kill_all_sent", &joint.kill_all_sent); - state_interfaces.emplace_back(joint.name, "kill_main_sent", &joint.kill_main_sent); - state_interfaces.emplace_back(joint.name, "kill_jetson_sent", &joint.kill_jetson_sent); - state_interfaces.emplace_back(joint.name, "is_connected", &joint.is_connected); - state_interfaces.emplace_back(joint.name, "status", &joint.status); + 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() { - auto & joint = PowerModuleJoints_.front(); std::vector command_interfaces; - command_interfaces.emplace_back(joint.name, "kill_all", &joint.cmd_kill_all); - command_interfaces.emplace_back(joint.name, "kill_main", &joint.cmd_kill_main); - command_interfaces.emplace_back(joint.name, "kill_jetson", &joint.cmd_kill_jetson); - command_interfaces.emplace_back(joint.name, "status_request", &joint.status_request); + + 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 &) { - auto & joint = PowerModuleJoints_.front(); - joint.cmd_kill_all = 0.0; - joint.cmd_kill_main = 0.0; - joint.cmd_kill_jetson = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } @@ -156,13 +238,9 @@ hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_deactivate( hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_cleanup( const rclcpp_lifecycle::State &) { - if (can_connected_) { - canBus_.close(); - } - can_connected_ = false; - auto & joint = PowerModuleJoints_.front(); - joint.is_connected = 0.0; - joint.status = 0.0; + canBus.close(); + auto & gpio = PowerModuleGPIOs_.front(); + gpio.status = 0.0; return hardware_interface::CallbackReturn::SUCCESS; } @@ -181,7 +259,7 @@ hardware_interface::return_type PowerModuleHardwareInterface::read( hardware_interface::return_type PowerModuleHardwareInterface::write( const rclcpp::Time &, const rclcpp::Duration & period) { - auto & joint = PowerModuleJoints_.front(); + auto & gpio = PowerModuleGPIOs_.front(); elapsed_time_ += period.seconds(); elapsed_logger_time_ += period.seconds(); @@ -192,63 +270,42 @@ hardware_interface::return_type PowerModuleHardwareInterface::write( } } - if (joint.cmd_kill_all > 0.5 && joint.kill_all_sent < 0.5) { - 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_KILL_ALL; - canBus_.send(can_tx_frame_); - RCLCPP_INFO(rclcpp::get_logger("PowerModuleHardwareInterface"), "Sent Kill All Command to CAN ID 0x%X", joint.can_id); - } - - joint.kill_all_sent = 1.0; - joint.status = 1.0; - } else if (joint.cmd_kill_all < 0.5) { - joint.kill_all_sent = 0.0; + 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 (joint.cmd_kill_main > 0.5 && joint.kill_main_sent < 0.5) { - 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_KILL_MAIN; - canBus_.send(can_tx_frame_); - RCLCPP_INFO(rclcpp::get_logger("PowerModuleHardwareInterface"), "Sent Kill Main Command to CAN ID 0x%X", joint.can_id); - } - joint.kill_main_sent = 1.0; - joint.status = 1.0; - } else if (joint.cmd_kill_main < 0.5) { - joint.kill_main_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 (joint.cmd_kill_jetson > 0.5 && joint.kill_jetson_sent < 0.5) { - 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_KILL_JETSON; - canBus_.send(can_tx_frame_); - RCLCPP_INFO(rclcpp::get_logger("PowerModuleHardwareInterface"), "Sent Kill Jetson Command to CAN ID 0x%X", joint.can_id); - } - - joint.kill_jetson_sent = 1.0; - joint.status = 1.0; - } else if (joint.cmd_kill_jetson < 0.5) { - joint.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 (joint.status_request < 0.0 && joint.prev_status_request >= 0.0) { - joint.status = can_connected_ ? 1.0 : 0.0; - } 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; - joint.status = can_connected_ ? 1.0 : 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; } - joint.prev_status_request = joint.status_request; + + 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; } 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 d2f3d035..83afbe55 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 @@ -463,7 +463,6 @@ std::vector RMDHardwareInterface::export_sta iface.c_str(), joint.name.c_str()); continue; } - state_interfaces.emplace_back(hardware_interface::StateInterface( joint.name, iface, val)); } @@ -501,7 +500,6 @@ RMDHardwareInterface::export_command_interfaces() iface.c_str(), joint.name.c_str()); continue; } - command_interfaces.emplace_back(hardware_interface::CommandInterface( joint.name, iface, val)); } @@ -514,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; } 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 2fb25d69..fb3a89f2 100644 --- a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml +++ b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml @@ -72,10 +72,10 @@ 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" @@ -119,12 +119,15 @@ led_gpio_controller: command_interfaces: status_led: interfaces: - - led_command + - red + - green + - blue + - intensity + - status_request state_interfaces: status_led: interfaces: - - led_state - - is_connected + - status # PowerModule GPIO Controller Configuration power_module_gpio_controller: @@ -135,15 +138,17 @@ power_module_gpio_controller: drive_power_module: interfaces: - kill_all - - kill_main - kill_jetson + - kill_main + - kill_by_voltage + - status_request state_interfaces: drive_power_module: interfaces: - kill_all_sent - - kill_main_sent - kill_jetson_sent - - is_connected + - kill_main_sent + - kill_by_voltage_sent motor_status_controller: ros__parameters: @@ -164,10 +169,10 @@ rear_ackermann_controller: - "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/general/README.md b/src/subsystems/general/README.md index 931e2894..cb251d22 100644 --- a/src/subsystems/general/README.md +++ b/src/subsystems/general/README.md @@ -3,8 +3,18 @@ ros2 service call /motor_status_controller/status_request msgs/srv/StatusReq "{j ros2 service call /motor_status_controller/maintenance_request msgs/srv/MaintenanceReq "{ joint_name: propulsion_fl_joint, request_rate: -1, - command_id: 0x78, + command_id: 0x81, i32_data: [], i16_data: [], u8_data: [] -}" \ No newline at end of file +}" + +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 +" From bde369c178201a4f099c4d63418c5164e49fa9e1 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Wed, 29 Apr 2026 04:14:32 -0400 Subject: [PATCH 19/37] Power module and SMCs seem to be working, initial implementation of LED HWI created --- .../drive/drive.led.ros2_control.xacro | 1 + .../led_hardware_interface.hpp | 20 +++ .../src/led_hardware_interface.cpp | 144 +++++++++++++---- .../src/power_module_hardware_interface.cpp | 87 +++++----- .../src/rmd_hardware_interface.cpp | 7 +- .../smc_hardware_interface.hpp | 4 +- .../src/smc_hardware_interface.cpp | 148 ++++++++++++++---- .../umdloop_can_library/src/SocketCanBus.cpp | 2 +- src/subsystems/general/README.md | 9 ++ 9 files changed, 299 insertions(+), 123 deletions(-) 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 619ad55b..fff377cc 100644 --- a/src/description/ros2_control/drive/drive.led.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.led.ros2_control.xacro @@ -15,6 +15,7 @@ true + 0 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 094256b6..0bdaf5a8 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 @@ -4,6 +4,9 @@ #include #include #include +#include +#include +#include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -52,6 +55,7 @@ class LEDHardwareInterface : public hardware_interface::SystemInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; // -- Helper Functions -- + void send_command(int can_id, int cmd_id); void logger_function(); private: @@ -67,6 +71,11 @@ class LEDHardwareInterface : public hardware_interface::SystemInterface 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; + uint32_t node_id; std::vector state_interface_names; std::vector command_interface_names; @@ -77,12 +86,23 @@ class LEDHardwareInterface : public hardware_interface::SystemInterface 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 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 51df592a..1f040468 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 @@ -6,10 +6,24 @@ #include "pluginlib/class_list_macros.hpp" #include "rclcpp/rclcpp.hpp" +#include +#include +#include namespace led_ros2_control { +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; + + canBus.send(frame); +} + void LEDHardwareInterface::logger_function() { if (LEDGPIOs_.empty()) { @@ -18,13 +32,15 @@ void LEDHardwareInterface::logger_function() const auto & gpio = LEDGPIOs_.front(); std::ostringstream oss; - oss << "\033[2J\033[H \nLED Logger" + oss << "\033[2J\033[H \nLED Logger" << "\n--- HWI Specific ---\n" - << "HWI Update Rate: " << update_rate_ + << "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 << "\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 @@ -54,11 +70,15 @@ hardware_interface::CallbackReturn LEDHardwareInterface::on_init( std::stoi(info_.hardware_parameters.at("logger_rate")) : 0; logger_state_ = info_.hardware_parameters.count("logger_state") ? std::stoi(info_.hardware_parameters.at("logger_state")) : 0; - if(info_.hardware_parameters.count("can_id")) { - const uint32_t can_id = static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); - } - else { - RCLCPP_ERROR(rclcpp::get_logger("PowerModuleHardwareInterface"), "CAN ID parameter 'can_id' is missing or invalid in hardware configuration."); + 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 { + RCLCPP_ERROR(rclcpp::get_logger("LEDHardwareInterface"), "CAN ID parameter 'can_id' is missing in hardware configuration."); return hardware_interface::CallbackReturn::ERROR; } @@ -80,11 +100,27 @@ hardware_interface::CallbackReturn LEDHardwareInterface::on_init( 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"), nullptr, 0)); + } 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()); + } + } LEDGPIOs_.push_back( LEDGPIO{ - .name = info_.gpios[0].name, - .is_rgb = false, + .name = gpio.name, + .is_rgb = is_rgb_flag, + .node_id = gpio_node_id, .status = 0.0, .intensity = 0.0, .red_command = 0.0, @@ -93,6 +129,10 @@ hardware_interface::CallbackReturn LEDHardwareInterface::on_init( .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, @@ -142,31 +182,29 @@ std::vector LEDHardwareInterface::export_c for (auto & gpio : LEDGPIOs_) { for (auto & iface : gpio.command_interface_names) { double * val = nullptr; - 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 if (iface == "intensity") { - if(gpio.is_rgb) { - RCLCPP_ERROR( - rclcpp::get_logger("LEDHardwareInterface"), - "Intensity command interface is not supported for RGB GPIO '%s'", - gpio.name.c_str()); - continue; + // 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 { + } else { + if (iface == "intensity") { val = &gpio.intensity; + } else if (iface == "status_request") { + val = &gpio.status_request; + } else { + // skip rgb channels when not rgb + continue; } - } else { - RCLCPP_WARN( - rclcpp::get_logger("LEDHardwareInterface"), - "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)); @@ -230,6 +268,48 @@ hardware_interface::return_type LEDHardwareInterface::write( } } 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) { + auto clamp_u8 = [](double v) -> uint8_t { + return static_cast(std::clamp(static_cast(std::round(v)), 0, 255)); + }; + frame.data[0] = static_cast(CMD_RGB + static_cast(gpio.node_id & 0xFF)); + frame.data[1] = clamp_u8(r); + frame.data[2] = clamp_u8(g); + frame.data[3] = clamp_u8(b); + 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) { + auto clamp_u8 = [](double v) -> uint8_t { + return static_cast(std::clamp(static_cast(std::round(v)), 0, 255)); + }; + frame.data[0] = static_cast(CMD_INTENSITY + static_cast(gpio.node_id & 0xFF)); + frame.data[1] = clamp_u8(intensity); + 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]); + } + } return hardware_interface::return_type::OK; } 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 index e646cbe3..09940f14 100644 --- 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 @@ -16,19 +16,6 @@ void PowerModuleHardwareInterface::send_command(int can_id, int cmd_id){ frame.data[0] = cmd_id; frame.data[1] = CONFIRM_SEND; - RCLCPP_INFO( - rclcpp::get_logger("can_logger"), - "Inputted can_id: 0x%X | cmd_id: 0x%X | CAN Frame | id: 0x%X | dlc: %u | ext: %s | rtr: %s | data: [%02X %02X %02X %02X %02X %02X %02X %02X]", - can_id, - cmd_id, - frame.id, - frame.dlc, - frame.is_extended ? "true" : "false", - frame.is_rtr ? "true" : "false", - frame.data[0], frame.data[1], frame.data[2], frame.data[3], - frame.data[4], frame.data[5], frame.data[6], frame.data[7] - ); - canBus.send(frame); } @@ -239,8 +226,9 @@ hardware_interface::CallbackReturn PowerModuleHardwareInterface::on_cleanup( const rclcpp_lifecycle::State &) { canBus.close(); - auto & gpio = PowerModuleGPIOs_.front(); - gpio.status = 0.0; + for (auto & gpio : PowerModuleGPIOs_) { + gpio.status = 0.0; + } return hardware_interface::CallbackReturn::SUCCESS; } @@ -259,8 +247,6 @@ hardware_interface::return_type PowerModuleHardwareInterface::read( hardware_interface::return_type PowerModuleHardwareInterface::write( const rclcpp::Time &, const rclcpp::Duration & period) { - auto & gpio = PowerModuleGPIOs_.front(); - elapsed_time_ += period.seconds(); elapsed_logger_time_ += period.seconds(); if (logger_rate_ > 0 && elapsed_logger_time_ > (1.0 / static_cast(logger_rate_))) { @@ -270,43 +256,44 @@ hardware_interface::return_type PowerModuleHardwareInterface::write( } } - 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; - } + 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_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_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_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; - } + 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; + } - 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); + 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; } 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 83afbe55..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 @@ -693,8 +693,6 @@ hardware_interface::return_type RMDHardwareInterface::read( hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - uint8_t data[8] = {0x00}; - // Logger update elapsed_time+=period.seconds(); elapsed_logger_time+=period.seconds(); @@ -744,8 +742,7 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( }; // Call it and store the result - joint.maintenance_frame = doubles_to_payload(joint.maintenance_frame_high, - joint.maintenance_frame_low); + 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)); @@ -754,7 +751,6 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( 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()); - joint.prev_maintenance_req = joint.motor_maintenance_req; continue; } @@ -785,7 +781,6 @@ hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( if(elapsed_update_time > update_period){ elapsed_update_time = 0.0; for(auto & joint : RMDJoints_) { - std::fill(std::begin(data), std::end(data), 0x00); can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated can_tx_frame_.id = joint.node_write_id; can_tx_frame_.dlc = 8; 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 91342eb8..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 @@ -165,6 +165,7 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface int logger_state; // Logger on/off state int num_joints; + int state_iterator; CANLib::SocketCanBus canBus; CANLib::CanFrame can_tx_frame_; @@ -176,6 +177,7 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface enum class MaintenanceCommands : uint8_t { WRITE_ACCELERATION_CMD = 0x34, + READ_SETTINGS_CMD = 0x40, WRITE_SETTINGS_TO_RAM_CMD = 0x42, WRITE_SETTINGS_TO_ROM_CMD = 0x44, MOTOR_SHUTDOWN_CMD = 0x80, @@ -195,7 +197,6 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface enum class StatusCommands : uint8_t { READ_ACCELERATION_CMD = 0x33, - READ_SETTINGS_CMD = 0x40, READ_ENCODER_CMD = 0x90, READ_ABS_ANGLE_CMD = 0x92, MOTOR_STATUS_1_CMD = 0x9A, @@ -217,7 +218,6 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface static constexpr std::array kStatusCommands = { StatusCommands::READ_ACCELERATION_CMD, - StatusCommands::READ_SETTINGS_CMD, StatusCommands::READ_ENCODER_CMD, StatusCommands::READ_ABS_ANGLE_CMD, StatusCommands::MOTOR_STATUS_1_CMD, 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 30c2ce3e..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 @@ -51,47 +51,116 @@ void SMCHardwareInterface::send_command(int can_id, int cmd_id) void SMCHardwareInterface::logger_function() { - if (num_joints == 0) { + // Prevent breaking the logger + if (SMCJoints_.empty()) { return; } + std::string log_msg = "\033[2J\033[H \nSMC Logger"; + std::string control_mode = ""; + std::ostringstream oss; - oss << "\033[2J\033[H \nSMC Logger" - << "\n--- HWI Specific ---\n" + oss << "\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 (const auto & joint : SMCJoints_) { - std::string control_mode = "UNDEFINED"; + for (auto & joint : SMCJoints_) { + if (joint.control_level == integration_level_t::POSITION) { control_mode = "POSITION"; } else if (joint.control_level == integration_level_t::VELOCITY) { control_mode = "VELOCITY"; + } else { + control_mode = "UNDEFINED"; } - oss << "\nJOINT: " << joint.name << "\n" - << "Parameters: CAN ID: 0x" << std::hex << std::uppercase << joint.node_id << std::dec - << " | Gear Ratio: " << joint.gear_ratio << "\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: " << 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 << "\n" - << "Motor Temperature: " << joint.motor_temperature - << " | Torque Current: " << joint.motor_torque_current - << " | Status: " << joint.motor_status << "\n"; - } - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "%s", oss.str().c_str()); + << "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(); + } } bool SMCHardwareInterface::interpret_settings_parameters( @@ -158,8 +227,7 @@ void SMCHardwareInterface::format_control_command(CANLib::CanFrame & frame, SMCJ joint.prev_joint_command_position = joint.joint_command_position; return; } - - if ( + else if ( joint.control_level == integration_level_t::VELOCITY && std::isfinite(joint.joint_command_velocity) && joint.joint_command_velocity != joint.prev_joint_command_velocity) @@ -176,8 +244,12 @@ void SMCHardwareInterface::format_control_command(CANLib::CanFrame & frame, SMCJ joint.prev_joint_command_velocity = joint.joint_command_velocity; return; } - - frame.data[0] = static_cast(StatusCommands::MOTOR_STATUS_2_CMD); + 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( @@ -197,6 +269,14 @@ bool SMCHardwareInterface::format_maintenance_command( 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 @@ -384,6 +464,7 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_init( } num_joints = static_cast(SMCJoints_.size()); + state_iterator = 0; elapsed_update_time = 0.0; elapsed_time = 0.0; elapsed_logger_time = 0.0; @@ -514,18 +595,12 @@ void SMCHardwareInterface::on_can_message(const CANLib::CanFrame & frame) continue; } - if (frame.data[0] == static_cast(StatusCommands::READ_SETTINGS_CMD)) { - const std::array data = frame.data; - interpret_settings_parameters(joint, data); - continue; - } - 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]); } - if(frame.data[0] == static_cast(StatusCommands::READ_SETTINGS_CMD)) { + 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()); } @@ -579,6 +654,7 @@ hardware_interface::return_type SMCHardwareInterface::read( hardware_interface::return_type SMCHardwareInterface::write( const rclcpp::Time &, const rclcpp::Duration & period) { + // Logger update elapsed_time += period.seconds(); elapsed_logger_time += period.seconds(); if (logger_rate > 0 && elapsed_logger_time > (1.0 / static_cast(logger_rate))) { @@ -588,6 +664,7 @@ hardware_interface::return_type SMCHardwareInterface::write( } } + // 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) @@ -611,6 +688,7 @@ hardware_interface::return_type SMCHardwareInterface::write( 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); @@ -629,24 +707,30 @@ hardware_interface::return_type SMCHardwareInterface::write( frame.id = joint.node_id; frame.dlc = 8; if (!format_maintenance_command(frame, decoded_maintenance_cmd)) { - joint.prev_maintenance_req = joint.motor_maintenance_req; + // 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) { + 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 elapsed_update_time += period.seconds(); if (update_rate > 0 && elapsed_update_time > (1.0 / static_cast(update_rate))) { elapsed_update_time = 0.0; 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/subsystems/general/README.md b/src/subsystems/general/README.md index cb251d22..af2df1ee 100644 --- a/src/subsystems/general/README.md +++ b/src/subsystems/general/README.md @@ -9,6 +9,15 @@ ros2 service call /motor_status_controller/maintenance_request msgs/srv/Maintena 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 From abcf1dc78ed4a819732fc83ee9d174bafba6a37b Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Wed, 29 Apr 2026 21:14:39 -0400 Subject: [PATCH 20/37] led hwi looks like its working --- .../drive/drive.led.ros2_control.xacro | 4 +- src/description/urdf/athena_drive.urdf.xacro | 2 +- .../led_hardware_interface.hpp | 4 +- .../src/led_hardware_interface.cpp | 106 +++++++++--------- .../config/athena_drive_controllers.yaml | 11 +- .../launch/athena_drive.launch.py | 2 +- src/subsystems/general/README.md | 10 ++ 7 files changed, 72 insertions(+), 67 deletions(-) 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 fff377cc..c3d91abe 100644 --- a/src/description/ros2_control/drive/drive.led.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.led.ros2_control.xacro @@ -10,7 +10,7 @@ 0x120 10 5 - 0 + 1 @@ -20,7 +20,7 @@ - + diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index 80c66f07..11119ab5 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -78,7 +78,7 @@ - + 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 0bdaf5a8..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 @@ -63,6 +63,7 @@ class LEDHardwareInterface : public hardware_interface::SystemInterface { std::string name; bool is_rgb; + uint32_t node_id; double status; double intensity; double red_command; @@ -75,7 +76,6 @@ class LEDHardwareInterface : public hardware_interface::SystemInterface double prev_red_command; double prev_green_command; double prev_blue_command; - uint32_t node_id; std::vector state_interface_names; std::vector command_interface_names; @@ -83,7 +83,7 @@ class LEDHardwareInterface : public hardware_interface::SystemInterface }; std::string can_interface_; - CANLib::SocketCanBus canBus_; + CANLib::SocketCanBus canBus; CANLib::CanFrame can_tx_frame_; // CAN bus ID (constant for this HWI) 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 1f040468..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 @@ -30,8 +30,8 @@ void LEDHardwareInterface::logger_function() return; } - const auto & gpio = LEDGPIOs_.front(); - std::ostringstream oss; + for (const auto & gpio : LEDGPIOs_) { + std::ostringstream oss; oss << "\033[2J\033[H \nLED Logger" << "\n--- HWI Specific ---\n" << "CAN Interface: " << can_interface_ @@ -49,7 +49,8 @@ void LEDHardwareInterface::logger_function() << "-- State --\n" << "Status: " << gpio.status << "\n"; - RCLCPP_INFO(rclcpp::get_logger("LEDHardwareInterface"), "%s", oss.str().c_str()); + RCLCPP_INFO(rclcpp::get_logger("LEDHardwareInterface"), "%s", oss.str().c_str()); + } } hardware_interface::CallbackReturn LEDHardwareInterface::on_init( @@ -110,7 +111,7 @@ hardware_interface::CallbackReturn LEDHardwareInterface::on_init( 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"), nullptr, 0)); + 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()); } @@ -229,8 +230,9 @@ hardware_interface::CallbackReturn LEDHardwareInterface::on_deactivate( hardware_interface::CallbackReturn LEDHardwareInterface::on_cleanup( const rclcpp_lifecycle::State &) { - auto & gpio = LEDGPIOs_.front(); - gpio.status = 0.0; + for (auto & gpio : LEDGPIOs_) { + gpio.status = 0.0; + } return hardware_interface::CallbackReturn::SUCCESS; } @@ -249,8 +251,6 @@ hardware_interface::return_type LEDHardwareInterface::read( hardware_interface::return_type LEDHardwareInterface::write( const rclcpp::Time &, const rclcpp::Duration & period) { - auto & gpio = LEDGPIOs_.front(); - elapsed_time_ += period.seconds(); elapsed_logger_time_ += period.seconds(); if (logger_rate_ > 0 && elapsed_logger_time_ > (1.0 / static_cast(logger_rate_))) { @@ -260,54 +260,50 @@ hardware_interface::return_type LEDHardwareInterface::write( } } - 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) { - auto clamp_u8 = [](double v) -> uint8_t { - return static_cast(std::clamp(static_cast(std::round(v)), 0, 255)); - }; - frame.data[0] = static_cast(CMD_RGB + static_cast(gpio.node_id & 0xFF)); - frame.data[1] = clamp_u8(r); - frame.data[2] = clamp_u8(g); - frame.data[3] = clamp_u8(b); - 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_); + 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; + } } - } else { - const double intensity = gpio.intensity; - const bool changed = std::isnan(gpio.prev_intensity) || intensity != gpio.prev_intensity; - if (changed) { - auto clamp_u8 = [](double v) -> uint8_t { - return static_cast(std::clamp(static_cast(std::round(v)), 0, 255)); - }; - frame.data[0] = static_cast(CMD_INTENSITY + static_cast(gpio.node_id & 0xFF)); - frame.data[1] = clamp_u8(intensity); - 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]); + 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]); + } } } 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 fb3a89f2..4eba18d9 100644 --- a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml +++ b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml @@ -42,7 +42,7 @@ controller_manager: # type: drive_controllers/SwerveDriveController # LED GPIO Controller - led_gpio_controller: + drive_led_gpio_controller: type: gpio_controllers/GpioCommandController # PowerModule GPIO Controller @@ -112,20 +112,19 @@ 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: - red - green - blue - - intensity - status_request state_interfaces: - status_led: + drive_led: interfaces: - status 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 12ec7d5d..101af9d2 100644 --- a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py +++ b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py @@ -256,7 +256,7 @@ def launch_setup(context, *args, **kwargs): ) ] - gpio_controller_names = ["led_gpio_controller", "power_module_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 += [ diff --git a/src/subsystems/general/README.md b/src/subsystems/general/README.md index af2df1ee..1307c14c 100644 --- a/src/subsystems/general/README.md +++ b/src/subsystems/general/README.md @@ -27,3 +27,13 @@ interface_values: 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 +" From 4b683252599bba0ce0de9c676f2d43b6614aa3a8 Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Fri, 1 May 2026 20:41:16 -0400 Subject: [PATCH 21/37] added limit switch and rotary encoder HWI, included all necessary HWIs, needs fixing --- .../arm.base_limit_switch.ros2_control.xacro | 27 ++ ...d_effector_limit_switch.ros2_control.xacro | 33 ++ .../arm/arm.rotary_encoder.ros2_control.xacro | 39 +++ .../arm/arm.servo.ros2_control.xacro | 36 +++ .../science/science.led.ros2_control.xacro | 23 ++ .../science.limit_switch.ros2_control.xacro | 21 ++ .../science/science.servo.ros2_control.xacro | 2 +- src/description/urdf/athena_arm.urdf.xacro | 12 +- .../urdf/athena_science.urdf.xacro | 8 +- .../limit_switch_ros2_control/CMakeLists.txt | 64 ++++ .../limit_switch_hardware_interface.hpp | 95 ++++++ .../limit_switch_hardware_interface.xml | 9 + .../limit_switch_ros2_control/package.xml | 29 ++ .../src/limit_switch_hardware_interface.cpp | 260 +++++++++++++++ .../CMakeLists.txt | 64 ++++ .../rotary_encoder_hardware_interface.hpp | 99 ++++++ .../rotary_encoder_ros2_control/package.xml | 29 ++ .../rotary_encoder_hardware_interface.xml | 9 + .../src/rotary_encoder_hardware_interface.cpp | 295 ++++++++++++++++++ .../config/athena_arm_controllers.yaml | 65 ++++ src/subsystems/arm/arm_bringup/package.xml | 4 + .../config/athena_science_controllers.yaml | 37 ++- .../science/science_bringup/package.xml | 1 + 23 files changed, 1257 insertions(+), 4 deletions(-) create mode 100644 src/description/ros2_control/arm/arm.base_limit_switch.ros2_control.xacro create mode 100644 src/description/ros2_control/arm/arm.end_effector_limit_switch.ros2_control.xacro create mode 100644 src/description/ros2_control/arm/arm.rotary_encoder.ros2_control.xacro create mode 100644 src/description/ros2_control/science/science.led.ros2_control.xacro create mode 100644 src/description/ros2_control/science/science.limit_switch.ros2_control.xacro create mode 100644 src/hardware_interfaces/limit_switch_ros2_control/CMakeLists.txt create mode 100644 src/hardware_interfaces/limit_switch_ros2_control/include/limit_switch_ros2_control/limit_switch_hardware_interface.hpp create mode 100644 src/hardware_interfaces/limit_switch_ros2_control/limit_switch_hardware_interface.xml create mode 100644 src/hardware_interfaces/limit_switch_ros2_control/package.xml create mode 100644 src/hardware_interfaces/limit_switch_ros2_control/src/limit_switch_hardware_interface.cpp create mode 100644 src/hardware_interfaces/rotary_encoder_ros2_control/CMakeLists.txt create mode 100644 src/hardware_interfaces/rotary_encoder_ros2_control/include/rotary_encoder_ros2_control/rotary_encoder_hardware_interface.hpp create mode 100644 src/hardware_interfaces/rotary_encoder_ros2_control/package.xml create mode 100644 src/hardware_interfaces/rotary_encoder_ros2_control/rotary_encoder_hardware_interface.xml create mode 100644 src/hardware_interfaces/rotary_encoder_ros2_control/src/rotary_encoder_hardware_interface.cpp 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..29b0dc3a --- /dev/null +++ b/src/description/ros2_control/arm/arm.base_limit_switch.ros2_control.xacro @@ -0,0 +1,27 @@ + + + + + + limit_switch_ros2_control/LimitSwitchHardwareInterface + can0 + 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..ff17d9ff --- /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 + can0 + 0x60 + 10 + 5 + 0 + + + + 2 + + + + + + 3 + + + + + + 4 + + + + + + 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..c5451c01 --- /dev/null +++ b/src/description/ros2_control/arm/arm.rotary_encoder.ros2_control.xacro @@ -0,0 +1,39 @@ + + + + + + rotary_encoder_ros2_control/RotaryEncoderHardwareInterface + can0 + 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 0a1fbb7f..b83f19fb 100644 --- a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro @@ -29,6 +29,42 @@ + + + 0x2 + 1 + 180 + standard + revolute + + + + + + + + + + + + + + 0x3 + 1 + 180 + standard + revolute + + + + + + + + + + + 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.servo.ros2_control.xacro b/src/description/ros2_control/science/science.servo.ros2_control.xacro index 700361b7..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 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_science.urdf.xacro b/src/description/urdf/athena_science.urdf.xacro index b81b61e3..608d8163 100644 --- a/src/description/urdf/athena_science.urdf.xacro +++ b/src/description/urdf/athena_science.urdf.xacro @@ -10,6 +10,8 @@ + + @@ -35,6 +37,10 @@ + + + + - \ No newline at end of file + 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/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/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml index 5152b611..0f6015c1 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -31,6 +31,15 @@ controller_manager: motor_status_controller: type: general_controllers/MotorStatusController + + cam_position_controller: + type: position_controllers/JointGroupPositionController + + rotary_encoder_state_request_controller: + type: forward_command_controller/ForwardCommandController + + limit_switch_gpio_controller: + type: gpio_controllers/GpioCommandController twodof_joint_trajectory_controller: @@ -132,6 +141,62 @@ 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 + +limit_switch_gpio_controller: + ros__parameters: + gpios: + - limit_switch_0 + - limit_switch_1 + - limit_switch_2 + - limit_switch_3 + - limit_switch_4 + command_interfaces: + 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: + 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 + manual_arm_cylindrical_controller: ros__parameters: joints: diff --git a/src/subsystems/arm/arm_bringup/package.xml b/src/subsystems/arm/arm_bringup/package.xml index 9075e4e9..75ded218 100644 --- a/src/subsystems/arm/arm_bringup/package.xml +++ b/src/subsystems/arm/arm_bringup/package.xml @@ -26,6 +26,10 @@ controller_manager joint_state_broadcaster + forward_command_controller + gpio_controllers + position_controllers + velocity_controllers joint_state_publisher joint_state_publisher_gui launch_ros 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 95a8b089..8a93a6c2 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -19,6 +19,12 @@ controller_manager: laser_gpio_controller: type: gpio_controllers/GpioCommandController + fluoro_led_gpio_controller: + type: gpio_controllers/GpioCommandController + + science_limit_switch_gpio_controller: + type: gpio_controllers/GpioCommandController + motor_status_controller: type: general_controllers/MotorStatusController @@ -93,6 +99,35 @@ laser_gpio_controller: - temperature - is_connected +fluoro_led_gpio_controller: + ros__parameters: + gpios: + - fluoro_led + command_interfaces: + fluoro_led: + interfaces: + - red + - green + - blue + - status_request + state_interfaces: + fluoro_led: + interfaces: + - status + +science_limit_switch_gpio_controller: + ros__parameters: + gpios: + - science_limit_switch + command_interfaces: + science_limit_switch: + interfaces: + - status_request + state_interfaces: + science_limit_switch: + interfaces: + - status + motor_status_controller: ros__parameters: joints: @@ -103,4 +138,4 @@ motor_status_controller: interfaces: - motor_temperature - torque_current - publish_rate: 10.0 \ No newline at end of file + publish_rate: 10.0 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 From d2a82cd7b9de462b9ec32a6303fcc63ddcb045f5 Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Sat, 2 May 2026 14:28:43 -0400 Subject: [PATCH 22/37] fixed motor status controller, manually choose command and state interfaces if default don't apply --- .../drive/drive.led.ros2_control.xacro | 2 +- .../config/athena_arm_controllers.yaml | 25 ++- .../arm_bringup/launch/athena_arm.launch.py | 15 +- .../config/athena_drive_controllers.yaml | 22 +- .../config/motor_status_controller.yaml | 30 ++- .../src/motor_status_controller.cpp | 188 +++++++++--------- .../config/athena_science_controllers.yaml | 70 ++++++- .../launch/athena_science.launch.py | 2 +- 8 files changed, 229 insertions(+), 125 deletions(-) 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 c3d91abe..24628c1d 100644 --- a/src/description/ros2_control/drive/drive.led.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.led.ros2_control.xacro @@ -10,7 +10,7 @@ 0x120 10 5 - 1 + 0 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 0f6015c1..bc30db97 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -261,6 +261,17 @@ manual_end_effector_gripper_claw_controller: joint_max_positions: - 0.03 # Actuator max displacement +# 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: @@ -270,8 +281,16 @@ motor_status_controller: - wrist_pitch - wrist_roll - actuator + - cam_0 + - cam_1 state_interfaces: - - motor_temperature - - torque_current - - status + actuator: + interfaces: + - status + cam_0: + interfaces: + - status + cam_1: + 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 b7e6c888..d060806d 100644 --- a/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py +++ b/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py @@ -312,7 +312,20 @@ def launch_setup(context, *args, **kwargs): arguments=[controller, "-c", "/controller_manager", "--inactive"], ) ] - + + active_robot_controller_names = [ + "limit_switch_gpio_controller", + "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], 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 4eba18d9..016e996f 100644 --- a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml +++ b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml @@ -77,8 +77,8 @@ front_ackermann_controller: - 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 @@ -149,6 +149,17 @@ power_module_gpio_controller: - 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: @@ -156,17 +167,14 @@ motor_status_controller: - propulsion_fr_joint - propulsion_bl_joint - propulsion_br_joint - interfaces: - - motor_temperature - - torque_current 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 diff --git a/src/subsystems/general/general_controllers/config/motor_status_controller.yaml b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml index dc6f8a86..9b618c26 100644 --- a/src/subsystems/general/general_controllers/config/motor_status_controller.yaml +++ b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml @@ -4,16 +4,26 @@ motor_status_controller: default_value: [], description: "Names of the joints to monitor for motor telemetry." } - command_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." - } - state_interfaces: { - type: string_array, - default_value: ["motor_temperature", "torque_current", "status"], - description: "State interface names to read from each joint." - } + 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 + } + } + 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 + } + } publish_rate: { type: double, default_value: 10.0, diff --git a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp index 2fcdfbe1..9ab85150 100644 --- a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp +++ b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp @@ -34,51 +34,51 @@ void MotorStatusController::logger_function() // 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()); - } + // 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()); @@ -109,14 +109,12 @@ MotorStatusController::command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - - command_interfaces_config.names.reserve(params_.joints.size() * params_.command_interfaces.size()); - for (const auto & joint : params_.joints) - { - for (const auto & iface : params_.command_interfaces) { - command_interfaces_config.names.push_back(joint + "/" + iface); + 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); } } + return command_interfaces_config; } @@ -124,10 +122,12 @@ controller_interface::InterfaceConfiguration MotorStatusController::state_interface_configuration() const { controller_interface::InterfaceConfiguration state_interfaces_config; - - // Request every available state interface - state_interfaces_config.type = - controller_interface::interface_configuration_type::ALL; + 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); + } + } return state_interfaces_config; } @@ -142,10 +142,10 @@ controller_interface::CallbackReturn MotorStatusController::on_configure( return controller_interface::CallbackReturn::ERROR; } - if (params_.state_interfaces.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "No interfaces 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) { @@ -237,8 +237,8 @@ controller_interface::CallbackReturn MotorStatusController::on_configure( RCLCPP_INFO( get_node()->get_logger(), - "Configured motor_status_controller for %zu joints, %zu state interfaces each, publish rate: %.1f Hz", - params_.joints.size(), params_.state_interfaces.size(), params_.publish_rate); + "Configured motor_status_controller for %zu joints at publish rate: %.1f Hz", + params_.joints.size(), params_.publish_rate); return controller_interface::CallbackReturn::SUCCESS; } @@ -253,14 +253,14 @@ controller_interface::CallbackReturn MotorStatusController::on_activate( 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); - } - } - } + // 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(); @@ -269,14 +269,14 @@ controller_interface::CallbackReturn MotorStatusController::on_activate( 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); - } - } - } + // 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(); @@ -306,26 +306,25 @@ controller_interface::return_type MotorStatusController::update( msgs::msg::SystemInfo system_info_msg; system_info_msg.header.stamp = time; - for (const auto & joint : params_.joints) { - - // Command Interfaces - for (const auto & iface : params_.command_interfaces) { - std::string key = joint + "/" + iface; + // Command Interfaces + for (const auto & [joint_name, joint_cfg] : params_.command_interfaces.joints_map){ + for(const auto & interface_name : joint_cfg.interfaces){ + std::string key = joint_name + "/" + interface_name; auto it = command_interface_map_.find(key); if (it != command_interface_map_.end()) { - if (iface == "maintenance_frame_high" && joint == maintenance_req_joint_name) { + if (interface_name == "maintenance_frame_high" && joint_name == maintenance_req_joint_name) { command_interfaces_[it->second].set_value(maintenance_frame_high); } - if (iface == "maintenance_frame_low" && joint == maintenance_req_joint_name) { + if (interface_name == "maintenance_frame_low" && joint_name == maintenance_req_joint_name) { command_interfaces_[it->second].set_value(maintenance_frame_low); } - if(iface == "maintenance_data_count" && joint == maintenance_req_joint_name) { + if(interface_name == "maintenance_data_count" && joint_name == maintenance_req_joint_name) { command_interfaces_[it->second].set_value(maintenance_data_count); } - if (iface == "status_request" && joint == status_req_joint_name) { + if (interface_name == "status_request" && joint_name == status_req_joint_name) { // Controller must turn status request back to 0 if it's a one-shot request if (status_request_rate < 0 && status_one_shot_sent == false) { command_interfaces_[it->second].set_value(status_request_rate); @@ -344,7 +343,7 @@ controller_interface::return_type MotorStatusController::update( } } - if (iface == "maintenance_request" && joint == maintenance_req_joint_name) { + if (interface_name == "maintenance_request" && joint_name == maintenance_req_joint_name) { // Controller must turn maintenance request back to 0 if it's a one-shot request if (maintenance_request_rate < 0 && maintenance_one_shot_sent == false) { command_interfaces_[it->second].set_value(maintenance_request_rate); @@ -364,28 +363,29 @@ controller_interface::return_type MotorStatusController::update( } } } + } // State Interfaces msgs::msg::JointStatus status; - status.joint_name = joint; // Initializing values in case state interfaces don't exist for them 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 & iface : params_.state_interfaces) { - std::string key = joint + "/" + iface; + for (const auto & [joint_name, joint_cfg] : params_.state_interfaces.joints_map){ + for(const auto & interface_name : joint_cfg.interfaces){ + status.joint_name = joint_name; + std::string key = joint_name + "/" + interface_name; auto it = state_interface_map_.find(key); if (it != state_interface_map_.end()) { double value = state_interfaces_[it->second].get_value(); - if (iface == "motor_temperature") { + if (interface_name == "motor_temperature") { status.temperature = static_cast(value); - } else if (iface == "torque_current") { + } else if (interface_name == "torque_current") { status.torque_current = value; - } else if (iface == "status") { + } 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"); 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 8a93a6c2..4ec4719f 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -15,7 +15,6 @@ controller_manager: science_controller: type: science_controllers/ScienceManual - # Laser GPIO Controller (CAN-based) laser_gpio_controller: type: gpio_controllers/GpioCommandController @@ -92,6 +91,7 @@ laser_gpio_controller: spectrometry_laser: interfaces: - laser_command + - status_request state_interfaces: spectrometry_laser: interfaces: @@ -106,9 +106,7 @@ fluoro_led_gpio_controller: command_interfaces: fluoro_led: interfaces: - - red - - green - - blue + - intensity - status_request state_interfaces: fluoro_led: @@ -128,14 +126,70 @@ science_limit_switch_gpio_controller: interfaces: - status +# 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 + - servo_rack_and_pinion_l + - servo_rack_and_pinion_r + - servo_scoop_a + - servo_scoop_b - talon_scoop - interfaces: - - motor_temperature - - torque_current + - servo_sampler_lift_l + - servo_sampler_lift_r + - talon_auger + - servo_auger_lift + command_interfaces: + talon_scoop: + interfaces: + - status_request + talon_auger: + interfaces: + - status_request + 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 + talon_scoop: + interfaces: + - status + servo_sampler_lift_l: + interfaces: + - status + servo_sampler_lift_r: + interfaces: + - status + talon_auger: + interfaces: + - status + servo_auger_lift: + interfaces: + - status publish_rate: 10.0 diff --git a/src/subsystems/science/science_bringup/launch/athena_science.launch.py b/src/subsystems/science/science_bringup/launch/athena_science.launch.py index 09d429f5..985eb1a2 100644 --- a/src/subsystems/science/science_bringup/launch/athena_science.launch.py +++ b/src/subsystems/science/science_bringup/launch/athena_science.launch.py @@ -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", "science_limit_switch_gpio_controller"] gpio_controller_spawners = [] for controller in gpio_controller_names: gpio_controller_spawners += [ From 2eb57c9068b8046e6d31829eab79408a02ec5211 Mon Sep 17 00:00:00 2001 From: duttaishan01 Date: Sat, 2 May 2026 15:08:51 -0400 Subject: [PATCH 23/37] 3dof specific fixes --- .../arm/arm.smc_3dof.ros2_control.xacro | 7 +++ .../arm/arm.talon_3dof.ros2_control.xacro | 10 ++++ .../config/athena_arm_controllers.yaml | 50 ++++++++++++++++++- .../arm_bringup/launch/athena_arm.launch.py | 12 ++++- 4 files changed, 75 insertions(+), 4 deletions(-) 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..9bb720c0 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 @@ -17,11 +17,18 @@ 300 + + + + + + + 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..756a961b 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 @@ -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/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml index bc30db97..31e4e05b 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -29,7 +29,10 @@ controller_manager: manual_end_effector_gripper_claw_controller: type: arm_controllers/ManualEndEffectorGripperClawController - motor_status_controller: + two_dof_motor_status_controller: + type: general_controllers/MotorStatusController + + three_dof_motor_status_controller: type: general_controllers/MotorStatusController cam_position_controller: @@ -272,7 +275,7 @@ manual_end_effector_gripper_claw_controller: # - motor_temperature # - torque_current # - status -motor_status_controller: +two_dof_motor_status_controller: ros__parameters: joints: - base_yaw @@ -281,8 +284,51 @@ motor_status_controller: - wrist_pitch - wrist_roll - actuator + - gripper_claw - cam_0 - cam_1 + command_interfaces: + gripper_claw: + interfaces: + - status_request + state_interfaces: + actuator: + interfaces: + - status + cam_0: + interfaces: + - status + cam_1: + interfaces: + - status + publish_rate: 10.0 + +three_dof_motor_status_controller: + ros__parameters: + joints: + - base_yaw + - shoulder_pitch + - elbow_pitch + - wrist_motor_a + - wrist_motor_b + - wrist_motor_c + - actuator + - gripper_claw + - cam_0 + - cam_1 + 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 state_interfaces: actuator: interfaces: 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 d060806d..843caaf4 100644 --- a/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py +++ b/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py @@ -261,7 +261,15 @@ def launch_setup(context, *args, **kwargs): motor_status_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["motor_status_controller", "-c", "/controller_manager"], + arguments=[ + PythonExpression([ + '"three_dof_motor_status_controller" if "', + use_3dof, + '" == "true" else "two_dof_motor_status_controller"' + ]), + "-c", + "/controller_manager", + ], ) @@ -312,7 +320,7 @@ def launch_setup(context, *args, **kwargs): arguments=[controller, "-c", "/controller_manager", "--inactive"], ) ] - + active_robot_controller_names = [ "limit_switch_gpio_controller", "rotary_encoder_state_request_controller", From addeb36df544858f9bc42f5bd739dfa4a8fb9935 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Sat, 2 May 2026 21:56:40 -0400 Subject: [PATCH 24/37] motor status controller includes gpios, removing unnecessary gpio controllers --- .../config/athena_arm_controllers.yaml | 116 ++++++---- .../arm_bringup/launch/athena_arm.launch.py | 1 - src/subsystems/arm/arm_bringup/package.xml | 1 - .../config/athena_drive_controllers.yaml | 23 +- .../config/motor_status_controller.yaml | 23 ++ .../motor_status_controller.hpp | 3 +- .../src/motor_status_controller.cpp | 208 +++++++++++------- .../config/athena_science_controllers.yaml | 41 ++-- .../launch/athena_science.launch.py | 2 +- 9 files changed, 258 insertions(+), 160 deletions(-) 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 31e4e05b..aa4f8a92 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -40,9 +40,6 @@ controller_manager: rotary_encoder_state_request_controller: type: forward_command_controller/ForwardCommandController - - limit_switch_gpio_controller: - type: gpio_controllers/GpioCommandController twodof_joint_trajectory_controller: @@ -159,47 +156,6 @@ rotary_encoder_state_request_controller: - shoulder_pitch_encoder interface_name: state_request -limit_switch_gpio_controller: - ros__parameters: - gpios: - - limit_switch_0 - - limit_switch_1 - - limit_switch_2 - - limit_switch_3 - - limit_switch_4 - command_interfaces: - 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: - 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 - manual_arm_cylindrical_controller: ros__parameters: joints: @@ -287,10 +243,31 @@ two_dof_motor_status_controller: - 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: @@ -301,6 +278,21 @@ two_dof_motor_status_controller: 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: @@ -316,6 +308,12 @@ three_dof_motor_status_controller: - 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: @@ -329,6 +327,21 @@ three_dof_motor_status_controller: 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: @@ -339,4 +352,19 @@ three_dof_motor_status_controller: 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 843caaf4..0fa388be 100644 --- a/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py +++ b/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py @@ -322,7 +322,6 @@ def launch_setup(context, *args, **kwargs): ] active_robot_controller_names = [ - "limit_switch_gpio_controller", "rotary_encoder_state_request_controller", "cam_position_controller", ] diff --git a/src/subsystems/arm/arm_bringup/package.xml b/src/subsystems/arm/arm_bringup/package.xml index 75ded218..bc813d56 100644 --- a/src/subsystems/arm/arm_bringup/package.xml +++ b/src/subsystems/arm/arm_bringup/package.xml @@ -27,7 +27,6 @@ controller_manager joint_state_broadcaster forward_command_controller - gpio_controllers position_controllers velocity_controllers joint_state_publisher 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 016e996f..27319c2c 100644 --- a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml +++ b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml @@ -122,11 +122,6 @@ drive_led_gpio_controller: - red - green - blue - - status_request - state_interfaces: - drive_led: - interfaces: - - status # PowerModule GPIO Controller Configuration power_module_gpio_controller: @@ -140,7 +135,6 @@ power_module_gpio_controller: - kill_jetson - kill_main - kill_by_voltage - - status_request state_interfaces: drive_power_module: interfaces: @@ -167,6 +161,23 @@ motor_status_controller: - propulsion_fr_joint - propulsion_bl_joint - propulsion_br_joint + 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 diff --git a/src/subsystems/general/general_controllers/config/motor_status_controller.yaml b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml index 9b618c26..bcb6f84f 100644 --- a/src/subsystems/general/general_controllers/config/motor_status_controller.yaml +++ b/src/subsystems/general/general_controllers/config/motor_status_controller.yaml @@ -4,6 +4,11 @@ motor_status_controller: 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: { @@ -14,6 +19,15 @@ motor_status_controller: 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: { @@ -24,6 +38,15 @@ motor_status_controller: 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, 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 index 8edd210c..8f837869 100644 --- 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 @@ -21,6 +21,7 @@ #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" @@ -76,7 +77,7 @@ class MotorStatusController : public controller_interface::ControllerInterface // Services for command interfaces rclcpp::Service::SharedPtr status_request_service_; rclcpp::Service::SharedPtr maintenance_request_service_; - std::string status_req_joint_name; + std::string status_req_resource_name; int status_request_rate; std::string maintenance_req_joint_name; int maintenance_request_rate; diff --git a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp index 9ab85150..632153f2 100644 --- a/src/subsystems/general/general_controllers/src/motor_status_controller.cpp +++ b/src/subsystems/general/general_controllers/src/motor_status_controller.cpp @@ -14,6 +14,7 @@ #include "general_controllers/motor_status_controller.hpp" +#include #include #include #include @@ -95,11 +96,12 @@ controller_interface::CallbackReturn MotorStatusController::on_init() return controller_interface::CallbackReturn::ERROR; } - status_req_joint_name = ""; + 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; } @@ -114,6 +116,11 @@ MotorStatusController::command_interface_configuration() const 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; } @@ -128,6 +135,11 @@ MotorStatusController::state_interface_configuration() const 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; } @@ -137,8 +149,8 @@ controller_interface::CallbackReturn MotorStatusController::on_configure( { params_ = param_listener_->get_params(); - if (params_.joints.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "No joints specified for motor_status_controller."); + 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; } @@ -165,15 +177,17 @@ controller_interface::CallbackReturn MotorStatusController::on_configure( { if (request->joint_name.empty()) { response->success = false; - response->message = "Joint name cannot be empty"; + response->message = "Joint/GPIO name cannot be empty"; RCLCPP_WARN(get_node()->get_logger(), "%s", response->message.c_str()); return; } - status_req_joint_name = request->joint_name; + status_req_resource_name = request->joint_name; status_request_rate = request->request_rate; - std::string msg = "Received status_request for joint " + status_req_joint_name + " at request_rate: " + std::to_string(status_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()); @@ -237,8 +251,8 @@ controller_interface::CallbackReturn MotorStatusController::on_configure( RCLCPP_INFO( get_node()->get_logger(), - "Configured motor_status_controller for %zu joints at publish rate: %.1f Hz", - params_.joints.size(), params_.publish_rate); + "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; } @@ -307,95 +321,121 @@ controller_interface::return_type MotorStatusController::update( system_info_msg.header.stamp = time; // Command Interfaces - for (const auto & [joint_name, joint_cfg] : params_.command_interfaces.joints_map){ - for(const auto & interface_name : joint_cfg.interfaces){ - std::string key = joint_name + "/" + interface_name; - auto it = command_interface_map_.find(key); - if (it != command_interface_map_.end()) { - if (interface_name == "maintenance_frame_high" && joint_name == maintenance_req_joint_name) { - command_interfaces_[it->second].set_value(maintenance_frame_high); - } - - if (interface_name == "maintenance_frame_low" && joint_name == maintenance_req_joint_name) { - command_interfaces_[it->second].set_value(maintenance_frame_low); - } - - if(interface_name == "maintenance_data_count" && joint_name == maintenance_req_joint_name) { - command_interfaces_[it->second].set_value(maintenance_data_count); - } - - if (interface_name == "status_request" && joint_name == status_req_joint_name) { - // Controller must turn status request back to 0 if it's a one-shot request - 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; + 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; } - 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."); + + if (allow_maintenance && interface_name == "maintenance_frame_high" && + resource_name == maintenance_req_joint_name) + { + command_interfaces_[it->second].set_value(maintenance_frame_high); } - else if (status_request_rate >= 0) { - command_interfaces_[it->second].set_value(status_request_rate); + + if (allow_maintenance && interface_name == "maintenance_frame_low" && + resource_name == maintenance_req_joint_name) + { + command_interfaces_[it->second].set_value(maintenance_frame_low); } - } - if (interface_name == "maintenance_request" && joint_name == maintenance_req_joint_name) { - // Controller must turn maintenance request back to 0 if it's a one-shot request - 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; + if (allow_maintenance && interface_name == "maintenance_data_count" && + resource_name == maintenance_req_joint_name) + { + command_interfaces_[it->second].set_value(maintenance_data_count); } - 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."); + + 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); + } } - else if (maintenance_request_rate >= 0) { - command_interfaces_[it->second].set_value(maintenance_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); + } } } } - } - } - - // State Interfaces - msgs::msg::JointStatus status; - - // Initializing values in case state interfaces don't exist for them - 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 & [joint_name, joint_cfg] : params_.state_interfaces.joints_map){ - for(const auto & interface_name : joint_cfg.interfaces){ - status.joint_name = joint_name; - std::string key = joint_name + "/" + interface_name; - auto it = state_interface_map_.find(key); - - if (it != state_interface_map_.end()) { - 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"); + }; + + 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); } - } + }; - 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); 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 4ec4719f..52aa34de 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -21,9 +21,6 @@ controller_manager: fluoro_led_gpio_controller: type: gpio_controllers/GpioCommandController - science_limit_switch_gpio_controller: - type: gpio_controllers/GpioCommandController - motor_status_controller: type: general_controllers/MotorStatusController @@ -91,7 +88,6 @@ laser_gpio_controller: spectrometry_laser: interfaces: - laser_command - - status_request state_interfaces: spectrometry_laser: interfaces: @@ -107,24 +103,6 @@ fluoro_led_gpio_controller: fluoro_led: interfaces: - intensity - - status_request - state_interfaces: - fluoro_led: - interfaces: - - status - -science_limit_switch_gpio_controller: - ros__parameters: - gpios: - - science_limit_switch - command_interfaces: - science_limit_switch: - interfaces: - - status_request - state_interfaces: - science_limit_switch: - interfaces: - - status # Default command interfaces: # - status_request @@ -151,7 +129,20 @@ motor_status_controller: - servo_sampler_lift_r - talon_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 talon_scoop: interfaces: - status_request @@ -192,4 +183,10 @@ motor_status_controller: 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/launch/athena_science.launch.py b/src/subsystems/science/science_bringup/launch/athena_science.launch.py index 985eb1a2..66fe1d28 100644 --- a/src/subsystems/science/science_bringup/launch/athena_science.launch.py +++ b/src/subsystems/science/science_bringup/launch/athena_science.launch.py @@ -223,7 +223,7 @@ def generate_launch_description(): ] # GPIO controller spawner for Laser - gpio_controller_names = ["laser_gpio_controller", "fluoro_led_gpio_controller", "science_limit_switch_gpio_controller"] + gpio_controller_names = ["laser_gpio_controller", "fluoro_led_gpio_controller"] gpio_controller_spawners = [] for controller in gpio_controller_names: gpio_controller_spawners += [ From cc00e003a8847ac766820a26c57386f4dfe14905 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Tue, 31 Mar 2026 14:42:01 -0400 Subject: [PATCH 25/37] Added DC Motor HWI --- .../science/science.dc.ros2_control.xacro | 49 ++ .../urdf/athena_science.urdf.xacro | 9 +- .../dc_ros2_control/CMakeLists.txt | 70 ++ .../dc_ros2_control/dc_hardware_interface.xml | 9 + .../dc_ros2_control/dc_hardware_interface.hpp | 169 +++++ .../dc_ros2_control/package.xml | 35 + .../src/dc_hardware_interface.cpp | 617 ++++++++++++++++++ .../config/athena_science_controllers.yaml | 86 +-- .../science_controller.hpp | 1 + .../src/science_controller.cpp | 8 +- .../src/science_controller.yaml | 6 +- 11 files changed, 975 insertions(+), 84 deletions(-) create mode 100644 src/description/ros2_control/science/science.dc.ros2_control.xacro create mode 100644 src/hardware_interfaces/dc_ros2_control/CMakeLists.txt create mode 100644 src/hardware_interfaces/dc_ros2_control/dc_hardware_interface.xml create mode 100644 src/hardware_interfaces/dc_ros2_control/include/dc_ros2_control/dc_hardware_interface.hpp create mode 100644 src/hardware_interfaces/dc_ros2_control/package.xml create mode 100644 src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp diff --git a/src/description/ros2_control/science/science.dc.ros2_control.xacro b/src/description/ros2_control/science/science.dc.ros2_control.xacro new file mode 100644 index 00000000..27eb71b2 --- /dev/null +++ b/src/description/ros2_control/science/science.dc.ros2_control.xacro @@ -0,0 +1,49 @@ + + + + + + + + + dc_ros2_control/DCHardwareInterface + 20 + 5 + 1 + can0 + 0x70 + + + + 0 + revolute + + 50.9 + true + + + + + + + + + + 2 + revolute + + 50.9 + true + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/description/urdf/athena_science.urdf.xacro b/src/description/urdf/athena_science.urdf.xacro index 608d8163..cdb206cc 100644 --- a/src/description/urdf/athena_science.urdf.xacro +++ b/src/description/urdf/athena_science.urdf.xacro @@ -6,8 +6,10 @@ - - + + + + @@ -32,6 +34,9 @@ + + + diff --git a/src/hardware_interfaces/dc_ros2_control/CMakeLists.txt b/src/hardware_interfaces/dc_ros2_control/CMakeLists.txt new file mode 100644 index 00000000..4c11c388 --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.8) +project(dc_ros2_control) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + umdloop_can_library +) + +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +include_directories(include) + +add_library(dc_ros2_control SHARED + src/dc_hardware_interface.cpp +) + +target_compile_features(dc_ros2_control PUBLIC cxx_std_20) + +target_include_directories(dc_ros2_control PUBLIC + $ + $ +) + +# Link umdloop_can_library directly via CMake target (same as servo HWI) +target_link_libraries(dc_ros2_control PUBLIC umdloop_can_library::umdloop_can_library) + +ament_target_dependencies(dc_ros2_control PUBLIC + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +# Export hardware plugin +pluginlib_export_plugin_description_file(hardware_interface dc_hardware_interface.xml) + +# Install +install( + DIRECTORY include/ + DESTINATION include/dc_ros2_control +) + +install(TARGETS dc_ros2_control + EXPORT export_dc_ros2_control + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_targets(export_dc_ros2_control) +ament_export_include_directories(include) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() \ No newline at end of file diff --git a/src/hardware_interfaces/dc_ros2_control/dc_hardware_interface.xml b/src/hardware_interfaces/dc_ros2_control/dc_hardware_interface.xml new file mode 100644 index 00000000..76bd8b7e --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/dc_hardware_interface.xml @@ -0,0 +1,9 @@ + + + + UMDLoop's ros2_control plugin for the DC motors + + + \ No newline at end of file diff --git a/src/hardware_interfaces/dc_ros2_control/include/dc_ros2_control/dc_hardware_interface.hpp b/src/hardware_interfaces/dc_ros2_control/include/dc_ros2_control/dc_hardware_interface.hpp new file mode 100644 index 00000000..f4b54f24 --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/include/dc_ros2_control/dc_hardware_interface.hpp @@ -0,0 +1,169 @@ +#ifndef DC_HARDWARE_INTERFACE_HPP_ +#define DC_HARDWARE_INTERFACE_HPP_ + +#include +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include +#include +#include + +#include "umdloop_can_library/SocketCanBus.hpp" +#include "umdloop_can_library/CanFrame.hpp" + +namespace dc_ros2_control +{ +class DCHardwareInterface : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(DCHardwareInterface) + + // Lifecycle + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::return_type perform_command_mode_switch( + const std::vector& start_interfaces, + const std::vector& stop_interfaces) override; + + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + // -- Helper Functions -- + void on_can_message(const CANLib::CanFrame& frame); + void logger_function(); + + // Unit conversions + double calculate_joint_position_from_motor_position(double motor_pos_deg, int gear_ratio); + double calculate_joint_displacement_from_motor_position(double motor_pos_deg, int gear_ratio, double dist_per_rev); + double calculate_joint_angular_velocity_from_motor_velocity(double motor_vel_dps, int gear_ratio); + double calculate_joint_linear_velocity_from_motor_velocity(double motor_vel_dps, int gear_ratio, double dist_per_rev); + + int16_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); + int16_t calculate_motor_position_from_desired_joint_displacement(double joint_position, int gear_ratio, double dist_per_rev); + int16_t calculate_motor_velocity_from_desired_joint_angular_velocity(double joint_velocity, int gear_ratio); + int16_t calculate_motor_velocity_from_desired_joint_linear_velocity(double joint_velocity, int gear_ratio, double dist_per_rev); + +private: + // Hardware Interface Parameters + int update_rate; + double elapsed_update_time; + double elapsed_time; + double elapsed_logger_time; + int logger_rate; + int logger_state; + int write_count; + + int num_joints; + + // Stores Arbitration IDs + int can_command_id; + uint32_t can_response_id; + + // Joint state (read from hardware) + std::vector joint_state_position_; + std::vector joint_state_velocity_; + + // Previous joint state (change detection) + std::vector prev_joint_state_position_; + std::vector prev_joint_state_velocity_; + + // Joint commands (written to hardware) + std::vector joint_command_position_; + std::vector joint_command_velocity_; + + // Previous joint commands (change detection) + std::vector prev_joint_command_position_; + std::vector prev_joint_command_velocity_; + + // Raw motor data from CAN bus (populated in on_can_message, consumed in read) + std::vector motor_position; + std::vector motor_velocity; + std::vector device_status; + + // Telemetry data (TODO: implement CAN telemetry commands) + std::vector motor_temperature_; + std::vector motor_torque_current_; + + // Maximum displacement for prismatic joints + std::vector max_disp; + + // Prismatic-specific: distance per revolution (future use) + std::vector distance_per_rev; + + // CAN Library + CANLib::SocketCanBus canBus; + CANLib::CanFrame can_tx_frame_; + CANLib::CanFrame can_rx_frame_; + std::string can_interface; + + // Per-joint parameters (read from XACRO) + std::vector joint_node_ids; + std::vector joint_gear_ratios; + std::vector joint_inverted; + + // Control mode + enum class integration_level_t : std::uint8_t + { + UNDEFINED = 0, + POSITION = 1, + VELOCITY = 2, + }; + + std::vector control_level_; + + // Joint type + enum class joint_type_t : std::uint8_t + { + REVOLUTE = 0, + PRISMATIC = 1, + }; + + std::vector joint_type_; + + // CAN Command Bytes (full byte values, used as: CMD + port_id) + static constexpr uint8_t PCB_HEARTBEAT_CMD = 0x10; + static constexpr uint8_t LED_STATUS_CMD = 0x11; + static constexpr uint8_t ABSOLUTE_POS_CONTROL_CMD = 0x20; + static constexpr uint8_t VELOCITY_CONTROL_CMD = 0x30; + static constexpr uint8_t MOTOR_STATE_CMD = 0x40; + static constexpr uint8_t MOTOR_STATUS_CMD = 0x50; + static constexpr uint8_t MAINTENANCE_CMD = 0x60; + static constexpr uint8_t DC_SPECS_CMD = 0x70; +}; + +} // namespace dc_ros2_control + +#endif // DC_HARDWARE_INTERFACE_HPP_ \ No newline at end of file diff --git a/src/hardware_interfaces/dc_ros2_control/package.xml b/src/hardware_interfaces/dc_ros2_control/package.xml new file mode 100644 index 00000000..6f174cc8 --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/package.xml @@ -0,0 +1,35 @@ + + + + dc_ros2_control + 0.0.0 + TODO: Package description + henry + TODO: License declaration + + ament_cmake + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + msgs + + + umdloop_can_library + umdloop_can_library + + ament_lint_auto + ament_lint_common + + ament_cmake + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file diff --git a/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp b/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp new file mode 100644 index 00000000..5cc4e956 --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp @@ -0,0 +1,617 @@ +#include "dc_ros2_control/dc_hardware_interface.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/lexical_casts.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace dc_ros2_control +{ + +// ============================================================================= +// Unit Conversions (matches servo HWI structure) +// ============================================================================= + +double DCHardwareInterface::calculate_joint_position_from_motor_position(double motor_pos_deg, int gear_ratio) { + // deg -> radians with gear ratio + return (motor_pos_deg * (M_PI / 180.0)) / gear_ratio; +} + +double DCHardwareInterface::calculate_joint_displacement_from_motor_position(double motor_pos_deg, int gear_ratio, double dist_per_rev) { + // deg -> meters: (deg / 360) * distance_per_rev / gear_ratio + return (motor_pos_deg / 360.0) * dist_per_rev / gear_ratio; +} + +double DCHardwareInterface::calculate_joint_angular_velocity_from_motor_velocity(double motor_vel_dps, int gear_ratio) { + // deg/s -> rad/s with gear ratio + return (motor_vel_dps * (M_PI / 180.0)) / gear_ratio; +} + +double DCHardwareInterface::calculate_joint_linear_velocity_from_motor_velocity(double motor_vel_dps, int gear_ratio, double dist_per_rev) { + // deg/s -> m/s + return (motor_vel_dps / 360.0) * dist_per_rev / gear_ratio; +} + +int16_t DCHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio) { + // radians -> deg with gear ratio + return static_cast(std::round((joint_position * (180.0 / M_PI)) * gear_ratio)); +} + +int16_t DCHardwareInterface::calculate_motor_position_from_desired_joint_displacement(double joint_position, int gear_ratio, double dist_per_rev) { + // meters -> deg: (m / distance_per_rev) * 360 * gear_ratio + return static_cast(std::round((joint_position / dist_per_rev) * 360.0 * gear_ratio)); +} + +int16_t DCHardwareInterface::calculate_motor_velocity_from_desired_joint_angular_velocity(double joint_velocity, int gear_ratio) { + // rad/s -> deg/s with gear ratio + return static_cast(std::round((joint_velocity * (180.0 / M_PI)) * gear_ratio)); +} + +int16_t DCHardwareInterface::calculate_motor_velocity_from_desired_joint_linear_velocity(double joint_velocity, int gear_ratio, double dist_per_rev) { + // m/s -> deg/s + return static_cast(std::round((joint_velocity / dist_per_rev) * 360.0 * gear_ratio)); +} + +// ============================================================================= +// Logger +// ============================================================================= + +void DCHardwareInterface::logger_function() +{ + if (num_joints == 0) return; + + std::string log_msg = "\033[2J\033[H \nDC Motor Logger"; + std::ostringstream oss; + std::string control_mode; + std::string joint_type_str; + std::string status; + + oss << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface + << " | Command CAN ID: 0x" << std::hex << std::uppercase << can_command_id + << " | Response CAN ID: 0x" << std::hex << std::uppercase << can_response_id + << " | HWI Update Rate: " << std::dec << update_rate + << " | Logger Update Rate: " << logger_rate << "\n" + << "Elapsed Time since first update: " << elapsed_time << "\n" + << "\n--- Joint Specific ---"; + + for (int i = 0; i < num_joints; i++) { + switch (control_level_[i]) { + case integration_level_t::POSITION: control_mode = "POSITION"; break; + case integration_level_t::VELOCITY: control_mode = "VELOCITY"; break; + default: control_mode = "UNDEFINED"; break; + } + + switch (joint_type_[i]) { + case joint_type_t::REVOLUTE: joint_type_str = "REVOLUTE"; break; + case joint_type_t::PRISMATIC: joint_type_str = "PRISMATIC"; break; + } + + switch (device_status[i]) { + case 0: status = "Undefined"; break; + case 1: status = "Idle"; break; + case 2: status = "Startup Sequence"; break; + case 3: status = "Error (Invalid Request)"; break; + case 4: status = "Error (Motor Disarmed)"; break; + case 5: status = "Error (Motor Failed)"; break; + case 6: status = "Error (Controller Failed)"; break; + case 7: status = "Error (ESTOP Requested)"; break; + case 8: status = "Error (Unknown Position)"; break; + case 9: status = "Position Control"; break; + case 10: status = "Velocity Control"; break; + case 11: status = "Motor Stopped"; break; + default: status = "UNKNOWN (" + std::to_string(device_status[i]) + ")"; break; + } + + oss << "\nJOINT: " << info_.joints[i].name << "\n" + << "Parameters: Node ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] + << " | Gear Ratio: " << std::dec << joint_gear_ratios[i] + << " | Inverted: " << (joint_inverted[i] ? "true" : "false") + << " | Device Status: " << status + << " | Control Mode: " << control_mode + << " | Joint Type: " << joint_type_str << "\n" + << "-- Commands --\n" + << "Joint Command Position: " << joint_command_position_[i] + << " | Joint Command Velocity: " << joint_command_velocity_[i] << "\n" + << "-- State --\n" + << "Joint Position: " << joint_state_position_[i] + << " | Joint Velocity: " << joint_state_velocity_[i] << "\n" + << "-- Telemetry --\n" + << "Temperature: " << motor_temperature_[i] << " C" + << " | Torque Current: " << motor_torque_current_[i] << " A\n"; + } + + log_msg += oss.str(); + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "%s", log_msg.c_str()); +} + +// ============================================================================= +// Initialization +// ============================================================================= + +hardware_interface::CallbackReturn DCHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // -- Per-joint parameters -- + for (auto& joint : info_.joints) { + joint_node_ids.push_back( + std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x0, 0xF)); + + // Gear ratio (default: 1) + if (joint.parameters.count("gear_ratio")) { + joint_gear_ratios.push_back(std::abs(std::stoi(joint.parameters.at("gear_ratio")))); + } else { + joint_gear_ratios.push_back(1); + } + + // Inverted (default: false) + if (joint.parameters.count("inverted")) { + joint_inverted.push_back(joint.parameters.at("inverted") == "true"); + } else { + joint_inverted.push_back(false); + } + + // Joint type (default: revolute) + std::string jtype = "revolute"; + if (joint.parameters.count("joint_type")) { + jtype = joint.parameters.at("joint_type"); + } + + if (jtype == "revolute") { + joint_type_.push_back(joint_type_t::REVOLUTE); + max_disp.push_back(std::nan("")); + distance_per_rev.push_back(0.0); + } else if (jtype == "prismatic" && joint.parameters.count("max_disp") && joint.parameters.count("distance_per_rev")) { + joint_type_.push_back(joint_type_t::PRISMATIC); + max_disp.push_back(std::abs(std::stod(joint.parameters.at("max_disp")))); + distance_per_rev.push_back(std::stod(joint.parameters.at("distance_per_rev"))); + } else { + RCLCPP_ERROR(rclcpp::get_logger("DCHardwareInterface"), + "Invalid joint_type for joint %s. Must be 'revolute' or 'prismatic' (with 'max_disp' and 'distance_per_rev').", + joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } + + // -- Hardware-level parameters -- + num_joints = static_cast(info_.joints.size()); + update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); + logger_rate = std::stoi(info_.hardware_parameters.at("logger_rate")); + logger_state = std::stoi(info_.hardware_parameters.at("logger_state")); + can_interface = info_.hardware_parameters.at("can_interface"); + can_command_id = std::stoi(info_.hardware_parameters.at("can_id"), nullptr, 0); + can_response_id = can_command_id + 0x01; + + elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; + write_count = 0; + + // -- Command and State Interface initialization -- + joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + prev_joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + joint_state_velocity_.assign(num_joints, 0.0); + prev_joint_state_velocity_.assign(num_joints, 0.0); + + joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + prev_joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + joint_command_velocity_.assign(num_joints, 0.0); + prev_joint_command_velocity_.assign(num_joints, 0.0); + + motor_position.assign(num_joints, 0.0); + motor_velocity.assign(num_joints, 0.0); + device_status.assign(num_joints, 0); + + // Telemetry placeholders (TODO: populate via CAN telemetry command) + motor_temperature_.assign(num_joints, 0.0); + motor_torque_current_.assign(num_joints, 0.0); + + // Default control mode: position + control_level_.resize(num_joints, integration_level_t::POSITION); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +// ============================================================================= +// Lifecycle +// ============================================================================= + +hardware_interface::CallbackReturn DCHardwareInterface::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + return this->on_cleanup(previous_state); +} + +std::vector +DCHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + + for (int i = 0; i < num_joints; i++) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_state_position_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_state_velocity_[i])); + + // Telemetry interfaces (TODO: populate via CAN telemetry command) + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, "motor_temperature", &motor_temperature_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, "torque_current", &motor_torque_current_[i])); + } + + return state_interfaces; +} + +std::vector +DCHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + + for (int i = 0; i < num_joints; i++) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_command_position_[i])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_command_velocity_[i])); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn DCHardwareInterface::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Configuring DC Motor HWI..."); + + if (!canBus.open(can_interface, + std::bind(&DCHardwareInterface::on_can_message, this, std::placeholders::_1))) + { + RCLCPP_ERROR(rclcpp::get_logger("DCHardwareInterface"), + "Failed to open CAN interface: %s", can_interface.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "CAN interface opened successfully."); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DCHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Cleaning up... please wait..."); + + // Send shutdown command (maintenance sub-command 3) to all joints + for (int i = 0; i < num_joints; i++) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = MAINTENANCE_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = 3; // DC Motor: Shutdown + canBus.send(can_tx_frame_); + } + + canBus.close(); + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Cleanup complete."); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DCHardwareInterface::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Activating... please wait..."); + + // Initialize command positions to current state + joint_command_position_ = joint_state_position_; + + for (size_t i = 0; i < joint_command_position_.size(); ++i) { + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "Joint %zu command position initialized to: %f", i, joint_command_position_[i]); + } + + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Successfully activated!"); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DCHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Deactivating... please wait..."); + + // Send stop command (maintenance sub-command 2) to all joints + for (int i = 0; i < num_joints; i++) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = MAINTENANCE_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = 2; // DC Motor: Stop + canBus.send(can_tx_frame_); + } + + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Successfully deactivated!"); + return hardware_interface::CallbackReturn::SUCCESS; +} + +// ============================================================================= +// CAN Receive Callback +// ============================================================================= + +void DCHardwareInterface::on_can_message(const CANLib::CanFrame& frame) +{ + can_rx_frame_ = frame; + + int data[7] = {0x00}; + uint8_t cmd_byte = can_rx_frame_.data[0]; + uint8_t command_nibble = cmd_byte & 0xF0; + uint8_t device_id_nibble = cmd_byte & 0x0F; + double raw_motor_position = 0.0; + double raw_motor_velocity = 0.0; + + for (int i = 0; i < num_joints; i++) { + if (can_rx_frame_.id != can_response_id || device_id_nibble != joint_node_ids[i]) + continue; + + if (command_nibble == MOTOR_STATE_CMD || + command_nibble == ABSOLUTE_POS_CONTROL_CMD || + command_nibble == VELOCITY_CONTROL_CMD) + { + // DECODING CAN MESSAGE + data[1] = can_rx_frame_.data[1]; // Position low byte + data[2] = can_rx_frame_.data[2]; // Position high byte + data[3] = can_rx_frame_.data[3]; // Velocity low byte + data[4] = can_rx_frame_.data[4]; // Velocity high byte + + // POSITION: int16 sign extension (NOT int32) + raw_motor_position = static_cast(static_cast((data[2] << 8) | data[1])); + + // VELOCITY: int16 + raw_motor_velocity = static_cast(static_cast((data[4] << 8) | data[3])); + + // Apply inversion + double dir = joint_inverted[i] ? -1.0 : 1.0; + + // CALCULATING JOINT STATE + if (joint_type_[i] == joint_type_t::REVOLUTE) { + motor_position[i] = dir * calculate_joint_position_from_motor_position(raw_motor_position, joint_gear_ratios[i]); + motor_velocity[i] = dir * calculate_joint_angular_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i]); + } + else if (joint_type_[i] == joint_type_t::PRISMATIC) { + motor_position[i] = dir * calculate_joint_displacement_from_motor_position(raw_motor_position, joint_gear_ratios[i], distance_per_rev[i]); + motor_velocity[i] = dir * calculate_joint_linear_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i], distance_per_rev[i]); + } + else { + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); + } + } + else if (command_nibble == MOTOR_STATUS_CMD) + { + device_status[i] = can_rx_frame_.data[1]; + } + // TODO: Handle telemetry response frames here when implemented + } +} + +// ============================================================================= +// Read (update state from CAN data) +// ============================================================================= + +hardware_interface::return_type DCHardwareInterface::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + for (int i = 0; i < num_joints; i++) { + if (prev_joint_state_velocity_[i] != motor_velocity[i]) { + joint_state_velocity_[i] = motor_velocity[i]; + prev_joint_state_velocity_[i] = joint_state_velocity_[i]; + } + + if (prev_joint_state_position_[i] != motor_position[i]) { + joint_state_position_[i] = motor_position[i]; + prev_joint_state_position_[i] = joint_state_position_[i]; + } + + // Check device status — allow Undefined(0), Idle(1), Position Control(9), Velocity Control(10) + int s = device_status[i]; + if (s != 0 && s != 1 && s != 9 && s != 10) { + RCLCPP_ERROR(rclcpp::get_logger("DCHardwareInterface"), + "Joint %s in error state: %d", info_.joints[i].name.c_str(), s); + return hardware_interface::return_type::ERROR; + } + + // TODO: Telemetry — populate motor_temperature_[i] and motor_torque_current_[i] via CAN + } + + return hardware_interface::return_type::OK; +} + +// ============================================================================= +// Write (send CAN commands) +// ============================================================================= + +hardware_interface::return_type DCHardwareInterface::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + elapsed_update_time += period.seconds(); + elapsed_time += period.seconds(); + double update_period = 1.0 / update_rate; + + // Logger update + elapsed_logger_time += period.seconds(); + double logging_period = 1.0 / logger_rate; + if (elapsed_logger_time > logging_period) { + elapsed_logger_time = 0.0; + if (logger_state == 1) { + logger_function(); + } + } + + // Rate-limited CAN writes — all joints per tick + if (elapsed_update_time > update_period) { + elapsed_update_time = 0.0; + + for (int i = 0; i < num_joints; i++) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + + // Apply inversion for outgoing commands + double dir = joint_inverted[i] ? -1.0 : 1.0; + + if (control_level_[i] == integration_level_t::POSITION && + std::isfinite(joint_command_position_[i]) && + joint_command_position_[i] != prev_joint_command_position_[i]) + { + // Clamp prismatic joints to [0, max_disp] + if (joint_type_[i] == joint_type_t::PRISMATIC) { + joint_command_position_[i] = std::clamp( + joint_command_position_[i], 0.0, max_disp[i]); + } + + int16_t motor_pos; + if (joint_type_[i] == joint_type_t::REVOLUTE) { + motor_pos = calculate_motor_position_from_desired_joint_position( + dir * joint_command_position_[i], joint_gear_ratios[i]); + } else { + motor_pos = calculate_motor_position_from_desired_joint_displacement( + dir * joint_command_position_[i], joint_gear_ratios[i], distance_per_rev[i]); + } + + can_tx_frame_.dlc = 3; + can_tx_frame_.data[0] = ABSOLUTE_POS_CONTROL_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = static_cast(motor_pos & 0xFF); + can_tx_frame_.data[2] = static_cast((motor_pos >> 8) & 0xFF); + + prev_joint_command_position_[i] = joint_command_position_[i]; + } + else if (control_level_[i] == integration_level_t::VELOCITY && + std::isfinite(joint_command_velocity_[i]) && + joint_command_velocity_[i] != prev_joint_command_velocity_[i]) + { + int16_t motor_vel; + if (joint_type_[i] == joint_type_t::REVOLUTE) { + motor_vel = calculate_motor_velocity_from_desired_joint_angular_velocity( + dir * joint_command_velocity_[i], joint_gear_ratios[i]); + } else { + motor_vel = calculate_motor_velocity_from_desired_joint_linear_velocity( + dir * joint_command_velocity_[i], joint_gear_ratios[i], distance_per_rev[i]); + } + + can_tx_frame_.dlc = 3; + can_tx_frame_.data[0] = VELOCITY_CONTROL_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = static_cast(motor_vel & 0xFF); + can_tx_frame_.data[2] = static_cast((motor_vel >> 8) & 0xFF); + + prev_joint_command_velocity_[i] = joint_command_velocity_[i]; + } + else + { + // No new command — poll motor state + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = MOTOR_STATE_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = 1; // Validate request + } + + canBus.send(can_tx_frame_); + } + } + + return hardware_interface::return_type::OK; +} + +// ============================================================================= +// Command Mode Switch +// ============================================================================= + +hardware_interface::return_type DCHardwareInterface::perform_command_mode_switch( + const std::vector& start_interfaces, + const std::vector& stop_interfaces) +{ + std::ostringstream ss; + ss << "perform_command_mode_switch called. start: ["; + for (auto &s : start_interfaces) ss << s << ","; + ss << "] stop: ["; + for (auto &s : stop_interfaces) ss << s << ","; + ss << "]"; + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "%s", ss.str().c_str()); + + std::vector requested_modes(num_joints, integration_level_t::UNDEFINED); + + // Process stop interfaces + for (const auto &ifname : stop_interfaces) { + for (int i = 0; i < num_joints; ++i) { + if (ifname.find(info_.joints[i].name) != std::string::npos) { + requested_modes[i] = integration_level_t::UNDEFINED; + } + } + } + + // Process start interfaces + for (const auto &ifname : start_interfaces) { + for (int i = 0; i < num_joints; ++i) { + const std::string pos_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_POSITION); + const std::string vel_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_VELOCITY); + if (ifname == pos_if) { + requested_modes[i] = integration_level_t::POSITION; + } else if (ifname == vel_if) { + requested_modes[i] = integration_level_t::VELOCITY; + } + } + } + + // Apply modes + for (int i = 0; i < num_joints; ++i) { + if (requested_modes[i] == integration_level_t::UNDEFINED) { + bool was_stopped = false; + for (const auto &ifname : stop_interfaces) { + if (ifname.find(info_.joints[i].name) != std::string::npos) { + was_stopped = true; + break; + } + } + if (was_stopped) { + control_level_[i] = integration_level_t::UNDEFINED; + joint_command_velocity_[i] = 0; + joint_command_position_[i] = joint_state_position_[i]; + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "Joint %s: stopped -> UNDEFINED", info_.joints[i].name.c_str()); + } + } else { + control_level_[i] = requested_modes[i]; + if (requested_modes[i] == integration_level_t::VELOCITY) { + joint_command_velocity_[i] = 0; + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "Joint %s: switched to VELOCITY", info_.joints[i].name.c_str()); + } else if (requested_modes[i] == integration_level_t::POSITION) { + joint_command_position_[i] = joint_state_position_[i]; + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "Joint %s: switched to POSITION (initialized to %f)", + info_.joints[i].name.c_str(), joint_command_position_[i]); + } + } + } + + return hardware_interface::return_type::OK; +} + +} // namespace dc_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + dc_ros2_control::DCHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file diff --git a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml index 52aa34de..3de6fc40 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -33,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 @@ -49,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 @@ -68,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] @@ -120,73 +120,9 @@ motor_status_controller: joints: - stepper_motor_a - stepper_motor_b - - servo_rack_and_pinion_l - - servo_rack_and_pinion_r - - servo_scoop_a - - servo_scoop_b - - talon_scoop - - servo_sampler_lift_l - - servo_sampler_lift_r - - talon_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 - talon_scoop: - interfaces: - - status_request - talon_auger: - interfaces: - - status_request - 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 - talon_scoop: - interfaces: - - status - servo_sampler_lift_l: - interfaces: - - status - servo_sampler_lift_r: - interfaces: - - status - talon_auger: - interfaces: - - status - servo_auger_lift: - interfaces: - - status - fluoro_led: - interfaces: - - status - science_limit_switch: - interfaces: - - status + - dc_lift + - dc_scoop + interfaces: + - motor_temperature + - torque_current publish_rate: 10.0 diff --git a/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp b/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp index 75872f50..6af6d49c 100644 --- a/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp +++ b/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp @@ -140,6 +140,7 @@ class ScienceManual : public controller_interface::ControllerInterface std::vector talon_joints_; std::vector servo_joints_; std::vector rack_pinion_joints_; + std::vector dc_joints_; std::vector joints_; //std::string talon_auger_; diff --git a/src/subsystems/science/science_controllers/src/science_controller.cpp b/src/subsystems/science/science_controllers/src/science_controller.cpp index ebbaea9e..c6997dd7 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.cpp +++ b/src/subsystems/science/science_controllers/src/science_controller.cpp @@ -85,8 +85,8 @@ controller_interface::CallbackReturn ScienceManual::on_configure( params_.pump_b }; - talon_joints_.clear(); - talon_joints_ = { + dc_joints_.clear(); + dc_joints_ = { params_.scoop_spinner, params_.auger_spinner }; @@ -246,7 +246,7 @@ controller_interface::CallbackReturn ScienceManual::on_deactivate( command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); } stepper_pump_joints_.clear(); - talon_joints_.clear(); + dc_joints_.clear(); servo_joints_.clear(); rack_pinion_joints_.clear(); state_joints_.clear(); @@ -375,7 +375,7 @@ controller_interface::return_type ScienceManual::update( // Stepper motors (position) command_interfaces_[IDX_PUMP_A_VELOCITY].set_value(stepper_cmd * (M_PI / 180.0)); command_interfaces_[IDX_PUMP_B_VELOCITY].set_value(stepper_cmd * (M_PI / 180.0)); - // Talons (velocity) + // DC motors (velocity) command_interfaces_[IDX_SAMPLER_LIFT_LEFT_VELOCITY].set_value(sampler_lift_pos_l * (M_PI / 180.0)); command_interfaces_[IDX_SAMPLER_LIFT_RIGHT_VELOCITY].set_value(sampler_lift_pos_r * (M_PI / 180.0)); command_interfaces_[IDX_SCOOP_SPINNER_VELOCITY].set_value(scoop_spinner_cmd); diff --git a/src/subsystems/science/science_controllers/src/science_controller.yaml b/src/subsystems/science/science_controllers/src/science_controller.yaml index 7371edf2..eb1938cf 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.yaml +++ b/src/subsystems/science/science_controllers/src/science_controller.yaml @@ -74,8 +74,8 @@ science_manual: scoop_spinner: type: string - default_value: "talon_scoop" - description: "Scoop Talon motor controller" + default_value: "dc_scoop" + description: "Scoop DC motor" read_only: true validation: not_empty<>: null @@ -94,7 +94,7 @@ science_manual: auger_spinner: type: string - default_value: "talon_auger" + default_value: "dc_auger" description: "(Optional)" read_only: false From 39d0f2f9239e77cccb4ae3753a968cbf5bea1a4e Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Fri, 3 Apr 2026 00:10:55 -0400 Subject: [PATCH 26/37] Small changes to DC Motor HWI --- .../include/dc_ros2_control/dc_hardware_interface.hpp | 7 +++++++ .../dc_ros2_control/src/dc_hardware_interface.cpp | 10 ++++++++-- 2 files changed, 15 insertions(+), 2 deletions(-) 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 index f4b54f24..5284c343 100644 --- 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 @@ -162,6 +162,13 @@ class DCHardwareInterface : public hardware_interface::SystemInterface static constexpr uint8_t MOTOR_STATUS_CMD = 0x50; static constexpr uint8_t MAINTENANCE_CMD = 0x60; static constexpr uint8_t DC_SPECS_CMD = 0x70; + + // DC Motor Maintenance Sub-Commands + static constexpr uint8_t MAINTENANCE_ZERO_ROM = 0; + static constexpr uint8_t MAINTENANCE_REQ_VECTORS = 1; + static constexpr uint8_t MAINTENANCE_STOP = 2; + static constexpr uint8_t MAINTENANCE_SHUTDOWN = 3; + static constexpr uint8_t MAINTENANCE_CLEAR_ERRORS = 4; }; } // namespace dc_ros2_control 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 index 5cc4e956..2bef74b7 100644 --- a/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp +++ b/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp @@ -298,13 +298,19 @@ hardware_interface::CallbackReturn DCHardwareInterface::on_cleanup( { RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Cleaning up... please wait..."); + // Stop worker thread if running + is_running.store(false); + if (worker.joinable()) { + worker.join(); + } + // Send shutdown command (maintenance sub-command 3) to all joints for (int i = 0; i < num_joints; i++) { can_tx_frame_ = CANLib::CanFrame(); can_tx_frame_.id = can_command_id; can_tx_frame_.dlc = 2; can_tx_frame_.data[0] = MAINTENANCE_CMD + (joint_node_ids[i] & 0x0F); - can_tx_frame_.data[1] = 3; // DC Motor: Shutdown + can_tx_frame_.data[1] = MAINTENANCE_SHUTDOWN; canBus.send(can_tx_frame_); } @@ -341,7 +347,7 @@ hardware_interface::CallbackReturn DCHardwareInterface::on_deactivate( can_tx_frame_.id = can_command_id; can_tx_frame_.dlc = 2; can_tx_frame_.data[0] = MAINTENANCE_CMD + (joint_node_ids[i] & 0x0F); - can_tx_frame_.data[1] = 2; // DC Motor: Stop + can_tx_frame_.data[1] = MAINTENANCE_STOP; canBus.send(can_tx_frame_); } From 57ee329eb8f5b77d175df54de5edbccbac4e63aa Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Fri, 3 Apr 2026 15:20:13 -0400 Subject: [PATCH 27/37] Fixing HWI issue --- .../dc_ros2_control/src/dc_hardware_interface.cpp | 6 ------ 1 file changed, 6 deletions(-) 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 index 2bef74b7..6a92f5ed 100644 --- a/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp +++ b/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp @@ -298,12 +298,6 @@ hardware_interface::CallbackReturn DCHardwareInterface::on_cleanup( { RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Cleaning up... please wait..."); - // Stop worker thread if running - is_running.store(false); - if (worker.joinable()) { - worker.join(); - } - // Send shutdown command (maintenance sub-command 3) to all joints for (int i = 0; i < num_joints; i++) { can_tx_frame_ = CANLib::CanFrame(); From bbc9f8b5fcf179ffab4a4fda756ea69d33eb07f0 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Sun, 3 May 2026 15:54:16 -0400 Subject: [PATCH 28/37] Small Fix --- .../ros2_control/science/science.dc.ros2_control.xacro | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/description/ros2_control/science/science.dc.ros2_control.xacro b/src/description/ros2_control/science/science.dc.ros2_control.xacro index 27eb71b2..442585dc 100644 --- a/src/description/ros2_control/science/science.dc.ros2_control.xacro +++ b/src/description/ros2_control/science/science.dc.ros2_control.xacro @@ -17,7 +17,7 @@ 0 revolute - + 50.9 true @@ -31,7 +31,7 @@ 2 revolute - + 50.9 true From 6bb312edf18a19f3447def763e2e36052337957f Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Fri, 1 May 2026 18:36:16 -0400 Subject: [PATCH 29/37] First test for steppers --- .../science.stepper.ros2_control.xacro | 36 +- .../stepper_ros2_control/CMakeLists.txt | 3 - .../stepper_hardware_interface.hpp | 38 ++- .../stepper_ros2_control/package.xml | 4 - .../src/stepper_hardware_interface.cpp | 323 +++++++++++++++--- 5 files changed, 321 insertions(+), 83 deletions(-) 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 4c65551c..f57a5bdf 100644 --- a/src/description/ros2_control/science/science.stepper.ros2_control.xacro +++ b/src/description/ros2_control/science/science.stepper.ros2_control.xacro @@ -7,9 +7,18 @@ stepper_ros2_control/STEPPERHardwareInterface + 10 + 5 + 0 + can0 + 0x90 + 0x2 + 1 + 20 + continuous @@ -18,19 +27,14 @@ - 0.0 - - - - 0x151 - 1 - - - 1 - + + 0x3 + 1 + 20 + continuous @@ -39,21 +43,11 @@ - 0.0 - - - - 0x152 - 1 - - - 1 - - + \ No newline at end of file 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 a9cdb3a8..83d47ea0 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,12 +1,26 @@ +// 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 STEPPER_HARDWARE_INTERFACE_HPP_ #define STEPPER_HARDWARE_INTERFACE_HPP_ -#include -#include #include #include #include #include +#include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -20,9 +34,17 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +// Real-time CAN communication library +#include +#include +#include +#include + +#include "umdloop_can_library/SocketCanBus.hpp" +#include "umdloop_can_library/CanFrame.hpp" + namespace stepper_ros2_control { - class STEPPERHardwareInterface : public hardware_interface::SystemInterface { public: @@ -60,6 +82,16 @@ class STEPPERHardwareInterface : public hardware_interface::SystemInterface POSITION = 1, VELOCITY = 2, }; + + // 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 = 0x40; + static constexpr uint8_t MOTOR_STATUS_CMD = 0x50; + static constexpr uint8_t MAINTENANCE_CMD = 0x60; + static constexpr uint8_t SERVO_SPECS_CMD = 0x70; struct StepperJoint { 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 862a582f..1e6791e4 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,9 +1,26 @@ +// 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 "stepper_ros2_control/stepper_hardware_interface.hpp" #include -#include #include +#include #include +#include +#include #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_list_macros.hpp" @@ -110,6 +127,53 @@ bool STEPPERHardwareInterface::format_maintenance_command( default: return false; } +void STEPPERHardwareInterface::logger_function() +{ + // Prevents breaking the logger + if (num_joints == 0) return; + + // Building Message + std::string log_msg = "\033[2J\033[H \nSTEPPER Logger"; + std::ostringstream oss; + std::string status; + + // HWI Specific + 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: " << 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; + } + + 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] + << " - " << status << "\n" + << "-- Commands --\n" + << "Control Mode: " << static_cast(control_level_[i]) << "\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"; + } + + log_msg += oss.str(); + RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), log_msg.c_str()); } hardware_interface::CallbackReturn STEPPERHardwareInterface::on_init( @@ -249,92 +313,247 @@ std::vector STEPPERHardwareInterface::expo hardware_interface::CallbackReturn STEPPERHardwareInterface::on_configure( const rclcpp_lifecycle::State &) { - node_ = rclcpp::Node::make_shared("stepper_hardware_node"); - science_can_publisher_ = node_->create_publisher("can_tx", rclcpp::QoS(10)); - science_can_subscriber_ = node_->create_subscription( - "can_rx", rclcpp::QoS(50).reliable(), - [this](const msgs::msg::CANA::SharedPtr message) { - received_joint_data_ = *message; - }); + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Configuring stepper hardware..."); + + if (!canBus_.open(can_interface, + std::bind(&STEPPERHardwareInterface::onCanMessage, this, std::placeholders::_1))) + { + RCLCPP_WARN( + rclcpp::get_logger("STEPPERHardwareInterface"), + "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("STEPPERHardwareInterface"), + "Successfully opened CAN interface %s", can_interface.c_str()); + } + + is_connected_ = can_connected_ ? 1.0 : 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Stepper hardware configured (%s)", can_connected_ ? "CAN MODE" : "SIMULATION"); + return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn STEPPERHardwareInterface::on_cleanup( - const rclcpp_lifecycle::State &) +// Per protocol spec, response decoding uses 16-bit values (not 24-bit). +// All responses return: data[1-2] = position (int16, deg), data[3-4] = velocity (int16, deg/s). +void STEPPERHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { - for (const auto & joint : STEPPERJoints_) { - msgs::msg::CANA frame; - if (format_maintenance_command( - frame, joint.node_id, - DecodedCommand{static_cast(MaintenanceCommands::MOTOR_SHUTDOWN_CMD), {}, {}, {}})) - { - science_can_publisher_->publish(frame); + RCLCPP_INFO( + rclcpp::get_logger("STEPPER"), + "RX id=0x%X dlc=%d b0=0x%02X", + frame.id, frame.dlc, frame.data[0]); + + can_rx_frame_ = frame; + + if (can_rx_frame_.id != can_response_id) { + return; + } + + uint8_t command_nibble = (can_rx_frame_.data[0] >> 4) & 0x0F; + uint8_t device_id_nibble = can_rx_frame_.data[0] & 0x0F; + + for (int i = 0; i < num_joints; i++) { + if (device_id_nibble != static_cast(joint_node_ids[i] & 0x0F)) { + continue; } - if (format_maintenance_command( - frame, joint.node_id, - DecodedCommand{static_cast(MaintenanceCommands::BRAKE_LOCK_CMD), {}, {}, {}})) - { - science_can_publisher_->publish(frame); + + if (command_nibble == MOTOR_STATE_CMD) { + // Per protocol spec, position and velocity are 16-bit signed (deg, deg/s), little endian + int16_t raw_position = static_cast( + can_rx_frame_.data[1] | (can_rx_frame_.data[2] << 8)); + int16_t raw_velocity = static_cast( + can_rx_frame_.data[3] | (can_rx_frame_.data[4] << 8)); + + // Convert deg -> rad and apply gear ratio + motor_position[i] = (static_cast(raw_position) * M_PI / 180.0) / joint_gear_ratios[i]; + motor_velocity[i] = (static_cast(raw_velocity) * M_PI / 180.0) / joint_gear_ratios[i]; + + } else if (command_nibble == MOTOR_STATUS_CMD) { + device_status[i] = can_rx_frame_.data[1]; } - } - science_can_publisher_.reset(); - science_can_subscriber_.reset(); - node_.reset(); - return hardware_interface::CallbackReturn::SUCCESS; + break; // Matched joint, no need to keep iterating + } } hardware_interface::CallbackReturn STEPPERHardwareInterface::on_activate( const rclcpp_lifecycle::State &) { - for (auto & joint : STEPPERJoints_) { - msgs::msg::CANA frame; - if (format_maintenance_command( - frame, joint.node_id, - DecodedCommand{static_cast(MaintenanceCommands::BRAKE_RELEASE_CMD), {}, {}, {}})) - { - science_can_publisher_->publish(frame); - } - joint.joint_command_position = joint.initial_position; - joint.joint_command_velocity = 0.0; + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Activating ...please wait..."); + + joint_command_position_ = joint_state_position_; + + 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"), + "Stepper hardware activated"); + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn STEPPERHardwareInterface::on_deactivate( const rclcpp_lifecycle::State &) { - for (const auto & joint : STEPPERJoints_) { - msgs::msg::CANA frame; - if (format_maintenance_command( - frame, joint.node_id, - DecodedCommand{static_cast(MaintenanceCommands::MOTOR_STOP_CMD), {}, {}, {}})) - { - science_can_publisher_->publish(frame); + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Deactivating stepper hardware..."); + + if (can_connected_) { + 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; + + // (MAINTENANCE_CMD << 4) | port_id = 0x60 | port_id, maintenance cmd 1 = Stop stepper + uint8_t device_id_nibble = joint_node_ids[i] & 0x0F; + can_tx_frame_.data[0] = static_cast((MAINTENANCE_CMD << 4) | device_id_nibble); + can_tx_frame_.data[1] = 1; // Maintenance cmd 1 = Stop stepper + canBus_.send(can_tx_frame_); } } + + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Stepper hardware deactivated%s", can_connected_ ? "" : " (simulated)"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +// can_connected_ and is_connected_ are always reset regardless of whether CAN was open. +hardware_interface::CallbackReturn STEPPERHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Cleaning up stepper hardware..."); + + if (can_connected_) { + 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; + + // FIX #4: Assign device_id_nibble (was previously using uninitialized variable) + uint8_t device_id_nibble = joint_node_ids[i] & 0x0F; + can_tx_frame_.data[0] = static_cast((MAINTENANCE_CMD << 4) | device_id_nibble); + can_tx_frame_.data[1] = 2; // Maintenance cmd 2 = Shutdown stepper + canBus_.send(can_tx_frame_); + } + + canBus_.close(); + } + + // Always reset connection state + can_connected_ = false; + is_connected_ = 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Stepper hardware cleanup complete"); + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::return_type STEPPERHardwareInterface::read( const rclcpp::Time &, const rclcpp::Duration &) { - if (!node_) { - return hardware_interface::return_type::ERROR; + if (can_connected_) { + current_joint_ = (current_joint_ + 1) % num_joints; + + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 2; + + // MOTOR_STATE_CMD = 0x4 → (0x4 << 4) | port_id = 0x40 | port_id + uint8_t port_id = joint_node_ids[current_joint_] & 0x0F; + can_tx_frame_.data[0] = static_cast((MOTOR_STATE_CMD << 4) | port_id); + can_tx_frame_.data[1] = 0x01; // Validate the request + canBus_.send(can_tx_frame_); } - if (rclcpp::ok()) { - rclcpp::spin_some(node_); + // Copy motor state (updated asynchronously by onCanMessage) into joint state + for (int i = 0; i < num_joints; i++) { + joint_state_position_[i] = motor_position[i]; + joint_state_velocity_[i] = motor_velocity[i]; + + // Return error on any fault status + if (device_status[i] != 0x00 && device_status[i] != 0x01 && device_status[i] != -1) { + RCLCPP_ERROR( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Joint %s has fault device_status=0x%02X", info_.joints[i].name.c_str(), device_status[i]); + return hardware_interface::return_type::ERROR; + } } - if (received_joint_data_.data.size() < 8) { + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type STEPPERHardwareInterface::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + elapsed_update_time += period.seconds(); + double update_period = 1.0 / update_rate; + + // Rate-limit CAN messages to configured update rate + if (elapsed_update_time < update_period) { return hardware_interface::return_type::OK; } - for (auto & joint : STEPPERJoints_) { - if (received_joint_data_.id != joint.node_id) { - continue; + elapsed_update_time = 0.0; + elapsed_time += period.seconds(); + + for (int i = 0; i < num_joints; i++) { + if (control_level_[i] == integration_level_t::VELOCITY && + std::isfinite(joint_command_velocity_[i])) + { + // Stepper only accepts three discrete speeds: +900, -900, or 0 (deg/s). + // Map the signed velocity command to the nearest valid value. + int16_t speed_dps; + if (joint_command_velocity_[i] > 0.0) { + speed_dps = 900; + } else if (joint_command_velocity_[i] < 0.0) { + speed_dps = -900; + } else { + speed_dps = 0; + } + + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 3; // 3 bytes per spec + + // VELOCITY_CONTROL_CMD = 0x3 → (0x3 << 4) | port_id = 0x30 | port_id + uint8_t port_id = joint_node_ids[i] & 0x0F; + can_tx_frame_.data[0] = static_cast((VELOCITY_CONTROL_CMD << 4) | port_id); + can_tx_frame_.data[1] = static_cast(speed_dps & 0xFF); // low byte + can_tx_frame_.data[2] = static_cast((speed_dps >> 8) & 0xFF); // high byte + + canBus_.send(can_tx_frame_); + } } + else if (control_level_[i] == integration_level_t::POSITION && + std::isfinite(joint_command_position_[i])) + { + // Convert rad -> deg, apply gear ratio, clamp to int16 range + int16_t position_deg = static_cast(std::clamp( + joint_command_position_[i] * (180.0 / M_PI) * joint_gear_ratios[i], + static_cast(std::numeric_limits::min()), + static_cast(std::numeric_limits::max()) + )); if (received_joint_data_.data[0] == static_cast(StatusCommands::MOTOR_STATUS_2_CMD)) { joint.encoder_position = static_cast( From dd8c1e558729a1467574b4e9b8c768b80d9a410a Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Thu, 13 Nov 2025 15:47:13 -0500 Subject: [PATCH 30/37] Moved revised athena_science code to monorepo --- .vscode/settings.json | 3 + .../science/science.mock.ros2_control.xacro | 93 ------- .../urdf/athena_science_macro.xacro | 254 ++++++++++++++++++ ...trollers.yaml => science_controllers.yaml} | 0 .../science_controllers/CMakeLists.txt | 2 +- .../science/science_controllers/package.xml | 4 +- 6 files changed, 260 insertions(+), 96 deletions(-) create mode 100644 .vscode/settings.json delete mode 100644 src/description/ros2_control/science/science.mock.ros2_control.xacro create mode 100644 src/description/urdf/athena_science_macro.xacro rename src/subsystems/science/science_bringup/config/{athena_science_controllers.yaml => science_controllers.yaml} (100%) diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..b6615b22 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "cmake.sourceDirectory": "/home/henry/ros2_new_ws/src/athena-code/src/subsystems/science/science_bringup" +} \ No newline at end of file diff --git a/src/description/ros2_control/science/science.mock.ros2_control.xacro b/src/description/ros2_control/science/science.mock.ros2_control.xacro deleted file mode 100644 index f1686fcc..00000000 --- a/src/description/ros2_control/science/science.mock.ros2_control.xacro +++ /dev/null @@ -1,93 +0,0 @@ - - - - - - - - mock_components/GenericSystem - false - - false - false - false - 0.0 - - - - - - - - - - - - - - - - - - - 1 - - 0 - 1 - - - - 0.0 - - - - - - 2 - - 0 - 1 - - - - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/description/urdf/athena_science_macro.xacro b/src/description/urdf/athena_science_macro.xacro new file mode 100644 index 00000000..2e90a2bf --- /dev/null +++ b/src/description/urdf/athena_science_macro.xacro @@ -0,0 +1,254 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml b/src/subsystems/science/science_bringup/config/science_controllers.yaml similarity index 100% rename from src/subsystems/science/science_bringup/config/athena_science_controllers.yaml rename to src/subsystems/science/science_bringup/config/science_controllers.yaml diff --git a/src/subsystems/science/science_controllers/CMakeLists.txt b/src/subsystems/science/science_controllers/CMakeLists.txt index 362ec6cf..7819021f 100644 --- a/src/subsystems/science/science_controllers/CMakeLists.txt +++ b/src/subsystems/science/science_controllers/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(science_controllers) +project(athena_science_controllers) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Wno-shadow) diff --git a/src/subsystems/science/science_controllers/package.xml b/src/subsystems/science/science_controllers/package.xml index 0917cbe1..3041977e 100644 --- a/src/subsystems/science/science_controllers/package.xml +++ b/src/subsystems/science/science_controllers/package.xml @@ -1,7 +1,7 @@ - science_controllers + athena_science_controllers 0.0.0 Contains Ishan Dutta @@ -28,7 +28,7 @@ std_srvs - + ament_cmake From e2e3064f24d3cfb38fac6e38e0a506a2bc435bc2 Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Mon, 17 Nov 2025 17:22:50 -0500 Subject: [PATCH 31/37] Fixing Science Controller + HWI Templates for Steppers & Servos --- .vscode/settings.json | 68 ++++- .../urdf/athena_science_macro.xacro | 254 ------------------ ...s.yaml => athena_science_controllers.yaml} | 0 .../science_controllers/CMakeLists.txt | 2 +- .../science/science_controllers/package.xml | 4 +- 5 files changed, 70 insertions(+), 258 deletions(-) delete mode 100644 src/description/urdf/athena_science_macro.xacro rename src/subsystems/science/science_bringup/config/{science_controllers.yaml => athena_science_controllers.yaml} (100%) diff --git a/.vscode/settings.json b/.vscode/settings.json index b6615b22..02e32c61 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,3 +1,69 @@ { - "cmake.sourceDirectory": "/home/henry/ros2_new_ws/src/athena-code/src/subsystems/science/science_bringup" + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp" + } } \ No newline at end of file diff --git a/src/description/urdf/athena_science_macro.xacro b/src/description/urdf/athena_science_macro.xacro deleted file mode 100644 index 2e90a2bf..00000000 --- a/src/description/urdf/athena_science_macro.xacro +++ /dev/null @@ -1,254 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/subsystems/science/science_bringup/config/science_controllers.yaml b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml similarity index 100% rename from src/subsystems/science/science_bringup/config/science_controllers.yaml rename to src/subsystems/science/science_bringup/config/athena_science_controllers.yaml diff --git a/src/subsystems/science/science_controllers/CMakeLists.txt b/src/subsystems/science/science_controllers/CMakeLists.txt index 7819021f..362ec6cf 100644 --- a/src/subsystems/science/science_controllers/CMakeLists.txt +++ b/src/subsystems/science/science_controllers/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(athena_science_controllers) +project(science_controllers) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Wno-shadow) diff --git a/src/subsystems/science/science_controllers/package.xml b/src/subsystems/science/science_controllers/package.xml index 3041977e..0917cbe1 100644 --- a/src/subsystems/science/science_controllers/package.xml +++ b/src/subsystems/science/science_controllers/package.xml @@ -1,7 +1,7 @@ - athena_science_controllers + science_controllers 0.0.0 Contains Ishan Dutta @@ -28,7 +28,7 @@ std_srvs - + ament_cmake From 3a4268384c8593fc356e26f9b4b95878d9e390eb Mon Sep 17 00:00:00 2001 From: HGardiner1 Date: Fri, 1 May 2026 19:30:59 -0400 Subject: [PATCH 32/37] Restore accidentally deleted science.mock.ros2_control.xacro --- .../science/science.mock.ros2_control.xacro | 93 +++++++++++++++++++ 1 file changed, 93 insertions(+) create mode 100644 src/description/ros2_control/science/science.mock.ros2_control.xacro diff --git a/src/description/ros2_control/science/science.mock.ros2_control.xacro b/src/description/ros2_control/science/science.mock.ros2_control.xacro new file mode 100644 index 00000000..37987cf4 --- /dev/null +++ b/src/description/ros2_control/science/science.mock.ros2_control.xacro @@ -0,0 +1,93 @@ + + + + + + + + mock_components/GenericSystem + false + + false + false + false + 0.0 + + + + + + + + + + + + + + + + + + + 1 + + 0 + 1 + + + + 0.0 + + + + + + 2 + + 0 + 1 + + + + 0.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From edcd60399e55b3d45b5a31d9376ef9886ca3d8d4 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Fri, 1 May 2026 23:14:36 -0400 Subject: [PATCH 33/37] stepper stuff like kinda works?? --- .gitignore | 1 + .../science/science.stepper.ros2_control.xacro | 4 ++-- .../src/stepper_hardware_interface.cpp | 13 ++++++++----- .../science/science_bringup/config/joystick.yaml | 3 ++- 4 files changed, 13 insertions(+), 8 deletions(-) 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/science/science.stepper.ros2_control.xacro b/src/description/ros2_control/science/science.stepper.ros2_control.xacro index f57a5bdf..8d69e6d1 100644 --- a/src/description/ros2_control/science/science.stepper.ros2_control.xacro +++ b/src/description/ros2_control/science/science.stepper.ros2_control.xacro @@ -15,7 +15,7 @@ - 0x2 + 0x0 1 20 continuous @@ -31,7 +31,7 @@ - 0x3 + 0x1 1 20 continuous 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 1e6791e4..9fc91af6 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 @@ -345,10 +345,10 @@ hardware_interface::CallbackReturn STEPPERHardwareInterface::on_configure( // All responses return: data[1-2] = position (int16, deg), data[3-4] = velocity (int16, deg/s). void STEPPERHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { - RCLCPP_INFO( - rclcpp::get_logger("STEPPER"), - "RX id=0x%X dlc=%d b0=0x%02X", - frame.id, frame.dlc, frame.data[0]); + // RCLCPP_INFO( + // rclcpp::get_logger("STEPPER"), + // "RX id=0x%X dlc=%d b0=0x%02X", + // frame.id, frame.dlc, frame.data[0]); can_rx_frame_ = frame; @@ -538,10 +538,13 @@ hardware_interface::return_type STEPPERHardwareInterface::write( // VELOCITY_CONTROL_CMD = 0x3 → (0x3 << 4) | port_id = 0x30 | port_id uint8_t port_id = joint_node_ids[i] & 0x0F; - can_tx_frame_.data[0] = static_cast((VELOCITY_CONTROL_CMD << 4) | port_id); + can_tx_frame_.data[0] = static_cast((VELOCITY_CONTROL_CMD & 0xF0) | port_id); can_tx_frame_.data[1] = static_cast(speed_dps & 0xFF); // low byte can_tx_frame_.data[2] = static_cast((speed_dps >> 8) & 0xFF); // high byte + RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Data[0]: 0x%02X | Data[1-2] (speed_dps): %d | Joint: %s", + can_tx_frame_.data[0], speed_dps, info_.joints[i].name.c_str()); + canBus_.send(can_tx_frame_); } } 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 From d8f3610bfb00318d7c79542feb3d8a83868d4394 Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Sun, 3 May 2026 21:39:25 -0400 Subject: [PATCH 34/37] implemented and tested in virtual can servo, stepper, and dc HWIs --- .../science/science.dc.ros2_control.xacro | 18 +- .../dc_ros2_control/dc_hardware_interface.hpp | 188 ++++- .../src/dc_hardware_interface.cpp | 712 ++++++++++++------ .../servo_hardware_interface.hpp | 53 +- .../src/servo_hardware_interface.cpp | 123 ++- .../stepper_hardware_interface.hpp | 130 ++-- .../src/stepper_hardware_interface.cpp | 651 ++++++++-------- src/subsystems/general/README.md | 22 + .../config/athena_science_controllers.yaml | 80 ++ 9 files changed, 1286 insertions(+), 691 deletions(-) diff --git a/src/description/ros2_control/science/science.dc.ros2_control.xacro b/src/description/ros2_control/science/science.dc.ros2_control.xacro index 442585dc..c93a807b 100644 --- a/src/description/ros2_control/science/science.dc.ros2_control.xacro +++ b/src/description/ros2_control/science/science.dc.ros2_control.xacro @@ -7,9 +7,9 @@ dc_ros2_control/DCHardwareInterface - 20 + 5 5 - 1 + 0 can0 0x70 @@ -22,8 +22,14 @@ true + + + + + + @@ -36,8 +42,14 @@ true + + + + + + @@ -46,4 +58,4 @@ - \ 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 index 5284c343..a9f9e196 100644 --- 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 @@ -2,8 +2,11 @@ #define DC_HARDWARE_INTERFACE_HPP_ #include +#include +#include #include #include +#include #include #include @@ -15,13 +18,14 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include -#include -#include - #include "umdloop_can_library/SocketCanBus.hpp" #include "umdloop_can_library/CanFrame.hpp" +namespace CANLib +{ +struct CanFrame; +} + namespace dc_ros2_control { class DCHardwareInterface : public hardware_interface::SystemInterface @@ -76,6 +80,19 @@ class DCHardwareInterface : public hardware_interface::SystemInterface 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; @@ -116,6 +133,20 @@ class DCHardwareInterface : public hardware_interface::SystemInterface // 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; @@ -153,24 +184,139 @@ class DCHardwareInterface : public hardware_interface::SystemInterface std::vector joint_type_; - // CAN Command Bytes (full byte values, used as: CMD + port_id) - static constexpr uint8_t PCB_HEARTBEAT_CMD = 0x10; - static constexpr uint8_t LED_STATUS_CMD = 0x11; - static constexpr uint8_t ABSOLUTE_POS_CONTROL_CMD = 0x20; - static constexpr uint8_t VELOCITY_CONTROL_CMD = 0x30; - static constexpr uint8_t MOTOR_STATE_CMD = 0x40; - static constexpr uint8_t MOTOR_STATUS_CMD = 0x50; - static constexpr uint8_t MAINTENANCE_CMD = 0x60; - static constexpr uint8_t DC_SPECS_CMD = 0x70; - - // DC Motor Maintenance Sub-Commands - static constexpr uint8_t MAINTENANCE_ZERO_ROM = 0; - static constexpr uint8_t MAINTENANCE_REQ_VECTORS = 1; - static constexpr uint8_t MAINTENANCE_STOP = 2; - static constexpr uint8_t MAINTENANCE_SHUTDOWN = 3; - static constexpr uint8_t MAINTENANCE_CLEAR_ERRORS = 4; + 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_ \ No newline at end of file +#endif // DC_HARDWARE_INTERFACE_HPP_ 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 index 6a92f5ed..0c505a78 100644 --- a/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp +++ b/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp @@ -4,16 +4,14 @@ #include #include #include -#include -#include #include #include #include #include #include +#include #include "hardware_interface/system_interface.hpp" -#include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" @@ -64,6 +62,131 @@ int16_t DCHardwareInterface::calculate_motor_velocity_from_desired_joint_linear_ 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 // ============================================================================= @@ -88,49 +211,50 @@ void DCHardwareInterface::logger_function() << "\n--- Joint Specific ---"; for (int i = 0; i < num_joints; i++) { - switch (control_level_[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_type_[i]) { + 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 (device_status[i]) { - case 0: status = "Undefined"; break; - case 1: status = "Idle"; break; - case 2: status = "Startup Sequence"; break; - case 3: status = "Error (Invalid Request)"; break; - case 4: status = "Error (Motor Disarmed)"; break; - case 5: status = "Error (Motor Failed)"; break; - case 6: status = "Error (Controller Failed)"; break; - case 7: status = "Error (ESTOP Requested)"; break; - case 8: status = "Error (Unknown Position)"; break; - case 9: status = "Position Control"; break; - case 10: status = "Velocity Control"; break; - case 11: status = "Motor Stopped"; break; - default: status = "UNKNOWN (" + std::to_string(device_status[i]) + ")"; break; + 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: " << info_.joints[i].name << "\n" - << "Parameters: Node ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] - << " | Gear Ratio: " << std::dec << joint_gear_ratios[i] - << " | Inverted: " << (joint_inverted[i] ? "true" : "false") + 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_command_position_[i] - << " | Joint Command Velocity: " << joint_command_velocity_[i] << "\n" + << "Joint Command Position: " << joint.joint_command_position + << " | Joint Command Velocity: " << joint.joint_command_velocity << "\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 << "\n" << "-- Telemetry --\n" - << "Temperature: " << motor_temperature_[i] << " C" - << " | Torque Current: " << motor_torque_current_[i] << " A\n"; + << "Temperature: " << joint.motor_temperature << " C" + << " | Torque Current: " << joint.motor_torque_current << " A\n"; } log_msg += oss.str(); @@ -150,45 +274,92 @@ hardware_interface::CallbackReturn DCHardwareInterface::on_init( return hardware_interface::CallbackReturn::ERROR; } - // -- Per-joint parameters -- - for (auto& joint : info_.joints) { - joint_node_ids.push_back( - std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x0, 0xF)); - - // Gear ratio (default: 1) - if (joint.parameters.count("gear_ratio")) { - joint_gear_ratios.push_back(std::abs(std::stoi(joint.parameters.at("gear_ratio")))); - } else { - joint_gear_ratios.push_back(1); + 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); } - // Inverted (default: false) - if (joint.parameters.count("inverted")) { - joint_inverted.push_back(joint.parameters.at("inverted") == "true"); - } else { - joint_inverted.push_back(false); + std::vector command_if_names; + for (const auto & ci : joint.command_interfaces) { + command_if_names.push_back(ci.name); } - // Joint type (default: revolute) - std::string jtype = "revolute"; - if (joint.parameters.count("joint_type")) { - jtype = joint.parameters.at("joint_type"); + 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_.push_back(joint_type_t::REVOLUTE); - max_disp.push_back(std::nan("")); - distance_per_rev.push_back(0.0); - } else if (jtype == "prismatic" && joint.parameters.count("max_disp") && joint.parameters.count("distance_per_rev")) { - joint_type_.push_back(joint_type_t::PRISMATIC); - max_disp.push_back(std::abs(std::stod(joint.parameters.at("max_disp")))); - distance_per_rev.push_back(std::stod(joint.parameters.at("distance_per_rev"))); + 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"), + 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 -- @@ -205,28 +376,6 @@ hardware_interface::CallbackReturn DCHardwareInterface::on_init( elapsed_logger_time = 0.0; write_count = 0; - // -- Command and State Interface initialization -- - joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); - prev_joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); - joint_state_velocity_.assign(num_joints, 0.0); - prev_joint_state_velocity_.assign(num_joints, 0.0); - - joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); - prev_joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); - joint_command_velocity_.assign(num_joints, 0.0); - prev_joint_command_velocity_.assign(num_joints, 0.0); - - motor_position.assign(num_joints, 0.0); - motor_velocity.assign(num_joints, 0.0); - device_status.assign(num_joints, 0); - - // Telemetry placeholders (TODO: populate via CAN telemetry command) - motor_temperature_.assign(num_joints, 0.0); - motor_torque_current_.assign(num_joints, 0.0); - - // Default control mode: position - control_level_.resize(num_joints, integration_level_t::POSITION); - return hardware_interface::CallbackReturn::SUCCESS; } @@ -245,17 +394,27 @@ DCHardwareInterface::export_state_interfaces() { std::vector state_interfaces; - for (int i = 0; i < num_joints; i++) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_state_position_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_state_velocity_[i])); - - // Telemetry interfaces (TODO: populate via CAN telemetry command) - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, "motor_temperature", &motor_temperature_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, "torque_current", &motor_torque_current_[i])); + 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; @@ -266,11 +425,31 @@ DCHardwareInterface::export_command_interfaces() { std::vector command_interfaces; - for (int i = 0; i < num_joints; i++) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_command_position_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_command_velocity_[i])); + 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; @@ -298,14 +477,20 @@ hardware_interface::CallbackReturn DCHardwareInterface::on_cleanup( { RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Cleaning up... please wait..."); - // Send shutdown command (maintenance sub-command 3) to all joints - for (int i = 0; i < num_joints; i++) { - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_command_id; - can_tx_frame_.dlc = 2; - can_tx_frame_.data[0] = MAINTENANCE_CMD + (joint_node_ids[i] & 0x0F); - can_tx_frame_.data[1] = MAINTENANCE_SHUTDOWN; - canBus.send(can_tx_frame_); + 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(); @@ -319,11 +504,14 @@ hardware_interface::CallbackReturn DCHardwareInterface::on_activate( RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Activating... please wait..."); // Initialize command positions to current state - joint_command_position_ = joint_state_position_; + for (auto & joint : DCJoints_) { + joint.joint_command_position = joint.joint_state_position; + joint.joint_command_velocity = 0.0; + } - for (size_t i = 0; i < joint_command_position_.size(); ++i) { + for (size_t i = 0; i < DCJoints_.size(); ++i) { RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), - "Joint %zu command position initialized to: %f", i, joint_command_position_[i]); + "Joint %zu command position initialized to: %f", i, DCJoints_[i].joint_command_position); } RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Successfully activated!"); @@ -335,14 +523,20 @@ hardware_interface::CallbackReturn DCHardwareInterface::on_deactivate( { RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Deactivating... please wait..."); - // Send stop command (maintenance sub-command 2) to all joints - for (int i = 0; i < num_joints; i++) { - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_command_id; - can_tx_frame_.dlc = 2; - can_tx_frame_.data[0] = MAINTENANCE_CMD + (joint_node_ids[i] & 0x0F); - can_tx_frame_.data[1] = MAINTENANCE_STOP; - canBus.send(can_tx_frame_); + 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!"); @@ -357,55 +551,80 @@ void DCHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { can_rx_frame_ = frame; - int data[7] = {0x00}; - uint8_t cmd_byte = can_rx_frame_.data[0]; - uint8_t command_nibble = cmd_byte & 0xF0; - uint8_t device_id_nibble = cmd_byte & 0x0F; - double raw_motor_position = 0.0; - double raw_motor_velocity = 0.0; - - for (int i = 0; i < num_joints; i++) { - if (can_rx_frame_.id != can_response_id || device_id_nibble != joint_node_ids[i]) - continue; + if (frame.id != can_response_id) { + return; + } - if (command_nibble == MOTOR_STATE_CMD || - command_nibble == ABSOLUTE_POS_CONTROL_CMD || - command_nibble == VELOCITY_CONTROL_CMD) - { - // DECODING CAN MESSAGE - data[1] = can_rx_frame_.data[1]; // Position low byte - data[2] = can_rx_frame_.data[2]; // Position high byte - data[3] = can_rx_frame_.data[3]; // Velocity low byte - data[4] = can_rx_frame_.data[4]; // Velocity high byte + const uint8_t command_nibble = static_cast((frame.data[0] >> 4) & 0x0F); + const uint8_t device_id_nibble = static_cast(frame.data[0] & 0x0F); - // POSITION: int16 sign extension (NOT int32) - raw_motor_position = static_cast(static_cast((data[2] << 8) | data[1])); + for (size_t i = 0; i < DCJoints_.size(); i++) { + auto & joint = DCJoints_[i]; + if (joint.node_id != device_id_nibble) { + continue; + } - // VELOCITY: int16 - raw_motor_velocity = static_cast(static_cast((data[4] << 8) | data[3])); + 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; + } - // Apply inversion - double dir = joint_inverted[i] ? -1.0 : 1.0; + if (command_nibble == (static_cast(StatusCommands::MOTOR_STATUS) >> 4)) { + joint.device_status = frame.data[1]; + joint.motor_status = static_cast(frame.data[1]); + return; + } - // CALCULATING JOINT STATE - if (joint_type_[i] == joint_type_t::REVOLUTE) { - motor_position[i] = dir * calculate_joint_position_from_motor_position(raw_motor_position, joint_gear_ratios[i]); - motor_velocity[i] = dir * calculate_joint_angular_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i]); - } - else if (joint_type_[i] == joint_type_t::PRISMATIC) { - motor_position[i] = dir * calculate_joint_displacement_from_motor_position(raw_motor_position, joint_gear_ratios[i], distance_per_rev[i]); - motor_velocity[i] = dir * calculate_joint_linear_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i], distance_per_rev[i]); - } - else { - RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), - "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); - } + if (command_nibble == (static_cast(MaintenanceCommands::MAINTENANCE_CMD) >> 4) && frame.data[0] == 1) { + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Successfully sent maintenance command"); + return; } - else if (command_nibble == MOTOR_STATUS_CMD) - { - device_status[i] = can_rx_frame_.data[1]; + + 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; } - // TODO: Handle telemetry response frames here when implemented } } @@ -416,26 +635,19 @@ void DCHardwareInterface::on_can_message(const CANLib::CanFrame& frame) hardware_interface::return_type DCHardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - for (int i = 0; i < num_joints; i++) { - if (prev_joint_state_velocity_[i] != motor_velocity[i]) { - joint_state_velocity_[i] = motor_velocity[i]; - prev_joint_state_velocity_[i] = joint_state_velocity_[i]; - } - - if (prev_joint_state_position_[i] != motor_position[i]) { - joint_state_position_[i] = motor_position[i]; - prev_joint_state_position_[i] = joint_state_position_[i]; - } + 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) - int s = device_status[i]; + 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", info_.joints[i].name.c_str(), s); + "Joint %s in error state: %d", joint.name.c_str(), s); return hardware_interface::return_type::ERROR; } - // TODO: Telemetry — populate motor_temperature_[i] and motor_torque_current_[i] via CAN + // TODO: Telemetry — populate motor_temperature and motor_torque_current via CAN } return hardware_interface::return_type::OK; @@ -448,86 +660,92 @@ hardware_interface::return_type DCHardwareInterface::read( hardware_interface::return_type DCHardwareInterface::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - elapsed_update_time += period.seconds(); elapsed_time += period.seconds(); - double update_period = 1.0 / update_rate; - // Logger update elapsed_logger_time += period.seconds(); - double logging_period = 1.0 / logger_rate; - if (elapsed_logger_time > logging_period) { + if (logger_rate > 0 && elapsed_logger_time > (1.0 / static_cast(logger_rate))) { elapsed_logger_time = 0.0; if (logger_state == 1) { logger_function(); } } - // Rate-limited CAN writes — all joints per tick - if (elapsed_update_time > update_period) { - elapsed_update_time = 0.0; - - for (int i = 0; i < num_joints; i++) { - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_command_id; - - // Apply inversion for outgoing commands - double dir = joint_inverted[i] ? -1.0 : 1.0; - - if (control_level_[i] == integration_level_t::POSITION && - std::isfinite(joint_command_position_[i]) && - joint_command_position_[i] != prev_joint_command_position_[i]) - { - // Clamp prismatic joints to [0, max_disp] - if (joint_type_[i] == joint_type_t::PRISMATIC) { - joint_command_position_[i] = std::clamp( - joint_command_position_[i], 0.0, max_disp[i]); - } - - int16_t motor_pos; - if (joint_type_[i] == joint_type_t::REVOLUTE) { - motor_pos = calculate_motor_position_from_desired_joint_position( - dir * joint_command_position_[i], joint_gear_ratios[i]); - } else { - motor_pos = calculate_motor_position_from_desired_joint_displacement( - dir * joint_command_position_[i], joint_gear_ratios[i], distance_per_rev[i]); + 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); } - - can_tx_frame_.dlc = 3; - can_tx_frame_.data[0] = ABSOLUTE_POS_CONTROL_CMD + (joint_node_ids[i] & 0x0F); - can_tx_frame_.data[1] = static_cast(motor_pos & 0xFF); - can_tx_frame_.data[2] = static_cast((motor_pos >> 8) & 0xFF); - - prev_joint_command_position_[i] = joint_command_position_[i]; } - else if (control_level_[i] == integration_level_t::VELOCITY && - std::isfinite(joint_command_velocity_[i]) && - joint_command_velocity_[i] != prev_joint_command_velocity_[i]) - { - int16_t motor_vel; - if (joint_type_[i] == joint_type_t::REVOLUTE) { - motor_vel = calculate_motor_velocity_from_desired_joint_angular_velocity( - dir * joint_command_velocity_[i], joint_gear_ratios[i]); - } else { - motor_vel = calculate_motor_velocity_from_desired_joint_linear_velocity( - dir * joint_command_velocity_[i], joint_gear_ratios[i], distance_per_rev[i]); + } 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; + } - can_tx_frame_.dlc = 3; - can_tx_frame_.data[0] = VELOCITY_CONTROL_CMD + (joint_node_ids[i] & 0x0F); - can_tx_frame_.data[1] = static_cast(motor_vel & 0xFF); - can_tx_frame_.data[2] = static_cast((motor_vel >> 8) & 0xFF); + 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; + } - prev_joint_command_velocity_[i] = joint_command_velocity_[i]; - } - else - { - // No new command — poll motor state - can_tx_frame_.dlc = 2; - can_tx_frame_.data[0] = MOTOR_STATE_CMD + (joint_node_ids[i] & 0x0F); - can_tx_frame_.data[1] = 1; // Validate request + 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; + } - canBus.send(can_tx_frame_); + 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); } } @@ -555,7 +773,7 @@ hardware_interface::return_type DCHardwareInterface::perform_command_mode_switch // Process stop interfaces for (const auto &ifname : stop_interfaces) { for (int i = 0; i < num_joints; ++i) { - if (ifname.find(info_.joints[i].name) != std::string::npos) { + if (ifname.find(DCJoints_[static_cast(i)].name) != std::string::npos) { requested_modes[i] = integration_level_t::UNDEFINED; } } @@ -564,8 +782,9 @@ hardware_interface::return_type DCHardwareInterface::perform_command_mode_switch // Process start interfaces for (const auto &ifname : start_interfaces) { for (int i = 0; i < num_joints; ++i) { - const std::string pos_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_POSITION); - const std::string vel_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_VELOCITY); + 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) { @@ -576,32 +795,33 @@ hardware_interface::return_type DCHardwareInterface::perform_command_mode_switch // 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(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_command_position_[i] = joint_state_position_[i]; + 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", info_.joints[i].name.c_str()); + "Joint %s: stopped -> UNDEFINED", joint.name.c_str()); } } else { - control_level_[i] = requested_modes[i]; + joint.control_level = requested_modes[i]; if (requested_modes[i] == integration_level_t::VELOCITY) { - joint_command_velocity_[i] = 0; + joint.joint_command_velocity = 0.0; RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), - "Joint %s: switched to VELOCITY", info_.joints[i].name.c_str()); + "Joint %s: switched to VELOCITY", joint.name.c_str()); } 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("DCHardwareInterface"), "Joint %s: switched to POSITION (initialized to %f)", - info_.joints[i].name.c_str(), joint_command_position_[i]); + joint.name.c_str(), joint.joint_command_position); } } } @@ -614,4 +834,4 @@ hardware_interface::return_type DCHardwareInterface::perform_command_mode_switch #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - dc_ros2_control::DCHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file + dc_ros2_control::DCHardwareInterface, 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 23661d73..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 @@ -168,31 +168,60 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface std::vector SERVOJoints_; - static constexpr uint8_t PCB_HEARTBEAT_CMD = 0x10; - static constexpr uint8_t ABSOLUTE_POS_CONTROL_CMD = 0x20; - static constexpr uint8_t VELOCITY_CONTROL_CMD = 0x30; - static constexpr uint8_t MOTOR_STATE_CMD = 0x40; - static constexpr uint8_t MOTOR_STATUS_CMD = 0x50; - static constexpr uint8_t MAINTENANCE_CMD = 0x60; - static constexpr uint8_t SERVO_SPECS_CMD = 0x70; + 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, + SERVO_SPECS_CMD = 0x70, + }; + + 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 = 0x01, - MOTOR_STATUS = 0x02, - SERVO_SPECS = 0x03, + MOTOR_STATE = 0x40, + MOTOR_STATUS = 0x50, + }; + + 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 + }; + + enum class ValidateRequest : uint8_t + { + INVALID = 0, + VALID = 1, }; - static constexpr std::array kStatusCommands = { + static constexpr std::array kStatusCommands = { StatusCommands::MOTOR_STATE, StatusCommands::MOTOR_STATUS, - StatusCommands::SERVO_SPECS, }; inline DecodedCommand unpack_command_full(int32_t counts_in, int64_t payload_in) 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 c7862860..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 @@ -116,6 +116,7 @@ void SERVOHardwareInterface::format_control_command(CANLib::CanFrame & frame, Se 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) { @@ -127,19 +128,19 @@ void SERVOHardwareInterface::format_control_command(CANLib::CanFrame & frame, Se } frame.dlc = 3; - frame.data[0] = static_cast(ABSOLUTE_POS_CONTROL_CMD + joint.node_id); + 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); - joint.prev_joint_command_position = joint.joint_command_position; return; } - - if ( + 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; @@ -152,31 +153,33 @@ void SERVOHardwareInterface::format_control_command(CANLib::CanFrame & frame, Se } frame.dlc = 3; - frame.data[0] = static_cast(VELOCITY_CONTROL_CMD + joint.node_id); + 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); - joint.prev_joint_command_velocity = joint.joint_command_velocity; return; } - - frame.dlc = 1; - frame.data[0] = static_cast(MOTOR_STATE_CMD + joint.node_id); + 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 = 1; + frame.dlc = 2; switch (static_cast(command_id)) { case StatusCommands::MOTOR_STATE: - frame.data[0] = static_cast(MOTOR_STATE_CMD + node_id); + 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(MOTOR_STATUS_CMD + node_id); - return true; - case StatusCommands::SERVO_SPECS: - frame.data[0] = static_cast(SERVO_SPECS_CMD + node_id); + frame.data[0] = static_cast(static_cast(StatusCommands::MOTOR_STATUS) + node_id); + frame.data[1] = static_cast(ValidateRequest::VALID); return true; default: return false; @@ -188,13 +191,38 @@ bool SERVOHardwareInterface::format_maintenance_command( { std::fill(std::begin(frame.data), std::end(frame.data), 0x00); frame.dlc = 2; - frame.data[0] = static_cast(MAINTENANCE_CMD + node_id); + 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::MOTOR_STOP_CMD: - case MaintenanceCommands::MOTOR_SHUTDOWN_CMD: - return true; + 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; } @@ -394,7 +422,9 @@ void SERVOHardwareInterface::on_can_message(const CANLib::CanFrame & frame) continue; } - if (command_nibble == (MOTOR_STATE_CMD >> 4)) { + 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( @@ -414,10 +444,43 @@ void SERVOHardwareInterface::on_can_message(const CANLib::CanFrame & frame) return; } - if (command_nibble == (MOTOR_STATUS_CMD >> 4)) { + if (command_nibble == (static_cast(StatusCommands::MOTOR_STATUS) >> 4)) { 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("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; + } } } @@ -429,7 +492,14 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_cleanup( frame.id = can_command_id; if (format_maintenance_command( frame, joint.node_id, - DecodedCommand{static_cast(MaintenanceCommands::MOTOR_SHUTDOWN_CMD), {}, {}, {}})) + DecodedCommand{ + static_cast(MaintenanceCommands::MAINTENANCE_CMD), + {static_cast(MaintenanceCommandOptions::MOTOR_SHUTDOWN_CMD)}, + {}, + {} + } + ) + ) { canBus.send(frame); } @@ -455,7 +525,14 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_deactivate( frame.id = can_command_id; if (format_maintenance_command( frame, joint.node_id, - DecodedCommand{static_cast(MaintenanceCommands::MOTOR_STOP_CMD), {}, {}, {}})) + DecodedCommand{ + static_cast(MaintenanceCommands::MAINTENANCE_CMD), + {static_cast(MaintenanceCommandOptions::MOTOR_STOP_CMD)}, + {}, + {} + } + ) + ) { canBus.send(frame); } 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 83d47ea0..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 @@ -16,6 +16,11 @@ #ifndef STEPPER_HARDWARE_INTERFACE_HPP_ #define STEPPER_HARDWARE_INTERFACE_HPP_ +#include + +#include +#include +#include #include #include #include @@ -26,22 +31,17 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "msgs/msg/cana.hpp" #include "rclcpp/macros.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/publisher.hpp" -#include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -// Real-time CAN communication library -#include -#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 stepper_ros2_control { @@ -82,32 +82,22 @@ class STEPPERHardwareInterface : public hardware_interface::SystemInterface POSITION = 1, VELOCITY = 2, }; - - // 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 = 0x40; - static constexpr uint8_t MOTOR_STATUS_CMD = 0x50; - static constexpr uint8_t MAINTENANCE_CMD = 0x60; - static constexpr uint8_t SERVO_SPECS_CMD = 0x70; struct StepperJoint { std::string name; - uint16_t node_id; + uint8_t node_id; int gear_ratio; int orientation; - double initial_position; 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 motor_status; - double acceleration; double joint_command_position; double joint_command_velocity; @@ -123,9 +113,6 @@ class STEPPERHardwareInterface : public hardware_interface::SystemInterface double prev_maintenance_req; double elapsed_status_request_time; double elapsed_maintenance_request_time; - double motor_position; - double motor_velocity; - double encoder_position; double prev_joint_command_position; double prev_joint_command_velocity; @@ -142,50 +129,92 @@ class STEPPERHardwareInterface : public hardware_interface::SystemInterface std::vector i32_data; }; + 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); + 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); - void format_control_command(msgs::msg::CANA & frame, StepperJoint & joint); - bool format_status_command(msgs::msg::CANA & frame, uint8_t command_id, uint16_t node_id); + 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( - msgs::msg::CANA & frame, uint16_t node_id, const DecodedCommand & decoded_cmd); + CANLib::CanFrame & frame, uint8_t node_id, const DecodedCommand & decoded_cmd); 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; - uint16_t current_iteration; - rclcpp::Publisher::SharedPtr science_can_publisher_; - rclcpp::Subscription::SharedPtr science_can_subscriber_; - rclcpp::Node::SharedPtr node_; - msgs::msg::CANA received_joint_data_; + 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; std::vector STEPPERJoints_; + enum class ControlCommands : uint8_t + { + RELATIVE_POS_CONTROL_CMD = 0x20, + VELOCITY_CONTROL_CMD = 0x30, + }; + enum class MaintenanceCommands : uint8_t { - BRAKE_RELEASE_CMD = 0x77, - BRAKE_LOCK_CMD = 0x78, - MOTOR_SHUTDOWN_CMD = 0x80, - MOTOR_STOP_CMD = 0x81, + PCB_HEARTBEAT_CMD = 0x10, + MAINTENANCE_CMD = 0x60, + STEPPER_SPECS_CMD = 0x70, }; - enum class ControlCommands : uint8_t + enum class MaintenanceCommandOptions : uint8_t { - SPEED_CONTROL_CMD = 0xA2, - ABSOLUTE_POS_CONTROL_CMD = 0xA4, + 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 { - READ_MULTI_TURN_ANGLE_CMD = 0x92, - MOTOR_STATUS_2_CMD = 0x9C, + 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 class ValidateRequest : uint8_t + { + INVALID = 0, + VALID = 1, }; static constexpr std::array kStatusCommands = { - StatusCommands::READ_MULTI_TURN_ANGLE_CMD, - StatusCommands::MOTOR_STATUS_2_CMD, + StatusCommands::MOTOR_STATE, + StatusCommands::MOTOR_STATUS, }; inline DecodedCommand unpack_command_full(int32_t counts_in, int64_t payload_in) @@ -200,7 +229,8 @@ class STEPPERHardwareInterface : public hardware_interface::SystemInterface 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); + const uint64_t mask = (bits == 64) ? std::numeric_limits::max() : + ((1ULL << bits) - 1ULL); return (payload >> bit_offset) & mask; }; 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 9fc91af6..0ede30d7 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 @@ -16,11 +16,9 @@ #include "stepper_ros2_control/stepper_hardware_interface.hpp" #include +#include #include -#include #include -#include -#include #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_list_macros.hpp" @@ -32,7 +30,7 @@ namespace stepper_ros2_control double STEPPERHardwareInterface::calculate_joint_position_from_motor_position( double motor_position, int gear_ratio) { - return (motor_position * 0.01 * (M_PI / 180.0)) / gear_ratio; + return (motor_position * (M_PI / 180.0)) / gear_ratio; } double STEPPERHardwareInterface::calculate_joint_velocity_from_motor_velocity( @@ -41,71 +39,138 @@ double STEPPERHardwareInterface::calculate_joint_velocity_from_motor_velocity( return (motor_velocity * (M_PI / 180.0)) / gear_ratio; } -int32_t STEPPERHardwareInterface::calculate_motor_position_from_desired_joint_position( +int16_t STEPPERHardwareInterface::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)); + return static_cast(std::round((joint_position * (180.0 / M_PI)) * gear_ratio)); } -int32_t STEPPERHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity( +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) * 100.0) * gear_ratio)); + return static_cast(std::round((joint_velocity * (180.0 / M_PI)) * gear_ratio)); } -void STEPPERHardwareInterface::format_control_command(msgs::msg::CANA & frame, StepperJoint & joint) +void STEPPERHardwareInterface::logger_function() { - frame.id = joint.node_id; - frame.data.assign(8, 0x00); + if (num_joints == 0) { + return; + } + + 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"; + } + + 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); 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_CONTROL_CMD); - frame.data[2] = 200; - frame.data[3] = 0x00; - 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; + 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; } - - if ( + 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; + 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); + } - frame.data[0] = static_cast(StatusCommands::MOTOR_STATUS_2_CMD); } bool STEPPERHardwareInterface::format_status_command( - msgs::msg::CANA & frame, uint8_t command_id, uint16_t node_id) + CANLib::CanFrame & frame, uint8_t command_id, uint8_t node_id) { - frame.id = node_id; - frame.data.assign(8, 0x00); + std::fill(std::begin(frame.data), std::end(frame.data), 0x00); + frame.dlc = 2; switch (static_cast(command_id)) { - case StatusCommands::READ_MULTI_TURN_ANGLE_CMD: - case StatusCommands::MOTOR_STATUS_2_CMD: - frame.data[0] = 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; @@ -113,67 +178,45 @@ bool STEPPERHardwareInterface::format_status_command( } bool STEPPERHardwareInterface::format_maintenance_command( - msgs::msg::CANA & frame, uint16_t node_id, const DecodedCommand & decoded_cmd) + CANLib::CanFrame & frame, uint8_t node_id, const DecodedCommand & decoded_cmd) { - frame.id = node_id; - frame.data.assign(8, 0x00); - frame.data[0] = decoded_cmd.command_id; + 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::BRAKE_RELEASE_CMD: - case MaintenanceCommands::BRAKE_LOCK_CMD: - case MaintenanceCommands::MOTOR_SHUTDOWN_CMD: - case MaintenanceCommands::MOTOR_STOP_CMD: + 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; } -void STEPPERHardwareInterface::logger_function() -{ - // Prevents breaking the logger - if (num_joints == 0) return; - - // Building Message - std::string log_msg = "\033[2J\033[H \nSTEPPER Logger"; - std::ostringstream oss; - std::string status; - - // HWI Specific - 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: " << 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; - } - - 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] - << " - " << status << "\n" - << "-- Commands --\n" - << "Control Mode: " << static_cast(control_level_[i]) << "\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"; - } - - log_msg += oss.str(); - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), log_msg.c_str()); } hardware_interface::CallbackReturn STEPPERHardwareInterface::on_init( @@ -186,6 +229,13 @@ hardware_interface::CallbackReturn STEPPERHardwareInterface::on_init( } 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) { @@ -202,49 +252,51 @@ hardware_interface::CallbackReturn STEPPERHardwareInterface::on_init( params_map.emplace(p.first, p.second); } - const uint16_t node_id = static_cast( - joint.parameters.count("node_id") ? std::stoi(joint.parameters.at("node_id"), nullptr, 0) : 0); - - StepperJoint stepper_joint{}; - stepper_joint.name = joint.name; - stepper_joint.node_id = node_id; - stepper_joint.gear_ratio = std::abs(std::stoi(joint.parameters.at("gear_ratio"))); - stepper_joint.orientation = joint.parameters.count("joint_orientation") && + const int orientation = joint.parameters.count("joint_orientation") && std::stoi(joint.parameters.at("joint_orientation")) == -1 ? -1 : 1; - stepper_joint.initial_position = joint.parameters.count("initial_position") ? - std::stod(joint.parameters.at("initial_position")) : 0.0; - stepper_joint.control_level = integration_level_t::POSITION; - stepper_joint.joint_state_position = 0.0; - stepper_joint.joint_state_velocity = 0.0; - stepper_joint.motor_temperature = 0.0; - stepper_joint.motor_torque_current = 0.0; - stepper_joint.motor_status = 0.0; - stepper_joint.acceleration = std::numeric_limits::quiet_NaN(); - stepper_joint.joint_command_position = 0.0; - stepper_joint.joint_command_velocity = 0.0; - stepper_joint.motor_status_req = 0.0; - stepper_joint.motor_maintenance_req = 0.0; - stepper_joint.maintenance_frame_high = 0.0; - stepper_joint.maintenance_frame_low = 0.0; - stepper_joint.maintenance_frame = 0.0; - stepper_joint.maintenance_data_count = 0.0; - stepper_joint.prev_status_req = 0.0; - stepper_joint.prev_maintenance_req = 0.0; - stepper_joint.elapsed_status_request_time = 0.0; - stepper_joint.elapsed_maintenance_request_time = 0.0; - stepper_joint.motor_position = 0.0; - stepper_joint.motor_velocity = 0.0; - stepper_joint.encoder_position = 0.0; - stepper_joint.prev_joint_command_position = std::numeric_limits::quiet_NaN(); - stepper_joint.prev_joint_command_velocity = std::numeric_limits::quiet_NaN(); - stepper_joint.state_interface_names = state_if_names; - stepper_joint.command_interface_names = command_if_names; - stepper_joint.parameters = params_map; - STEPPERJoints_.push_back(std::move(stepper_joint)); + 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()); - current_iteration = 0; + elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; + write_count = 0; return hardware_interface::CallbackReturn::SUCCESS; } @@ -264,17 +316,14 @@ std::vector STEPPERHardwareInterface::export 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; } - if (value != nullptr) { - state_interfaces.emplace_back(joint.name, iface, value); + if (value == nullptr) { + continue; } + state_interfaces.emplace_back(joint.name, iface, value); } } return state_interfaces; @@ -302,9 +351,10 @@ std::vector STEPPERHardwareInterface::expo value = &joint.maintenance_data_count; } - if (value != nullptr) { - command_interfaces.emplace_back(joint.name, iface, value); + if (value == nullptr) { + continue; } + command_interfaces.emplace_back(joint.name, iface, value); } } return command_interfaces; @@ -313,89 +363,113 @@ std::vector STEPPERHardwareInterface::expo hardware_interface::CallbackReturn STEPPERHardwareInterface::on_configure( const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("STEPPERHardwareInterface"), - "Configuring stepper hardware..."); - - if (!canBus_.open(can_interface, - std::bind(&STEPPERHardwareInterface::onCanMessage, this, std::placeholders::_1))) + if (!canBus.open( + can_interface, + std::bind(&STEPPERHardwareInterface::on_can_message, this, std::placeholders::_1))) { - RCLCPP_WARN( - rclcpp::get_logger("STEPPERHardwareInterface"), - "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("STEPPERHardwareInterface"), - "Successfully opened CAN interface %s", can_interface.c_str()); + RCLCPP_ERROR(rclcpp::get_logger("STEPPERHardwareInterface"), "Failed to open CAN interface"); + return hardware_interface::CallbackReturn::ERROR; } - - is_connected_ = can_connected_ ? 1.0 : 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("STEPPERHardwareInterface"), - "Stepper hardware configured (%s)", can_connected_ ? "CAN MODE" : "SIMULATION"); - return hardware_interface::CallbackReturn::SUCCESS; } -// Per protocol spec, response decoding uses 16-bit values (not 24-bit). -// All responses return: data[1-2] = position (int16, deg), data[3-4] = velocity (int16, deg/s). -void STEPPERHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) +void STEPPERHardwareInterface::on_can_message(const CANLib::CanFrame & frame) { - // RCLCPP_INFO( - // rclcpp::get_logger("STEPPER"), - // "RX id=0x%X dlc=%d b0=0x%02X", - // frame.id, frame.dlc, frame.data[0]); - can_rx_frame_ = frame; - - if (can_rx_frame_.id != can_response_id) { + if (frame.id != can_response_id) { return; } - uint8_t command_nibble = (can_rx_frame_.data[0] >> 4) & 0x0F; - uint8_t device_id_nibble = can_rx_frame_.data[0] & 0x0F; + 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 (int i = 0; i < num_joints; i++) { - if (device_id_nibble != static_cast(joint_node_ids[i] & 0x0F)) { + for (auto & joint : STEPPERJoints_) { + if (joint.node_id != device_id_nibble) { continue; } - if (command_nibble == MOTOR_STATE_CMD) { - // Per protocol spec, position and velocity are 16-bit signed (deg, deg/s), little endian - int16_t raw_position = static_cast( - can_rx_frame_.data[1] | (can_rx_frame_.data[2] << 8)); - int16_t raw_velocity = static_cast( - can_rx_frame_.data[3] | (can_rx_frame_.data[4] << 8)); + 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; + } - // Convert deg -> rad and apply gear ratio - motor_position[i] = (static_cast(raw_position) * M_PI / 180.0) / joint_gear_ratios[i]; - motor_velocity[i] = (static_cast(raw_velocity) * M_PI / 180.0) / joint_gear_ratios[i]; + if (command_nibble == (static_cast(StatusCommands::MOTOR_STATUS) >> 4)) { + joint.motor_status = static_cast(frame.data[1]); + return; + } - } else if (command_nibble == MOTOR_STATUS_CMD) { - device_status[i] = can_rx_frame_.data[1]; + if (command_nibble == (static_cast(MaintenanceCommands::MAINTENANCE_CMD) >> 4) && frame.data[0] == 1) { + RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Successfully sent maintenance command"); + return; } - break; // Matched joint, no need to keep iterating + 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]; + + const uint16_t max_stepper_position = + static_cast(frame.data[2]) | + (static_cast(frame.data[3]) << 8); + + const uint16_t max_stepper_velocity = + static_cast(frame.data[4]) | + (static_cast(frame.data[5]) << 8); + + 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::CallbackReturn STEPPERHardwareInterface::on_activate( +hardware_interface::CallbackReturn STEPPERHardwareInterface::on_cleanup( const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("STEPPERHardwareInterface"), - "Activating ...please wait..."); - - joint_command_position_ = joint_state_position_; + 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; +} - 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]); +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; } RCLCPP_INFO( @@ -408,21 +482,18 @@ hardware_interface::CallbackReturn STEPPERHardwareInterface::on_activate( hardware_interface::CallbackReturn STEPPERHardwareInterface::on_deactivate( const rclcpp_lifecycle::State &) { - RCLCPP_INFO( - rclcpp::get_logger("STEPPERHardwareInterface"), - "Deactivating stepper hardware..."); - - if (can_connected_) { - 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; - - // (MAINTENANCE_CMD << 4) | port_id = 0x60 | port_id, maintenance cmd 1 = Stop stepper - uint8_t device_id_nibble = joint_node_ids[i] & 0x0F; - can_tx_frame_.data[0] = static_cast((MAINTENANCE_CMD << 4) | device_id_nibble); - can_tx_frame_.data[1] = 1; // Maintenance cmd 1 = Stop stepper - canBus_.send(can_tx_frame_); + 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); } } @@ -471,134 +542,33 @@ hardware_interface::CallbackReturn STEPPERHardwareInterface::on_cleanup( hardware_interface::return_type STEPPERHardwareInterface::read( const rclcpp::Time &, const rclcpp::Duration &) { - if (can_connected_) { - current_joint_ = (current_joint_ + 1) % num_joints; - - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_command_id; - can_tx_frame_.dlc = 2; - - // MOTOR_STATE_CMD = 0x4 → (0x4 << 4) | port_id = 0x40 | port_id - uint8_t port_id = joint_node_ids[current_joint_] & 0x0F; - can_tx_frame_.data[0] = static_cast((MOTOR_STATE_CMD << 4) | port_id); - can_tx_frame_.data[1] = 0x01; // Validate the request - canBus_.send(can_tx_frame_); - } - - // Copy motor state (updated asynchronously by onCanMessage) into joint state - for (int i = 0; i < num_joints; i++) { - joint_state_position_[i] = motor_position[i]; - joint_state_velocity_[i] = motor_velocity[i]; - - // Return error on any fault status - if (device_status[i] != 0x00 && device_status[i] != 0x01 && device_status[i] != -1) { - RCLCPP_ERROR( - rclcpp::get_logger("STEPPERHardwareInterface"), - "Joint %s has fault device_status=0x%02X", info_.joints[i].name.c_str(), device_status[i]); - return hardware_interface::return_type::ERROR; - } + for (auto & joint : STEPPERJoints_) { + joint.joint_state_position = joint.motor_position; + joint.joint_state_velocity = joint.motor_velocity; } - return hardware_interface::return_type::OK; } hardware_interface::return_type STEPPERHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & period) + const rclcpp::Time &, const rclcpp::Duration & period) { - elapsed_update_time += period.seconds(); - double update_period = 1.0 / update_rate; - - // Rate-limit CAN messages to configured update rate - if (elapsed_update_time < update_period) { - return hardware_interface::return_type::OK; - } - - elapsed_update_time = 0.0; elapsed_time += period.seconds(); - - for (int i = 0; i < num_joints; i++) { - if (control_level_[i] == integration_level_t::VELOCITY && - std::isfinite(joint_command_velocity_[i])) - { - // Stepper only accepts three discrete speeds: +900, -900, or 0 (deg/s). - // Map the signed velocity command to the nearest valid value. - int16_t speed_dps; - if (joint_command_velocity_[i] > 0.0) { - speed_dps = 900; - } else if (joint_command_velocity_[i] < 0.0) { - speed_dps = -900; - } else { - speed_dps = 0; - } - - if (can_connected_) { - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = can_command_id; - can_tx_frame_.dlc = 3; // 3 bytes per spec - - // VELOCITY_CONTROL_CMD = 0x3 → (0x3 << 4) | port_id = 0x30 | port_id - uint8_t port_id = joint_node_ids[i] & 0x0F; - can_tx_frame_.data[0] = static_cast((VELOCITY_CONTROL_CMD & 0xF0) | port_id); - can_tx_frame_.data[1] = static_cast(speed_dps & 0xFF); // low byte - can_tx_frame_.data[2] = static_cast((speed_dps >> 8) & 0xFF); // high byte - - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Data[0]: 0x%02X | Data[1-2] (speed_dps): %d | Joint: %s", - can_tx_frame_.data[0], speed_dps, info_.joints[i].name.c_str()); - - canBus_.send(can_tx_frame_); - } + 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(); } - else if (control_level_[i] == integration_level_t::POSITION && - std::isfinite(joint_command_position_[i])) - { - // Convert rad -> deg, apply gear ratio, clamp to int16 range - int16_t position_deg = static_cast(std::clamp( - joint_command_position_[i] * (180.0 / M_PI) * joint_gear_ratios[i], - static_cast(std::numeric_limits::min()), - static_cast(std::numeric_limits::max()) - )); - - if (received_joint_data_.data[0] == static_cast(StatusCommands::MOTOR_STATUS_2_CMD)) { - joint.encoder_position = static_cast( - static_cast((received_joint_data_.data[7] << 8) | received_joint_data_.data[6])); - joint.motor_velocity = static_cast( - static_cast((received_joint_data_.data[5] << 8) | received_joint_data_.data[4])); - joint.motor_temperature = static_cast(received_joint_data_.data[1]); - joint.motor_torque_current = static_cast( - static_cast((received_joint_data_.data[3] << 8) | received_joint_data_.data[2])) * 0.01; - joint.motor_status = 1.0; - } else if ( - received_joint_data_.data[0] == static_cast(StatusCommands::READ_MULTI_TURN_ANGLE_CMD)) - { - joint.motor_position = static_cast( - static_cast( - (received_joint_data_.data[7] << 24) | - (received_joint_data_.data[6] << 16) | - (received_joint_data_.data[5] << 8) | - received_joint_data_.data[4])); - } - - joint.joint_state_position = calculate_joint_position_from_motor_position( - joint.motor_position, joint.gear_ratio); - joint.joint_state_velocity = calculate_joint_velocity_from_motor_velocity( - joint.motor_velocity, joint.gear_ratio); } - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type STEPPERHardwareInterface::write( - const rclcpp::Time &, const rclcpp::Duration & period) -{ - (void)period; - 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) { - msgs::msg::CANA frame; + CANLib::CanFrame frame; + frame.id = can_command_id; if (format_status_command(frame, static_cast(status_cmd), joint.node_id)) { - science_can_publisher_->publish(frame); + canBus.send(frame); } } } else if (curr_status_req > 0.0) { @@ -606,9 +576,10 @@ hardware_interface::return_type STEPPERHardwareInterface::write( if (joint.elapsed_status_request_time > (1.0 / curr_status_req)) { joint.elapsed_status_request_time = 0.0; for (auto status_cmd : kStatusCommands) { - msgs::msg::CANA frame; + CANLib::CanFrame frame; + frame.id = can_command_id; if (format_status_command(frame, static_cast(status_cmd), joint.node_id)) { - science_can_publisher_->publish(frame); + canBus.send(frame); } } } @@ -629,7 +600,8 @@ hardware_interface::return_type STEPPERHardwareInterface::write( static_cast(joint.maintenance_frame)); pack_decoded_maintenance_frame(joint, decoded_maintenance_cmd); - msgs::msg::CANA frame; + 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; @@ -637,21 +609,28 @@ hardware_interface::return_type STEPPERHardwareInterface::write( const double curr_maintenance_req = joint.motor_maintenance_req; if (curr_maintenance_req < 0.0 && joint.prev_maintenance_req >= 0.0) { - science_can_publisher_->publish(frame); + 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; - science_can_publisher_->publish(frame); + canBus.send(frame); } } joint.prev_maintenance_req = curr_maintenance_req; } - for (auto & joint : STEPPERJoints_) { - msgs::msg::CANA frame; - format_control_command(frame, joint); - science_can_publisher_->publish(frame); + 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); + } } return hardware_interface::return_type::OK; diff --git a/src/subsystems/general/README.md b/src/subsystems/general/README.md index 1307c14c..cafe6892 100644 --- a/src/subsystems/general/README.md +++ b/src/subsystems/general/README.md @@ -1,5 +1,27 @@ 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, 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 3de6fc40..784892dc 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -120,9 +120,89 @@ motor_status_controller: joints: - stepper_motor_a - stepper_motor_b +<<<<<<< HEAD - dc_lift - dc_scoop interfaces: - motor_temperature - torque_current +======= + - 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 +>>>>>>> 48331df (implemented and tested in virtual can servo, stepper, and dc HWIs) publish_rate: 10.0 From af7651710703df75181c6184b27a1fcbc7c43ffa Mon Sep 17 00:00:00 2001 From: Ishan Dutta Date: Sat, 9 May 2026 03:28:05 -0400 Subject: [PATCH 35/37] working arm and steppers/DC motors --- .../arm/arm.smc_2dof.ros2_control.xacro | 4 +- .../science/science.dc.ros2_control.xacro | 4 +- .../science.stepper.ros2_control.xacro | 2 + .../urdf/athena_science.urdf.xacro | 2 +- .../src/stepper_hardware_interface.cpp | 62 +------------------ .../config/athena_science_controllers.yaml | 8 --- 6 files changed, 8 insertions(+), 74 deletions(-) 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 4064c89e..42bba4b4 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 @@ -30,7 +30,7 @@ - 0x145 + 0x147 40 1.0 1000 @@ -49,7 +49,7 @@ - 0x147 + 0x145 40 1.0 1000 diff --git a/src/description/ros2_control/science/science.dc.ros2_control.xacro b/src/description/ros2_control/science/science.dc.ros2_control.xacro index c93a807b..49687248 100644 --- a/src/description/ros2_control/science/science.dc.ros2_control.xacro +++ b/src/description/ros2_control/science/science.dc.ros2_control.xacro @@ -7,7 +7,7 @@ dc_ros2_control/DCHardwareInterface - 5 + 20 5 0 can0 @@ -35,7 +35,7 @@ - 2 + 1 revolute 50.9 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 8d69e6d1..8d8fc00f 100644 --- a/src/description/ros2_control/science/science.stepper.ros2_control.xacro +++ b/src/description/ros2_control/science/science.stepper.ros2_control.xacro @@ -28,6 +28,7 @@ + @@ -44,6 +45,7 @@ + diff --git a/src/description/urdf/athena_science.urdf.xacro b/src/description/urdf/athena_science.urdf.xacro index cdb206cc..92b023a4 100644 --- a/src/description/urdf/athena_science.urdf.xacro +++ b/src/description/urdf/athena_science.urdf.xacro @@ -38,7 +38,7 @@ - + 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 0ede30d7..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,18 +1,3 @@ -// Copyright (c) 2024 UMD Loop -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// - #include "stepper_ros2_control/stepper_hardware_interface.hpp" #include @@ -471,11 +456,6 @@ hardware_interface::CallbackReturn STEPPERHardwareInterface::on_activate( joint.joint_command_position = joint.joint_state_position; joint.joint_command_velocity = 0.0; } - - RCLCPP_INFO( - rclcpp::get_logger("STEPPERHardwareInterface"), - "Stepper hardware activated"); - return hardware_interface::CallbackReturn::SUCCESS; } @@ -496,46 +476,6 @@ hardware_interface::CallbackReturn STEPPERHardwareInterface::on_deactivate( canBus.send(frame); } } - - RCLCPP_INFO( - rclcpp::get_logger("STEPPERHardwareInterface"), - "Stepper hardware deactivated%s", can_connected_ ? "" : " (simulated)"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -// can_connected_ and is_connected_ are always reset regardless of whether CAN was open. -hardware_interface::CallbackReturn STEPPERHardwareInterface::on_cleanup( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - RCLCPP_INFO( - rclcpp::get_logger("STEPPERHardwareInterface"), - "Cleaning up stepper hardware..."); - - if (can_connected_) { - 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; - - // FIX #4: Assign device_id_nibble (was previously using uninitialized variable) - uint8_t device_id_nibble = joint_node_ids[i] & 0x0F; - can_tx_frame_.data[0] = static_cast((MAINTENANCE_CMD << 4) | device_id_nibble); - can_tx_frame_.data[1] = 2; // Maintenance cmd 2 = Shutdown stepper - canBus_.send(can_tx_frame_); - } - - canBus_.close(); - } - - // Always reset connection state - can_connected_ = false; - is_connected_ = 0.0; - - RCLCPP_INFO( - rclcpp::get_logger("STEPPERHardwareInterface"), - "Stepper hardware cleanup complete"); - return hardware_interface::CallbackReturn::SUCCESS; } @@ -699,4 +639,4 @@ hardware_interface::return_type STEPPERHardwareInterface::perform_command_mode_s } // namespace stepper_ros2_control PLUGINLIB_EXPORT_CLASS( - stepper_ros2_control::STEPPERHardwareInterface, hardware_interface::SystemInterface) + stepper_ros2_control::STEPPERHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file diff --git a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml index 784892dc..a75ca5d5 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -120,13 +120,6 @@ motor_status_controller: joints: - stepper_motor_a - stepper_motor_b -<<<<<<< HEAD - - dc_lift - - dc_scoop - interfaces: - - motor_temperature - - torque_current -======= - servo_rack_and_pinion_l - servo_rack_and_pinion_r - servo_scoop_a @@ -204,5 +197,4 @@ motor_status_controller: science_limit_switch: interfaces: - status ->>>>>>> 48331df (implemented and tested in virtual can servo, stepper, and dc HWIs) publish_rate: 10.0 From d1b7a00255459272f76df442b314d6a0b25fa440 Mon Sep 17 00:00:00 2001 From: UMDLoop Jetson Orin NX Date: Sun, 10 May 2026 18:51:13 -0400 Subject: [PATCH 36/37] WORKING DRIVE AND ARM --- .../ros2_control/arm/arm.base_limit_switch.ros2_control.xacro | 2 +- .../arm/arm.end_effector_limit_switch.ros2_control.xacro | 2 +- src/description/ros2_control/arm/arm.rmd.ros2_control.xacro | 2 +- .../ros2_control/arm/arm.rotary_encoder.ros2_control.xacro | 2 +- src/description/ros2_control/arm/arm.servo.ros2_control.xacro | 2 +- .../ros2_control/arm/arm.smc_2dof.ros2_control.xacro | 2 +- .../ros2_control/arm/arm.smc_3dof.ros2_control.xacro | 2 +- .../ros2_control/arm/arm.talon_2dof.ros2_control.xacro | 2 +- .../ros2_control/arm/arm.talon_3dof.ros2_control.xacro | 2 +- 9 files changed, 9 insertions(+), 9 deletions(-) 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 index 29b0dc3a..29735835 100644 --- 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 @@ -4,7 +4,7 @@ limit_switch_ros2_control/LimitSwitchHardwareInterface - can0 + can1 0x130 10 5 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 index ff17d9ff..597e9576 100644 --- 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 @@ -4,7 +4,7 @@ limit_switch_ros2_control/LimitSwitchHardwareInterface - can0 + can1 0x60 10 5 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 c529abb0..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 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 index c5451c01..e24ece2c 100644 --- a/src/description/ros2_control/arm/arm.rotary_encoder.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.rotary_encoder.ros2_control.xacro @@ -4,7 +4,7 @@ rotary_encoder_ros2_control/RotaryEncoderHardwareInterface - can0 + can1 0x130 10 5 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 b83f19fb..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 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 42bba4b4..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 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 9bb720c0..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 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 b1b1bb7d..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 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 756a961b..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 From 0d359a2655d7468e9223e27d4fedca3b27e736a0 Mon Sep 17 00:00:00 2001 From: UMDLoop Jetson Orin NX Date: Sun, 10 May 2026 19:42:42 -0400 Subject: [PATCH 37/37] uhh quick random fixes lul --- .codex | 0 .vscode/settings.json | 69 ------------------- .../drive/drive_bringup/config/joystick.yaml | 5 +- 3 files changed, 3 insertions(+), 71 deletions(-) delete mode 100644 .codex delete mode 100644 .vscode/settings.json diff --git a/.codex b/.codex deleted file mode 100644 index e69de29b..00000000 diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 02e32c61..00000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,69 +0,0 @@ -{ - "files.associations": { - "cctype": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "array": "cpp", - "atomic": "cpp", - "bit": "cpp", - "bitset": "cpp", - "chrono": "cpp", - "compare": "cpp", - "complex": "cpp", - "concepts": "cpp", - "condition_variable": "cpp", - "cstdint": "cpp", - "deque": "cpp", - "forward_list": "cpp", - "list": "cpp", - "map": "cpp", - "set": "cpp", - "string": "cpp", - "unordered_map": "cpp", - "unordered_set": "cpp", - "vector": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "functional": "cpp", - "iterator": "cpp", - "memory": "cpp", - "memory_resource": "cpp", - "numeric": "cpp", - "optional": "cpp", - "random": "cpp", - "ratio": "cpp", - "string_view": "cpp", - "system_error": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "utility": "cpp", - "fstream": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "mutex": "cpp", - "new": "cpp", - "numbers": "cpp", - "ostream": "cpp", - "semaphore": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "stop_token": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "cinttypes": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp" - } -} \ No newline at end of file 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