diff --git a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml index 7f812fa52..a1209c72f 100644 --- a/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml @@ -2,8 +2,8 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - # - rmcs::AutoAimPlayerComponent -> auto_aim_player - - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player + - rmcs::AutoAimPlayerComponent -> auto_aim_player + # - rmcs::AutoAimVideoPlayerComponent -> auto_aim_video_player # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimComponent -> auto_aim_component @@ -29,10 +29,10 @@ auto_aim_recorder: auto_aim_component: ros__parameters: - dangerous_fallback: "red" + dangerous_fallback: "blue" manual_shoot: false enable_rune: true - track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST] + track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST, RUNE] camera_translation: [0., 0., 0.] fire_control: @@ -50,3 +50,4 @@ auto_aim_component: pitch_tolerance: 0.04 rune_idle_duration: 0.4 rune_shoot_duration: 0.2 + # is_lazy_gimbal: false diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml deleted file mode 100644 index aa952607c..000000000 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ /dev/null @@ -1,348 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::DeformableInfantryOmniB -> deformable_infantry - - - rmcs_core::referee::Status -> referee_status - - rmcs_core::referee::Command -> referee_command - - - rmcs_core::referee::command::Interaction -> referee_interaction - - rmcs_core::referee::command::interaction::Ui -> referee_ui - - rmcs_core::referee::app::ui::DeformableInfantry -> referee_ui_infantry - - - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_controller - - - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - - rmcs_core::controller::shooting::HeatController -> heat_controller - - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller - - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - - - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller - - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension - - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller - - - rmcs_core::controller::chassis::DeformableJointController -> lf_joint_controller - - rmcs_core::controller::chassis::DeformableJointController -> lb_joint_controller - - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller - - - rmcs::AutoAimComponent -> auto_aim_component - - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui - - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - # - rmcs_core::debug::ValueCollector -> value_collector - -value_collector: - ros__parameters: - csv_path: "/tmp/pitch_.csv" - signals: - - /gimbal/pitch/angle - - /gimbal/pitch/velocity - - /gimbal/pitch/velocity_imu - - /gimbal/pitch/angle_error - - /gimbal/pitch/control_torque - - /gimbal/pitch/control_velocity - write_interval: 5 - flush_interval: 1000 - -value_broadcaster: - ros__parameters: - forward_list: - - /gimbal/pitch/angle - - /gimbal/pitch/velocity - -auto_aim_capturer: - ros__parameters: - camera_name: "" - exposure_us: 4000.0 - gain: 8.0 - framerate: 120.0 - invert_image: false - rls_tau_sec: 10.0 - use_hardware_sync: false - delay_ms: 6.5 - -auto_aim_component: - ros__parameters: - # WARN: 危险!可选 red | blue,生效后裁判系统 ID 缺席时 - # 将强行绑定为对应阵营哨兵,仅供调试,严禁比赛启用。 - # 留空或填 unknow 表示禁用。 - dangerous_fallback: "" - manual_shoot: true - enable_rune: true - camera_translation: [0.058, -0.08, 0.0] - fire_control: - bullet_speed: 22.5 - shoot_delay: 0.04 - offset_yaw: +2.5 - offset_pitch: +0.5 - attack_window: 80.0 - degraded_angle_speed: 12.0 - window_redundancy: 0.8 - window_hysteresis: 0.2 - attack_preaim: false - require_stable_command: false - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 - rune_idle_duration: 0.4 - rune_shoot_duration: 0.2 - -auto_aim_ui: - ros__parameters: - offset_x: 0.0 - offset_y: +0.08 - offset_z: 0.0 - -deformable_infantry: - ros__parameters: - serial_filter_bottom_board: "AF-73B2-E8A1-A544-79ED-5BDA-D088-7F21-A6A6" - serial_filter_top_board: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" - chassis_radius: 0.2341741 - rod_length: 0.140 - yaw_motor_zero_point: 57900 - pitch_motor_zero_point: 56354 - debug_log_supercap: false - debug_log_wheel_motor: false - debug_log_deformable_joint_motor: false - -chassis_controller: - ros__parameters: - # Deploy geometry / chassis-owned joint intent - min_angle: 5.0 - max_angle: 59.0 - active_suspension_enable: true - spin_ratio: 1.0 - -deformable_suspension: - ros__parameters: - # IMU attitude correction at min-angle stance. - active_suspension_pitch_outer_kp: 12.0 - active_suspension_pitch_outer_ki: 0.02 - active_suspension_pitch_outer_kd: 0.0 - active_suspension_pitch_outer_integral_min: -2.0 - active_suspension_pitch_outer_integral_max: 2.0 - active_suspension_pitch_outer_output_min: -3.0 - active_suspension_pitch_outer_output_max: 3.0 - - active_suspension_pitch_inner_kp: 0.45 - active_suspension_pitch_inner_ki: 0.0 - active_suspension_pitch_inner_kd: 0.0 - active_suspension_pitch_inner_integral_min: -1.0 - active_suspension_pitch_inner_integral_max: 1.0 - active_suspension_pitch_inner_output_min: -0.785 - active_suspension_pitch_inner_output_max: 0.785 - - active_suspension_roll_outer_kp: 12.0 - active_suspension_roll_outer_ki: 0.02 - active_suspension_roll_outer_kd: 0.0 - active_suspension_roll_outer_integral_min: -2.0 - active_suspension_roll_outer_integral_max: 2.0 - active_suspension_roll_outer_output_min: -3.0 - active_suspension_roll_outer_output_max: 3.0 - - active_suspension_roll_inner_kp: 0.45 - active_suspension_roll_inner_ki: 0.0 - active_suspension_roll_inner_kd: 0.0 - active_suspension_roll_inner_integral_min: -1.0 - active_suspension_roll_inner_integral_max: 1.0 - active_suspension_roll_inner_output_min: -0.785 - active_suspension_roll_inner_output_max: 0.785 - - # Chassis-owned joint intent trajectory limits while attitude correction is active. - active_suspension_target_velocity_limit_deg: 80.0 - active_suspension_target_acceleration_limit_deg: 360.0 - active_suspension_correction_velocity_limit_deg: 720.0 - active_suspension_correction_acceleration_limit_deg: 3600.0 - active_suspension_rate_lpf_cutoff_hz: 10.0 - - # Automatic IMU mounting-error calibration. - # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. - chassis_imu_calibration_wait_s: 2.0 - chassis_imu_calibration_sample_s: 3.0 - -gimbal_controller: - ros__parameters: - upper_limit: -0.47123 # -27 deg - lower_limit: 0.10 # 8 deg - ctrl_hold_pitch_target_angle: 0.0 - - yaw_angle_kp: 15.0 - yaw_angle_ki: 0.0 - yaw_angle_kd: 0.0 - - yaw_velocity_kp: 15.0 - yaw_velocity_ki: 0.0 - yaw_velocity_kd: 0.0 - - pitch_angle_kp: 35.0 - pitch_angle_ki: 0.02 - pitch_angle_kd: 0.3 - - pitch_velocity_kp: 2.0 - pitch_velocity_ki: 0.0 - pitch_velocity_kd: 0.0 - - pitch_gravity_ff_gain: 4.302 - pitch_gravity_ff_phase: 0.589 - - pitch_torque_control: true - -friction_wheel_controller: - ros__parameters: - friction_wheels: - - /gimbal/left_friction - - /gimbal/right_friction - friction_velocities: - - 580.0 - - 580.0 - friction_soft_start_stop_time: 1.0 - -heat_controller: - ros__parameters: - heat_per_shot: 10000 - reserved_heat: 15000 - -bullet_feeder_controller: - ros__parameters: - bullets_per_feeder_turn: 8.0 - shot_frequency: 30.0 - safe_shot_frequency: 10.0 - eject_frequency: 10.0 - eject_time: 0.05 - deep_eject_frequency: 5.0 - deep_eject_time: 0.2 - single_shot_max_stop_delay: 2.0 - -left_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/left_friction/velocity - setpoint: /gimbal/left_friction/control_velocity - control: /gimbal/left_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -right_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/right_friction/velocity - setpoint: /gimbal/right_friction/control_velocity - control: /gimbal/right_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -bullet_feeder_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/bullet_feeder/velocity - setpoint: /gimbal/bullet_feeder/control_velocity - control: /gimbal/bullet_feeder/control_torque - kp: 1.5 - ki: 0.0 - kd: 0.0 - -deformable_chassis_controller: - ros__parameters: - mass: 25.5 - moment_of_inertia: 1.0 - wheel_radius: 0.075 - friction_coefficient: 6.6 - k1: 2.958580e+00 - k2: 3.082190e-03 - no_load_power: 11.37 - -lf_joint_controller: - ros__parameters: - measurement_angle: /chassis/left_front_joint/physical_angle - setpoint_angle: /chassis/left_front_joint/target_physical_angle - setpoint_velocity: /chassis/left_front_joint/target_physical_velocity - control: /chassis/left_front_joint/control_torque - dt: 0.001 - b0: -1.0 - kt: 1.0 - td_h: 0.001 - td_r: 50.0 - eso_w0: 250.0 - eso_auto_beta: true - k1: 30.0 - k2: 17.0 - alpha1: 0.75 - alpha2: 0.7 - delta: 0.02 - u_min: -200.0 - u_max: 200.0 - output_min: -200.0 - output_max: 200.0 - -lb_joint_controller: - ros__parameters: - measurement_angle: /chassis/left_back_joint/physical_angle - setpoint_angle: /chassis/left_back_joint/target_physical_angle - setpoint_velocity: /chassis/left_back_joint/target_physical_velocity - control: /chassis/left_back_joint/control_torque - dt: 0.001 - b0: -1.0 - kt: 1.0 - td_h: 0.001 - td_r: 50.0 - eso_w0: 250.0 - eso_auto_beta: true - k1: 30.0 - k2: 17.0 - alpha1: 0.75 - alpha2: 0.7 - delta: 0.02 - u_min: -200.0 - u_max: 200.0 - output_min: -200.0 - output_max: 200.0 - -rb_joint_controller: - ros__parameters: - measurement_angle: /chassis/right_back_joint/physical_angle - setpoint_angle: /chassis/right_back_joint/target_physical_angle - setpoint_velocity: /chassis/right_back_joint/target_physical_velocity - control: /chassis/right_back_joint/control_torque - dt: 0.001 - b0: -1.0 - kt: 1.0 - td_h: 0.001 - td_r: 50.0 - eso_w0: 250.0 - eso_auto_beta: true - k1: 30.0 - k2: 17.0 - alpha1: 0.75 - alpha2: 0.7 - delta: 0.02 - u_min: -200.0 - u_max: 200.0 - output_min: -200.0 - output_max: 200.0 - -rf_joint_controller: - ros__parameters: - measurement_angle: /chassis/right_front_joint/physical_angle - setpoint_angle: /chassis/right_front_joint/target_physical_angle - setpoint_velocity: /chassis/right_front_joint/target_physical_velocity - control: /chassis/right_front_joint/control_torque - dt: 0.001 - b0: -1.0 - kt: 1.0 - td_h: 0.001 - td_r: 50.0 - eso_w0: 250.0 - eso_auto_beta: true - k1: 30.0 - k2: 17.0 - alpha1: 0.75 - alpha2: 0.7 - delta: 0.02 - u_min: -200.0 - u_max: 200.0 - output_min: -200.0 - output_max: 200.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 272d4f091..5724db107 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -30,6 +30,7 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller + - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimComponent -> auto_aim_component - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui @@ -56,15 +57,24 @@ value_broadcaster: - /gimbal/pitch/angle - /gimbal/pitch/velocity +auto_aim_recorder: + ros__parameters: + output_path: "/autoaim/recoder" + queue_depth: 16 + flush_every_n_frames: 64 + max_duration_seconds: 0 + max_videos_size_gb: 200.0 + auto_record: false + auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 4000.0 - gain: 8.0 + exposure_us: 2000.0 + gain: 10.0 framerate: 120.0 invert_image: false rls_tau_sec: 10.0 - use_hardware_sync: true + use_hardware_sync: false delay_ms: 6.5 auto_aim_component: @@ -73,23 +83,23 @@ auto_aim_component: # 将强行绑定为对应阵营哨兵,仅供调试,严禁比赛启用。 # 留空或填 unknow 表示禁用。 dangerous_fallback: "" - manual_shoot: true + manual_shoot: false enable_rune: true camera_translation: [0.058, -0.08, 0.0] fire_control: bullet_speed: 22.5 - shoot_delay: 0.04 - offset_yaw: +0.1 - offset_pitch: -0.4 - attack_window: 80.0 + shoot_delay: 0.07 + offset_yaw: +0.8 + offset_pitch: -1.4 + attack_window: 120.0 degraded_angle_speed: 12.0 - window_redundancy: 0.8 + window_redundancy: 0.6 window_hysteresis: 0.2 attack_preaim: false require_stable_command: false - yaw_tolerance: 0.07 - pitch_tolerance: 0.04 - rune_idle_duration: 0.4 + yaw_tolerance: 0.21 + pitch_tolerance: 0.12 + rune_idle_duration: 0.6 rune_shoot_duration: 0.2 auto_aim_ui: @@ -100,32 +110,26 @@ auto_aim_ui: deformable_infantry: ros__parameters: - serial_filter_bottom_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" - serial_filter_top_board: "AF-7A42-07AA-D181-0356-7715-6D7C-4C65-5762" + serial_filter_bottom_board: "AF-C1C3-DFE8-40A8-FC4B-B853-6ED7-AC9F-1DED" + serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" chassis_radius: 0.2341741 rod_length: 0.140 - yaw_motor_zero_point: 43365 - pitch_motor_zero_point: 6432 - debug_log_supercap: false - debug_log_wheel_motor: false - debug_log_deformable_joint_motor: false + yaw_motor_zero_point: 38910 + pitch_motor_zero_point: 6783 chassis_controller: ros__parameters: - # Deploy geometry / chassis-owned joint intent - min_angle: 5.0 + min_angle: 8.0 max_angle: 59.0 active_suspension_enable: true - spin_ratio: 1.0 deformable_suspension: ros__parameters: - # IMU attitude correction at min-angle stance. active_suspension_pitch_outer_kp: 12.0 active_suspension_pitch_outer_ki: 0.02 active_suspension_pitch_outer_kd: 0.0 - active_suspension_pitch_outer_integral_min: -2.0 - active_suspension_pitch_outer_integral_max: 2.0 + active_suspension_pitch_outer_integral_min: -1.0 + active_suspension_pitch_outer_integral_max: 1.0 active_suspension_pitch_outer_output_min: -3.0 active_suspension_pitch_outer_output_max: 3.0 @@ -140,8 +144,8 @@ deformable_suspension: active_suspension_roll_outer_kp: 12.0 active_suspension_roll_outer_ki: 0.02 active_suspension_roll_outer_kd: 0.0 - active_suspension_roll_outer_integral_min: -2.0 - active_suspension_roll_outer_integral_max: 2.0 + active_suspension_roll_outer_integral_min: -1.0 + active_suspension_roll_outer_integral_max: 1.0 active_suspension_roll_outer_output_min: -3.0 active_suspension_roll_outer_output_max: 3.0 @@ -153,21 +157,20 @@ deformable_suspension: active_suspension_roll_inner_output_min: -0.785 active_suspension_roll_inner_output_max: 0.785 - # Chassis-owned joint intent trajectory limits while attitude correction is active. - active_suspension_target_velocity_limit_deg: 80.0 - active_suspension_target_acceleration_limit_deg: 360.0 + active_suspension_target_velocity_limit_deg: 150.0 + active_suspension_target_acceleration_limit_deg: 600.0 active_suspension_correction_velocity_limit_deg: 720.0 active_suspension_correction_acceleration_limit_deg: 3600.0 active_suspension_rate_lpf_cutoff_hz: 10.0 - # Automatic IMU mounting-error calibration. - # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. + active_suspension_target_pitch_deg: 0.0 + chassis_imu_calibration_wait_s: 2.0 chassis_imu_calibration_sample_s: 3.0 gimbal_controller: ros__parameters: - upper_limit: -0.47123 # -27 deg + upper_limit: -0.60 # -27 deg lower_limit: 0.10 # 8 deg ctrl_hold_pitch_target_angle: 0.0 @@ -175,20 +178,32 @@ gimbal_controller: yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 10.0 - yaw_velocity_ki: 0.0 + yaw_velocity_kp: 13.0 + yaw_velocity_ki: 0.02 yaw_velocity_kd: 0.0 + yaw_velocity_integral_min: -5.0 + yaw_velocity_integral_max: 5.0 pitch_angle_kp: 35.0 pitch_angle_ki: 0.02 pitch_angle_kd: 0.3 + pitch_angle_integral_min: -0.5 + pitch_angle_integral_max: 0.5 pitch_velocity_kp: 2.0 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - pitch_gravity_ff_gain: 4.302 - pitch_gravity_ff_phase: 0.589 + yaw_ref_velocity_gain: 1.0 + pitch_ref_velocity_gain: 1.0 + + yaw_velocity_ff_gain: 0.13 + yaw_acceleration_ff_gain: 0.18 + pitch_velocity_ff_gain: 0.378 + pitch_acceleration_ff_gain: 0.0396 + + pitch_gravity_ff_gain: 2.575 + pitch_gravity_ff_phase: 1.784 pitch_torque_control: true @@ -200,6 +215,9 @@ friction_wheel_controller: friction_velocities: - 580.0 - 580.0 + friction_velocities_low_mode: + - 530.0 + - 530.0 friction_soft_start_stop_time: 1.0 heat_controller: @@ -210,7 +228,7 @@ heat_controller: bullet_feeder_controller: ros__parameters: bullets_per_feeder_turn: 8.0 - shot_frequency: 30.0 + shot_frequency: 15.0 safe_shot_frequency: 10.0 eject_frequency: 10.0 eject_time: 0.05 @@ -241,7 +259,7 @@ bullet_feeder_velocity_pid_controller: measurement: /gimbal/bullet_feeder/velocity setpoint: /gimbal/bullet_feeder/control_velocity control: /gimbal/bullet_feeder/control_torque - kp: 1.5 + kp: 0.9 ki: 0.0 kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index c4c0bd3aa..3f8d323e9 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -62,7 +62,7 @@ auto_aim_component: manual_shoot: true enable_rune: true # track_ids: [OUTPOST, BASE] - track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST, BASE] + track_ids: [HERO, ENGINEER, INFANTRY_3, INFANTRY_4, SENTRY, OUTPOST, BASE, RUNE] camera_translation: [0.07128, 0.0, 0.0481] fire_control: bullet_speed: 22.5 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index a066759ae..131819c78 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -3,7 +3,6 @@ - diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 397400cfe..ae167f385 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -30,7 +30,6 @@ class DeformableChassis get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , following_velocity_controller_(10.0, 0.0, 0.0) - , spin_ratio_(std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0)) , joint_mode_mgr_(*this) { following_velocity_controller_.output_max = angular_velocity_max_; @@ -46,6 +45,10 @@ class DeformableChassis register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + register_input("/auto_aim/single_shoot", auto_aim_single_shoot_, false); + register_input("/auto_aim/robot_center", auto_aim_robot_center_, false); + register_input("/tf", tf_, false); + register_output("/chassis/angle", chassis_angle_, nan_); register_output("/chassis/control_angle", chassis_control_angle_, nan_); register_output("/chassis/control_mode", mode_); @@ -109,7 +112,8 @@ class DeformableChassis double rotary_knob = rotary_knob_.ready() ? *rotary_knob_ : 0.0; - joint_mode_mgr_.update(switch_left, switch_right, keyboard, rotary_knob, update_dt()); + joint_mode_mgr_.update( + switch_left, switch_right, keyboard, rotary_knob, update_dt(), *gimbal_yaw_angle_); *mode_ = joint_mode_mgr_.mode(); *pitch_lock_active_ = joint_mode_mgr_.pitch_lock_active(); @@ -122,6 +126,10 @@ class DeformableChassis *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); publish_joint_posture_targets_(); + update_auto_aim_override_state_(); + if (auto_aim_posture_override_) + apply_auto_aim_posture_override_(); + update_velocity_control(); } while (false); } @@ -153,6 +161,36 @@ class DeformableChassis *chassis_control_angle_ = nan_; } + void update_auto_aim_override_state_() { + auto_aim_posture_override_ = auto_aim_single_shoot_.ready() && *auto_aim_single_shoot_; + auto_aim_vector_follow_ = // + auto_aim_posture_override_ && tf_.ready() && auto_aim_robot_center_.ready() + && auto_aim_robot_center_->allFinite() && !auto_aim_robot_center_->isZero(); + } + + void apply_auto_aim_posture_override_() { + const double front_rad = deg_to_rad(joint_mode_mgr_.max_angle()); + const double back_rad = deg_to_rad(joint_mode_mgr_.min_angle()); + *joint_posture_target_angle_rad_[kLeftFront] = front_rad; + *joint_posture_target_angle_rad_[kRightFront] = front_rad; + *joint_posture_target_angle_rad_[kLeftBack] = back_rad; + *joint_posture_target_angle_rad_[kRightBack] = back_rad; + + *symmetric_posture_target_ = false; + *low_prone_active_ = false; + *active_suspension_active_ = false; + + const double min_deg = joint_mode_mgr_.min_angle(); + const double max_deg = joint_mode_mgr_.max_angle(); + const double reference_deg = (min_deg + max_deg) / 2.0; + *suspension_reference_angle_deg_ = reference_deg; + // Always true: reference_deg == (min_deg + max_deg) / 2.0 + // > (min_deg - 5.0 + max_deg) / 2.0 == reference_deg - 2.5. + // Under auto-aim low-prone override the correction direction is + // intentionally always inverted. + *correction_inverted_ = reference_deg > (min_deg - 5.0 + max_deg) / 2.0; + } + double update_dt() const { if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) return 1.0 / *update_rate_; @@ -183,33 +221,36 @@ class DeformableChassis double angular_velocity = 0.0; double chassis_control_angle = nan_; - switch (*mode_) { - case rmcs_msgs::ChassisMode::AUTO: break; - - case rmcs_msgs::ChassisMode::SPIN_FAST: { - bool forward = joint_mode_mgr_.spinning_forward(); - angular_velocity = - spin_ratio_ * (forward ? angular_velocity_max_ : -angular_velocity_max_); - angular_velocity = - std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); - } break; - - case rmcs_msgs::ChassisMode::STEP_DOWN: { - double chassis_angle_error = - calculate_unsigned_chassis_angle_error(chassis_control_angle); - - constexpr double alignment = std::numbers::pi; - while (chassis_angle_error > alignment / 2) { - chassis_control_angle -= alignment; - if (chassis_control_angle < 0) - chassis_control_angle += 2 * std::numbers::pi; - chassis_angle_error -= alignment; + if (auto_aim_posture_override_) { + angular_velocity = update_auto_aim_override_angular_velocity_(chassis_control_angle); + } else { + switch (*mode_) { + case rmcs_msgs::ChassisMode::AUTO: break; + + case rmcs_msgs::ChassisMode::SPIN_FAST: { + bool forward = joint_mode_mgr_.spinning_forward(); + angular_velocity = forward ? angular_velocity_max_ : -angular_velocity_max_; + angular_velocity = + std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); + } break; + + case rmcs_msgs::ChassisMode::STEP_DOWN: { + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); + + constexpr double alignment = std::numbers::pi; + while (chassis_angle_error > alignment / 2) { + chassis_control_angle -= alignment; + if (chassis_control_angle < 0) + chassis_control_angle += 2 * std::numbers::pi; + chassis_angle_error -= alignment; + } + + angular_velocity = following_velocity_controller_.update(chassis_angle_error); + } break; + + default: break; } - - angular_velocity = following_velocity_controller_.update(chassis_angle_error); - } break; - - default: break; } *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; @@ -218,6 +259,26 @@ class DeformableChassis return angular_velocity; } + double update_auto_aim_override_angular_velocity_(double& chassis_control_angle) { + double chassis_angle_error; + if (auto_aim_vector_follow_) { + const auto target_in_base = fast_tf::cast( + rmcs_description::OdomImu::Position{*auto_aim_robot_center_}, *tf_); + chassis_angle_error = target_in_base->head<2>().norm() > 1e-6 + ? std::atan2(target_in_base->y(), target_in_base->x()) + : 0.0; + chassis_control_angle = normalize_positive_angle( + 2 * std::numbers::pi - *gimbal_yaw_angle_ + chassis_angle_error); + } else { + chassis_angle_error = normalize_signed_angle( + calculate_unsigned_chassis_angle_error(chassis_control_angle)); + } + + return std::clamp( + following_velocity_controller_.update(chassis_angle_error), -angular_velocity_max_, + angular_velocity_max_); + } + double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { chassis_control_angle = *gimbal_yaw_angle_error_; if (chassis_control_angle < 0) @@ -232,6 +293,22 @@ class DeformableChassis static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } + static double normalize_positive_angle(double angle) { + constexpr double full_turn = 2 * std::numbers::pi; + while (angle >= full_turn) + angle -= full_turn; + while (angle < 0.0) + angle += full_turn; + return angle; + } + + static double normalize_signed_angle(double angle) { + angle = normalize_positive_angle(angle); + if (angle > std::numbers::pi) + angle -= 2 * std::numbers::pi; + return angle; + } + void publish_joint_posture_targets_() { std::array targets_deg{}; joint_mode_mgr_.copy_joint_posture_target_deg(targets_deg); @@ -246,6 +323,10 @@ class DeformableChassis "right_back", "right_front", }; + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; InputInterface joystick_right_; InputInterface switch_right_; @@ -257,6 +338,12 @@ class DeformableChassis InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; + InputInterface auto_aim_single_shoot_; + InputInterface auto_aim_robot_center_; + InputInterface tf_; + bool auto_aim_posture_override_ = false; + bool auto_aim_vector_follow_ = false; + OutputInterface mode_; OutputInterface chassis_control_velocity_; OutputInterface pitch_lock_active_; @@ -271,7 +358,6 @@ class DeformableChassis std::array, kJointCount> joint_posture_target_angle_rad_; pid::PidCalculator following_velocity_controller_; - const double spin_ratio_; DeformableChassisModeManager joint_mode_mgr_; }; @@ -280,4 +366,5 @@ class DeformableChassis #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::DeformableChassis, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::DeformableChassis, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index dac11d423..3dd39d7bc 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -64,17 +64,19 @@ class DeformableChassisModeManager { apply_symmetric_target_ = true; suspension_enabled_by_toggle_ = false; low_prone_enabled_by_toggle_ = false; + step_down_combo_active_ = false; last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; last_keyboard_ = rmcs_msgs::Keyboard::zero(); last_rotary_knob_ = 0.0; + suspension_was_active_ = false; update_joint_posture_state_(false); } void update( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, - const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt) { + const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt, double gimbal_yaw_rad) { update_mode_from_inputs_(switch_left, switch_right, keyboard); update_low_prone_toggle_from_inputs_(switch_left, switch_right); @@ -82,10 +84,13 @@ class DeformableChassisModeManager { joint_posture_state_.ctrl_low_prone_active = keyboard.ctrl; joint_posture_state_.low_prone_active = joint_posture_state_.ctrl_low_prone_active || low_prone_enabled_by_toggle_; - joint_posture_state_.pitch_lock_active = joint_posture_state_.ctrl_low_prone_active; update_suspension_mode_from_inputs_(switch_left, switch_right, keyboard, rotary_knob); - update_posture_target_from_inputs_(switch_left, switch_right, keyboard, rotary_knob, dt); + update_posture_target_from_inputs_( + switch_left, switch_right, keyboard, rotary_knob, dt, gimbal_yaw_rad); + update_step_down_combo_from_inputs_(keyboard); + joint_posture_state_.pitch_lock_active = + joint_posture_state_.ctrl_low_prone_active || step_down_combo_active_; update_joint_posture_state_(joint_posture_state_.low_prone_active); last_switch_right_ = switch_right; @@ -105,13 +110,8 @@ class DeformableChassisModeManager { out = joint_posture_state_.joint_posture_target_deg; } - const JointPostureState& joint_posture_state() const { return joint_posture_state_; } - double min_angle() const { return min_angle_; } double max_angle() const { return max_angle_; } - double max_angle_rad() const { return deg_to_rad_(max_angle_); } - - double active_suspension_min_angle_rad() const { return deg_to_rad_(min_angle_ - 5.0); } bool correction_inverted() const { double midpoint = (min_angle_ - 5.0 + max_angle_) / 2.0; @@ -125,7 +125,12 @@ class DeformableChassisModeManager { static constexpr size_t kRightFront = 3; static constexpr size_t kJointCount = 4; - static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } + static bool gimbal_faces_physical_front_(double gimbal_yaw_rad) { + if (!std::isfinite(gimbal_yaw_rad)) + return true; + const double signed_yaw = std::remainder(gimbal_yaw_rad, 2 * std::numbers::pi); + return std::abs(signed_yaw) <= std::numbers::pi / 2; + } static bool symmetric_joint_target_requested_(const std::array& joint_target_deg) { @@ -140,10 +145,8 @@ class DeformableChassisModeManager { const rmcs_msgs::Keyboard& keyboard) { auto next_mode = joint_posture_state_.mode; - if (switch_left == rmcs_msgs::Switch::DOWN) { - joint_posture_state_.mode = next_mode; + if (switch_left == rmcs_msgs::Switch::DOWN) return; - } if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::DOWN) { @@ -200,13 +203,11 @@ class DeformableChassisModeManager { const bool remote_active_toggle_requested = remote_suspension_rotary_mode && rotary_knob_down_edge_(rotary_knob); - const bool keyboard_active_suspension_toggle_requested = !last_keyboard_.e && keyboard.e; + const bool keyboard_active_suspension_toggle_requested = !last_keyboard_.g && keyboard.g; if (keyboard_active_suspension_toggle_requested || remote_active_toggle_requested) suspension_enabled_by_toggle_ = !suspension_enabled_by_toggle_; - const bool active_requested = - suspension_enable_ - && (joint_posture_state_.low_prone_active || suspension_enabled_by_toggle_); + const bool active_requested = suspension_enable_ && suspension_enabled_by_toggle_; joint_posture_state_.suspension_mode = SuspensionMode::OFF; if (active_requested) @@ -214,6 +215,13 @@ class DeformableChassisModeManager { joint_posture_state_.suspension_active = joint_posture_state_.suspension_mode == SuspensionMode::ACTIVE; + + if (joint_posture_state_.suspension_active && !suspension_was_active_) { + active_suspension_base_angle_ = current_target_angle_; + } else if (!joint_posture_state_.suspension_active && suspension_was_active_) { + current_target_angle_ = active_suspension_base_angle_; + } + suspension_was_active_ = joint_posture_state_.suspension_active; } void update_low_prone_toggle_from_inputs_( @@ -226,7 +234,8 @@ class DeformableChassisModeManager { void update_posture_target_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, - const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double /*dt*/) { + const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double /*dt*/, + double gimbal_yaw_rad) { const bool remote_joint_posture_rotary_mode = switch_left == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::MIDDLE; @@ -236,14 +245,12 @@ class DeformableChassisModeManager { const bool remote_front_back_posture_toggle_condition = remote_joint_posture_rotary_mode && rotary_knob_up_edge_(rotary_knob); const bool front_high_rear_low = !last_keyboard_.b && keyboard.b; - const bool front_low_rear_high = !last_keyboard_.g && keyboard.g; + const bool posture_toggle_requested = + remote_posture_toggle_condition || keyboard_posture_toggle_condition; if (apply_symmetric_target_) joint_current_target_angle_.fill(current_target_angle_); - const bool posture_toggle_requested = - remote_posture_toggle_condition || keyboard_posture_toggle_condition; - if (posture_toggle_requested) { if (joint_posture_state_.suspension_active) { active_suspension_base_angle_ = @@ -261,14 +268,70 @@ class DeformableChassisModeManager { } else if (remote_front_back_posture_toggle_condition) { toggle_front_back_posture_target_(); } else if (front_high_rear_low) { - apply_front_high_rear_low_target_(); - } else if (front_low_rear_high) { - apply_front_low_rear_high_target_(); + if (gimbal_faces_physical_front_(gimbal_yaw_rad)) + apply_front_high_rear_low_target_(); + else + apply_front_low_rear_high_target_(); } last_rotary_knob_ = rotary_knob; } + void update_step_down_combo_from_inputs_(const rmcs_msgs::Keyboard& keyboard) { + const bool e_held = keyboard.e; + + if (e_held && !step_down_combo_active_) { + saved_combo_mode_ = joint_posture_state_.mode; + saved_combo_suspension_enabled_ = suspension_enabled_by_toggle_; + saved_combo_current_target_angle_ = current_target_angle_; + saved_combo_active_suspension_base_angle_ = active_suspension_base_angle_; + saved_combo_joint_target_ = joint_current_target_angle_; + saved_combo_apply_symmetric_ = apply_symmetric_target_; + + step_down_combo_active_ = true; + joint_posture_state_.mode = rmcs_msgs::ChassisMode::STEP_DOWN; + joint_posture_state_.suspension_mode = SuspensionMode::ACTIVE; + joint_posture_state_.suspension_active = true; + suspension_enabled_by_toggle_ = true; + suspension_was_active_ = true; + current_target_angle_ = min_angle_ - 5.0; + active_suspension_base_angle_ = min_angle_ - 5.0; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(min_angle_ - 5.0); + update_joint_posture_state_(joint_posture_state_.low_prone_active); + return; + } + + if (!e_held && step_down_combo_active_) { + joint_posture_state_.mode = saved_combo_mode_; + suspension_enabled_by_toggle_ = saved_combo_suspension_enabled_; + suspension_was_active_ = saved_combo_suspension_enabled_ && suspension_enable_; + current_target_angle_ = saved_combo_current_target_angle_; + active_suspension_base_angle_ = saved_combo_active_suspension_base_angle_; + joint_current_target_angle_ = saved_combo_joint_target_; + apply_symmetric_target_ = saved_combo_apply_symmetric_; + joint_posture_state_.suspension_active = + suspension_enable_ && suspension_enabled_by_toggle_; + joint_posture_state_.suspension_mode = + joint_posture_state_.suspension_active ? SuspensionMode::ACTIVE + : SuspensionMode::OFF; + step_down_combo_active_ = false; + update_joint_posture_state_(joint_posture_state_.low_prone_active); + return; + } + + if (step_down_combo_active_) { + joint_posture_state_.mode = rmcs_msgs::ChassisMode::STEP_DOWN; + joint_posture_state_.suspension_mode = SuspensionMode::ACTIVE; + joint_posture_state_.suspension_active = true; + current_target_angle_ = min_angle_ - 5.0; + active_suspension_base_angle_ = min_angle_ - 5.0; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(min_angle_ - 5.0); + update_joint_posture_state_(joint_posture_state_.low_prone_active); + } + } + bool rotary_knob_down_edge_(double rotary_knob) const { constexpr double rotary_knob_edge_threshold = 0.7; return last_rotary_knob_ < rotary_knob_edge_threshold @@ -322,6 +385,15 @@ class DeformableChassisModeManager { bool apply_symmetric_target_ = true; bool suspension_enabled_by_toggle_ = false; bool low_prone_enabled_by_toggle_ = false; + bool suspension_was_active_ = false; + + bool step_down_combo_active_ = false; + rmcs_msgs::ChassisMode saved_combo_mode_ = rmcs_msgs::ChassisMode::AUTO; + bool saved_combo_suspension_enabled_ = false; + double saved_combo_current_target_angle_ = 0.0; + double saved_combo_active_suspension_base_angle_ = 0.0; + std::array saved_combo_joint_target_{}; + bool saved_combo_apply_symmetric_ = true; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp index 6da008098..ee31f2205 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp @@ -211,6 +211,8 @@ class DeformableSuspension 1e-6); active_rate_lpf_cutoff_hz_ = std::max(get_parameter_or("active_suspension_rate_lpf_cutoff_hz", 10.0), 1e-6); + active_target_pitch_rad_ = + deg_to_rad_(get_parameter_or("active_suspension_target_pitch_deg", 0.0)); calibration_wait_time_ = std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); @@ -451,7 +453,8 @@ class DeformableSuspension const double clamped_pitch = std::clamp(pitch, -max_attitude, max_attitude); const double clamped_roll = std::clamp(roll, -max_attitude, max_attitude); - const double pitch_outer = pitch_outer_pid_.update(-clamped_pitch); + const double pitch_outer = + pitch_outer_pid_.update(active_target_pitch_rad_ - clamped_pitch); const double roll_outer = roll_outer_pid_.update(clamped_roll); const double pitch_diff = pitch_inner_pid_.update(pitch_outer - pitch_rate); const double roll_diff = roll_inner_pid_.update(roll_outer + roll_rate); @@ -589,6 +592,7 @@ class DeformableSuspension double active_correction_vel_limit_ = 40.0; double active_correction_acc_limit_ = 200.0; double active_rate_lpf_cutoff_hz_ = 10.0; + double active_target_pitch_rad_ = 0.0; double active_rate_filter_sampling_hz_ = 0.0; double calibration_wait_time_ = 2.0; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 9396df1d1..13ecd8e23 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -35,6 +35,12 @@ class DeformableInfantryGimbalController get_parameter_or("pitch_gravity_ff_gain", pitch_gravity_ff_gain_, 0.0); get_parameter_or("pitch_gravity_ff_phase", pitch_gravity_ff_phase_, 0.0); + get_parameter_or("yaw_ref_velocity_gain", yaw_ref_velocity_gain_, 1.0); + get_parameter_or("pitch_ref_velocity_gain", pitch_ref_velocity_gain_, 1.0); + get_parameter_or("yaw_velocity_ff_gain", yaw_velocity_ff_gain_, 0.0); + get_parameter_or("yaw_acceleration_ff_gain", yaw_acceleration_ff_gain_, 0.0); + get_parameter_or("pitch_velocity_ff_gain", pitch_velocity_ff_gain_, 0.0); + get_parameter_or("pitch_acceleration_ff_gain", pitch_acceleration_ff_gain_, 0.0); get_parameter_or("ctrl_hold_pitch_target_angle", ctrl_hold_pitch_target_angle_, 0.0); } @@ -70,16 +76,18 @@ class DeformableInfantryGimbalController if (!ctrl_hold_active_) *output_.pitch_angle_error = angle_error.pitch_angle_error; + const auto trajectory_ff = trajectory_feedforward(auto_aim_active); + if (!std::isfinite(angle_error.yaw_angle_error)) { yaw_angle_pid_.reset(); yaw_velocity_pid_.reset(); *output_.yaw_control_torque = kNaN; - } - - if (std::isfinite(angle_error.yaw_angle_error)) { - const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error); + } else { + const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error) + + trajectory_ff.yaw_ref_velocity; *output_.yaw_control_torque = - yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu); + yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu) + + trajectory_ff.yaw_velocity + trajectory_ff.yaw_acceleration; } if (!ctrl_hold_active_) { @@ -91,13 +99,15 @@ class DeformableInfantryGimbalController } else { const auto pitch_gravity_ff = pitch_gravity_feedforward(); const auto pitch_velocity_ref = - pitch_angle_pid_.update(angle_error.pitch_angle_error); + pitch_angle_pid_.update(angle_error.pitch_angle_error) + + trajectory_ff.pitch_ref_velocity; if (pitch_torque_control_enabled_) { *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) - + pitch_gravity_ff; + + pitch_gravity_ff + trajectory_ff.pitch_velocity + + trajectory_ff.pitch_acceleration; } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; @@ -130,16 +140,15 @@ class DeformableInfantryGimbalController component.register_input("/remote/mouse", mouse); component.register_input("/predefined/update_rate", update_rate, false); - component.register_input("/gimbal/yaw/angle", yaw_angle); - component.register_input("/gimbal/yaw/velocity", yaw_velocity); component.register_input("/gimbal/pitch/angle", pitch_angle); - component.register_input("/gimbal/pitch/velocity", pitch_velocity); component.register_input("/gimbal/yaw/velocity_imu", yaw_velocity_imu); component.register_input("/gimbal/pitch/velocity_imu", pitch_velocity_imu); component.register_input("/auto_aim/should_control", auto_aim_should_control, false); component.register_input( "/auto_aim/control_direction", auto_aim_control_direction, false); + component.register_input("/auto_aim/ff_v", auto_aim_ff_v, false); + component.register_input("/auto_aim/ff_a", auto_aim_ff_a, false); } InputInterface joystick_left; @@ -150,15 +159,14 @@ class DeformableInfantryGimbalController InputInterface mouse; InputInterface update_rate; - InputInterface yaw_angle; - InputInterface yaw_velocity; InputInterface pitch_angle; - InputInterface pitch_velocity; InputInterface yaw_velocity_imu; InputInterface pitch_velocity_imu; InputInterface auto_aim_should_control; InputInterface auto_aim_control_direction; + InputInterface auto_aim_ff_v; + InputInterface auto_aim_ff_a; } input_{*this}; struct Output { @@ -220,9 +228,39 @@ class DeformableInfantryGimbalController auto pitch_gravity_feedforward() const -> double { if (ctrl_hold_active_) return 0.0; - if (!input_.pitch_angle.ready() || !std::isfinite(*input_.pitch_angle)) - return 0.0; - return pitch_gravity_ff_gain_ * std::sin(*input_.pitch_angle - pitch_gravity_ff_phase_); + return pitch_gravity_ff_gain_ + * std::sin(gimbal_solver_.gimbal_world_pitch() - pitch_gravity_ff_phase_); + } + + struct TrajectoryFeedforward { + double yaw_ref_velocity = 0.0; + double pitch_ref_velocity = 0.0; + double yaw_velocity = 0.0; + double yaw_acceleration = 0.0; + double pitch_velocity = 0.0; + double pitch_acceleration = 0.0; + }; + + auto trajectory_feedforward(bool auto_aim_active) const -> TrajectoryFeedforward { + if (!auto_aim_active || !input_.auto_aim_ff_v.ready() || !input_.auto_aim_ff_a.ready() + || !input_.auto_aim_ff_v->allFinite() || !input_.auto_aim_ff_a->allFinite()) + return {}; + + const auto ff_v = + gimbal_solver_.odom_to_yaw_link(OdomImu::DirectionVector{*input_.auto_aim_ff_v}); + const auto ff_a = + gimbal_solver_.odom_to_yaw_link(OdomImu::DirectionVector{*input_.auto_aim_ff_a}); + return { + .yaw_ref_velocity = yaw_ref_velocity_gain_ * ff_v->z(), + .pitch_ref_velocity = + gimbal_solver_.clamp_pitch_feedforward(pitch_ref_velocity_gain_ * ff_v->y()), + .yaw_velocity = yaw_velocity_ff_gain_ * ff_v->z(), + .yaw_acceleration = yaw_acceleration_ff_gain_ * ff_a->z(), + .pitch_velocity = + gimbal_solver_.clamp_pitch_feedforward(pitch_velocity_ff_gain_ * ff_v->y()), + .pitch_acceleration = + gimbal_solver_.clamp_pitch_feedforward(pitch_acceleration_ff_gain_ * ff_a->y()), + }; } auto update_pitch_lock_state( @@ -233,7 +271,7 @@ class DeformableInfantryGimbalController suspension_on_by_switch_ = !suspension_on_by_switch_; } - pitch_lock_active_ = keyboard.ctrl || suspension_on_by_switch_; + pitch_lock_active_ = keyboard.ctrl || keyboard.e || suspension_on_by_switch_; last_switch_right_ = switch_right; } @@ -337,6 +375,12 @@ class DeformableInfantryGimbalController double ctrl_hold_pitch_target_angle_ = 0.0; double pitch_gravity_ff_gain_ = 0.0; double pitch_gravity_ff_phase_ = 0.0; + double yaw_ref_velocity_gain_ = 1.0; + double pitch_ref_velocity_gain_ = 1.0; + double yaw_velocity_ff_gain_ = 0.0; + double yaw_acceleration_ff_gain_ = 0.0; + double pitch_velocity_ff_gain_ = 0.0; + double pitch_acceleration_ff_gain_ = 0.0; bool pitch_lock_active_ = false; bool suspension_on_by_switch_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index 5c048d0ef..b5e5e66af 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -104,6 +104,7 @@ class TwoAxisGimbalSolver { }; AngleError update(const Operation& operation) { + pitch_limit_ = PitchLimit::None; update_yaw_axis(); PitchLink::DirectionVector control_direction = operation.update(*this); @@ -125,6 +126,25 @@ class TwoAxisGimbalSolver { bool enabled() const { return control_enabled_; } + double gimbal_world_pitch() const { + auto dir = + fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + return std::asin(std::clamp(dir->z(), -1.0, 1.0)); + } + + YawLink::DirectionVector odom_to_yaw_link(const OdomImu::DirectionVector& vector) const { + return fast_tf::cast(vector, *tf_); + } + + double clamp_pitch_feedforward(double feedforward) const { + // Positive local pitch moves toward the lower limit. + if (pitch_limit_ == PitchLimit::Upper) + return std::max(feedforward, 0.0); + if (pitch_limit_ == PitchLimit::Lower) + return std::min(feedforward, 0.0); + return feedforward; + } + private: void update_yaw_axis() { auto yaw_axis = @@ -168,10 +188,17 @@ class TwoAxisGimbalSolver { return; } - if (z > upper_limit_.y()) - *control_direction << upper_limit_.x() * projection, upper_limit_.y(); - else if (z < lower_limit_.y()) - *control_direction << lower_limit_.x() * projection, lower_limit_.y(); + // Frame transforms can place a target on the limit slightly inside it. + constexpr double limit_tolerance = 1e-12; + if (z >= upper_limit_.y() - limit_tolerance) { + pitch_limit_ = PitchLimit::Upper; + if (z > upper_limit_.y()) + *control_direction << upper_limit_.x() * projection, upper_limit_.y(); + } else if (z <= lower_limit_.y() + limit_tolerance) { + pitch_limit_ = PitchLimit::Lower; + if (z < lower_limit_.y()) + *control_direction << lower_limit_.x() * projection, lower_limit_.y(); + } } void clamp_yaw_limit(YawLink::DirectionVector& control_direction) { @@ -212,7 +239,10 @@ class TwoAxisGimbalSolver { static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + enum class PitchLimit { None, Upper, Lower }; + const Eigen::Vector2d upper_limit_, lower_limit_; + PitchLimit pitch_limit_ = PitchLimit::None; rmcs_executor::Component::InputInterface tf_; double yaw_cw_min_ = 0.; diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp index 2951f53f8..b6bf346ef 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp @@ -31,6 +31,7 @@ class PidCalculator { double update(double err) { if (!std::isfinite(err)) { + reset(); return nan; } else { double control = kp * err; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp deleted file mode 100644 index 8efa589ba..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ /dev/null @@ -1,876 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "hardware/device/bmi088.hpp" -#include "hardware/device/bmi088_ekf.hpp" -#include "hardware/device/board_clock_lifter.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/remote_control.hpp" -#include "hardware/device/supercap.hpp" -#include "hardware/device/vt13.hpp" -#include "hardware/util/status_monitor.hpp" - -namespace rmcs_core::hardware { - -using Clock = std::chrono::steady_clock; - -class DeformableInfantryOmniB - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DeformableInfantryOmniB() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , command_(create_partner_component(get_component_name() + "_command", *this)) { - using namespace rmcs_description; - - register_input("/predefined/timestamp", timestamp_); - register_output("/tf", tf_); - register_output( - "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); - register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); - register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); - - tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); - - remote_control_ = std::make_unique(*this); - - bottom_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_bottom_board").as_string()); - top_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_top_board").as_string()); - - // For command: remote-status - using Srv = std_srvs::srv::Trigger; - status_service_ = create_service( - "/rmcs/service/robot_status", - [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { - status_service_callback(response); - }); - } - - ~DeformableInfantryOmniB() override = default; - - void before_updating() override { top_board_->request_hard_sync_read(); } - - void update() override { - bottom_board_->update(); - top_board_->update(); - remote_control_->update(); - - using namespace rmcs_description; - *camera_transform_ = fast_tf::lookup_transform(*tf_); - *barrel_direction_ = - *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); - *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); - } - - void command_update() { - const bool even = ((cmd_tick_++ & 1u) == 0u); - bottom_board_->command_update(even); - top_board_->command_update(); - } - -private: - static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); - static constexpr auto kLeftFront = 0; - static constexpr auto kLeftBack = 1; - static constexpr auto kRightBack = 2; - static constexpr auto kRightFront = 3; - static constexpr auto kJointName = std::array{ - "left_front", - "left_back", - "right_back", - "right_front", - }; - - class Command : public Component { - public: - explicit Command(DeformableInfantryOmniB& deformableInfantry) - : deformableInfantry(deformableInfantry) {} - - void update() override { deformableInfantry.command_update(); } - - DeformableInfantryOmniB& deformableInfantry; - }; - - struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { - public: - explicit TopBoard( - DeformableInfantryOmniB& status, Component& command, - const std::string& serial_filter = {}) - : status_{status} - , tf_{status.tf_} - , bmi088_{device::Bmi088Ekf::Config{ - .body_to_sensor = - Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} - .toRotationMatrix()}} - , gimbal_pitch_motor_(status, command, "/gimbal/pitch") - , gimbal_left_friction_(status, command, "/gimbal/left_friction") - , gimbal_right_friction_(status, command, "/gimbal/right_friction") { - - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .set_reversed() - .set_encoder_zero_point( - static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} - .set_reduction_ratio(1.) - .set_reversed()); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( - 1.)); - - status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); - status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); - - auto options = librmcs::board::AdvancedOptions{}; - options.dangerously_skip_version_checks = true; - board_ = std::make_unique(*this, serial_filter, options); - - board_->start_transmit().gpio_digital_read( - Spec::kGpios.kUart1Rx, // - { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, - .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, - }); - - board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); - - status_.remote_control_->register_vt13(&vt13_); - } - ~TopBoard() override = default; - - [[nodiscard]] auto gimbal_yaw_velocity() const -> double { - return *gimbal_yaw_velocity_bmi088_; - } - - void request_hard_sync_read() { - // RMCS-lite top board variant currently has no GPIO hard-sync request - // path. - } - - void update() { - vt13_.update_status(); - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - - if (auto snapshot = bmi088_.snapshot()) { - *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); - *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); - tf_->set_transform( - snapshot->orientation.conjugate()); - } - - tf_->set_state( - pitch_encoder_angle); - } - - void command_update() const { - auto builder = board_->start_transmit(); - { - auto packet = gimbal_pitch_motor_.generate_torque_command(); - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x141, - .can_data = packet.as_bytes(), - }); - } - { - auto packet = device::CanPacket8{uint64_t{0}}; - packet << gimbal_right_friction_; - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = gimbal_right_friction_.send_id(), - .can_data = packet.as_bytes(), - }); - } - { - auto packet = device::CanPacket8{uint64_t{0}}; - packet << gimbal_left_friction_; - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = gimbal_left_friction_.send_id(), - .can_data = packet.as_bytes(), - }); - } - } - - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (can == Spec::kCans.kCan0) { - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - monitor_.tick("Top::Can0", data.can_id); - } else if (can == Spec::kCans.kCan1) { - gimbal_right_friction_.match_then_store_status(data.can_id, data.can_data); - monitor_.tick("Top::Can1", data.can_id); - } else if (can == Spec::kCans.kCan2) { - gimbal_left_friction_.match_then_store_status(data.can_id, data.can_data); - monitor_.tick("Top::Can2", data.can_id); - } - } - - void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { - if (uart == Spec::kUarts.kUart0) - vt13_.store_status(data.uart_data); - } - - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); - bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); - monitor_.tick("Top::Imu", "Acc"); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); - monitor_.tick("Top::Imu", "Gyr"); - if (!timestamp.has_value()) - return; - auto snapshot = - bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); - if (snapshot) - imu_snapshot_output_.emit(*snapshot); - } - - void gpio_digital_read_result_callback( - const Spec::Gpio& gpio, const View::GpioDigital& data) override { - if (gpio != Spec::kGpios.kUart1Rx) - return; - if (!data.timestamp_quarter_us) - return; - - const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); - if (!timestamp.has_value()) - return; - - camera_signal_output_.emit(*timestamp); - monitor_.tick("Top::CameraSync", "Active"); - } - - auto status() const -> std::vector { return monitor_.text(); } - - DeformableInfantryOmniB& status_; - OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; - - EventOutputInterface imu_snapshot_output_; - EventOutputInterface camera_signal_output_; - - device::Bmi088Ekf bmi088_; - device::BoardClockLifter board_clock_lifter_; - device::Vt13 vt13_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - - StatusMonitor monitor_{}; - std::unique_ptr board_; - }; - - struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { - public: - explicit BottomBoard( - DeformableInfantryOmniB& status, Component& command, - const std::string& serial_filter = {}) - : status_{status} - , command_{command} - , kChassisRadiusBase(status.get_parameter("chassis_radius").as_double()) - , kRodLength(status.get_parameter("rod_length").as_double()) - , kDefaultRadius(kChassisRadiusBase + kRodLength) { - - status.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - board_->start_transmit().uart_transmit( - Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); - return size; - }; - - gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} - .set_reversed() - .set_reduction_ratio(13.0) - .enable_multi_turn_angle()); - - for (auto& motor : chassis_joint_motors_) - motor.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} - .set_reversed() - .enable_multi_turn_angle()); - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU - // installation is re-validated on hardware. - return std::make_tuple(-y, x, z); - }); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 3} - .enable_multi_turn_angle()); - - status.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - status.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); - status.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); - status.register_output("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); - status.register_output("/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); - for (size_t i = 0; i < 4; ++i) { - status.register_output( - std::format( - "/chassis/{}_joint/physical_angle", DeformableInfantryOmniB::kJointName[i]), - joint_physical_angle_[i], kNaN); - status.register_output( - std::format( - "/chassis/{}_joint/physical_velocity", - DeformableInfantryOmniB::kJointName[i]), - joint_physical_velocity_[i], kNaN); - } - status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); - status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); - status.register_output("/chassis/radius", radius_, kDefaultRadius); - - status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); - status.get_parameter_or( - "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - - auto options = librmcs::board::AdvancedOptions{}; - options.dangerously_skip_version_checks = true; - board_ = std::make_unique(*this, serial_filter, options); - - status_.remote_control_->register_dr16(&dr16_); - } - void update() { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gz(); - { - const double q0 = imu_.q0(); - const double q1 = imu_.q1(); - const double q2 = imu_.q2(); - const double q3 = imu_.q3(); - - double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); - sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); - - const double standard_pitch = std::asin(sin_pitch); - const double standard_roll = - std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); - - // Export chassis attitude using the requested convention: - // pitch < 0 when the front is higher, roll > 0 when the left side is higher. - *chassis_imu_pitch_ = -standard_pitch; - *chassis_imu_roll_ = standard_roll; - *chassis_imu_pitch_rate_ = -imu_.gy(); - *chassis_imu_roll_rate_ = imu_.gx(); - } - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_joint_motors_) - motor.update_status(); - - for (size_t i = 0; i < 4; ++i) - update_joint_physical_feedback_( - i, joint_physical_angle_[i], joint_physical_velocity_[i]); - - update_geometry_feedback_(); - if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) - log_chassis_feedback_once_per_second_(); - - dr16_.update_status(); - gimbal_yaw_motor_.update_status(); - if (supercap_status_received_.load(std::memory_order_relaxed)) - supercap_.update_status(); - if (debug_log_supercap_) - log_supercap_feedback_once_per_second_(); - gimbal_bullet_feeder_.update_status(); - - tf_->set_state( - gimbal_yaw_motor_.angle()); - } - - void command_update(bool even) { - auto builder = board_->start_transmit(); - if (even) { - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kLeftFront].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kLeftBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan3, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightFront].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - } else { - for (size_t i = 0; i < 4; ++i) { - switch (i) { - case kLeftFront: - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); - break; - case kLeftBack: - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); - break; - case kRightBack: - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); - break; - case kRightFront: - builder.can_transmit( - Spec::kCans.kCan3, // - { - .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), - }); - break; - default: break; - } - } - } - } - - static constexpr double kJointZeroPhysicalAngleRad = 62.5 * std::numbers::pi / 180.0; - - DeformableInfantryOmniB& status_; - Component& command_; - - std::unique_ptr board_; - - // Interfaces - - OutputInterface& tf_{status_.tf_}; - - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_imu_pitch_; - OutputInterface chassis_imu_roll_; - OutputInterface chassis_imu_pitch_rate_; - OutputInterface chassis_imu_roll_rate_; - - std::array, 4> joint_physical_angle_; - std::array, 4> joint_physical_velocity_; - - OutputInterface encoder_alpha_; - OutputInterface encoder_alpha_dot_; - OutputInterface radius_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - - // State - - std::atomic wheel_status_received_[4] = {false, false, false, false}; - std::atomic joint_status_received_[4] = {false, false, false, false}; - - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - - const double kChassisRadiusBase; - const double kRodLength; - const double kDefaultRadius; - - Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - - // Device - - device::Bmi088 imu_{1000, 0.2, 0.0}; - device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_; - - device::DjiMotor chassis_wheel_motors_[4]{ - device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, - device::DjiMotor{status_, command_, "/chassis/left_back_wheel"}, - device::DjiMotor{status_, command_, "/chassis/right_back_wheel"}, - device::DjiMotor{status_, command_, "/chassis/right_front_wheel"}, - }; - device::LkMotor chassis_joint_motors_[4]{ - device::LkMotor{status_, command_, "/chassis/left_front_joint"}, - device::LkMotor{status_, command_, "/chassis/left_back_joint"}, - device::LkMotor{status_, command_, "/chassis/right_back_joint"}, - device::LkMotor{status_, command_, "/chassis/right_front_joint"}, - }; - - std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; - std::atomic supercap_status_received_{false}; - device::Supercap supercap_{status_, command_}; - - device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; - - void process_chassis_can_receive_(size_t index, const View::Can& data) { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[index].store_status(data.can_data); - wheel_status_received_[index].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[index].store_status(data.can_data); - joint_status_received_[index].store(true, std::memory_order_relaxed); - } - } - - void update_joint_physical_feedback_( - size_t index, OutputInterface& angle_output, - OutputInterface& velocity_output) { - - if (!joint_status_received_[index].load(std::memory_order_relaxed)) { - *angle_output = kNaN; - *velocity_output = kNaN; - return; - } - - const auto to_physical_angle = [](double motor_angle) { - return kJointZeroPhysicalAngleRad - motor_angle; - }; - const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; - - *angle_output = to_physical_angle(chassis_joint_motors_[index].angle()); - *velocity_output = to_physical_velocity(chassis_joint_motors_[index].velocity()); - } - - void update_geometry_feedback_() { - const Eigen::Vector4d alpha_rad{ - *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], - *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront]}; - const Eigen::Vector4d alpha_dot_rad{ - *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], - *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront]}; - - if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { - *encoder_alpha_ = kNaN; - *encoder_alpha_dot_ = kNaN; - *radius_ = kDefaultRadius; - RCLCPP_WARN_THROTTLE( - status_.get_logger(), *status_.get_clock(), 1000, - "deformable joint feedback invalid, fallback chassis radius to default %.3f m", - kDefaultRadius); - return; - } - - *encoder_alpha_ = alpha_rad.mean(); - *encoder_alpha_dot_ = alpha_dot_rad.mean(); - *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); - } - - void log_chassis_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_chassis_feedback_log_time_) - return; - - const auto wheel_rx = [this](size_t index) { - return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - const auto joint_rx = [this](size_t index) { - return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - - if (debug_log_wheel_motor_) { - std::string wheel_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - wheel_rx_str.push_back(' '); - wheel_rx_str.push_back(wheel_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%s]", - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); - } - - if (debug_log_deformable_joint_motor_) { - std::string joint_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - joint_rx_str.push_back(' '); - joint_rx_str.push_back(joint_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%s]", - *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], - *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], - *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], - *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], - joint_rx_str.c_str()); - } - - next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void log_supercap_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_supercap_feedback_log_time_) - return; - - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); - auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); - const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); - - RCLCPP_INFO( - status_.get_logger(), - "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " - "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : kNaN, - supercap_rx ? supercap_.chassis_voltage() : kNaN, - supercap_rx ? supercap_.chassis_power() : kNaN, - std::to_integer(supercap_raw_bytes[0]), - std::to_integer(supercap_raw_bytes[1]), - std::to_integer(supercap_raw_bytes[2]), - std::to_integer(supercap_raw_bytes[3]), - std::to_integer(supercap_raw_bytes[4]), - std::to_integer(supercap_raw_bytes[5]), - std::to_integer(supercap_raw_bytes[6]), - std::to_integer(supercap_raw_bytes[7])); - - next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (can == Spec::kCans.kCan0) { - process_chassis_can_receive_(0, data); - monitor_.tick("Bottom::Can0", data.can_id); - } else if (can == Spec::kCans.kCan1) { - process_chassis_can_receive_(1, data); - if (!data.is_extended_can_id && !data.is_remote_transmission - && data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); - supercap_.store_status(data.can_data); - supercap_status_received_.store(true, std::memory_order_relaxed); - } - monitor_.tick("Bottom::Can1", data.can_id); - } else if (can == Spec::kCans.kCan2) { - process_chassis_can_receive_(2, data); - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x142) - gimbal_yaw_motor_.store_status(data.can_data); - else if (data.can_id == 0x203) - gimbal_bullet_feeder_.store_status(data.can_data); - monitor_.tick("Bottom::Can2", data.can_id); - } else if (can == Spec::kCans.kCan3) { - process_chassis_can_receive_(3, data); - monitor_.tick("Bottom::Can3", data.can_id); - } - } - - void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { - if (uart == Spec::kUarts.kDbus) { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - monitor_.tick("Bottom::Dbus", "Active"); - } else if (uart == Spec::kUarts.kUart0) { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, - data.uart_data.size()); - monitor_.tick("Bottom::Uart0", "Active"); - } - } - - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - monitor_.tick("Bottom::Imu", "Acc"); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - monitor_.tick("Bottom::Imu", "Gyr"); - } - - auto status() const -> std::vector { return monitor_.text(); } - - StatusMonitor monitor_{}; - }; - - auto status_service_callback(const std::shared_ptr& response) - -> void { - response->success = true; - - auto feedback_message = std::ostringstream{}; - auto text = [&](std::format_string format, Args&&... args) { - std::println(feedback_message, format, std::forward(args)...); - }; - - text(" yaw_motor_zero_point: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); - text(" pitch_motor_zero_point: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - constexpr auto kPosition = - std::array{"left_front", "left_back", "right_back", "right_front"}; - - text(""); - for (auto&& [index, motor] : - std::views::zip(kPosition, bottom_board_->chassis_joint_motors_)) { - text(" {}_zero_point: {}", index, motor.last_raw_angle()); - } - - text("\nBottomBoard Status:"); - for (const auto& line : bottom_board_->status()) - text("> {}", line); - - text("\nTopBoard Status:"); - for (const auto& line : top_board_->status()) - text("> {}", line); - - response->message = feedback_message.str(); - } - - OutputInterface tf_; - OutputInterface camera_transform_; - OutputInterface barrel_direction_; - OutputInterface auto_aim_yaw_velocity_; - InputInterface timestamp_; - - std::unique_ptr bottom_board_; - std::unique_ptr top_board_; - std::unique_ptr remote_control_; - - std::shared_ptr command_; - uint32_t cmd_tick_ = 0; - - std::shared_ptr> status_service_; -}; - -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmniB, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 83c40c97b..023bc203a 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -8,12 +8,12 @@ #include #include #include +#include #include #include #include #include -#include #include #include @@ -34,6 +34,7 @@ #include "hardware/device/lk_motor.hpp" #include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" #include "hardware/util/status_monitor.hpp" namespace rmcs_core::hardware { @@ -111,7 +112,7 @@ class DeformableInfantryOmni "right_front", }; - class Command : public rmcs_executor::Component { + class Command : public Component { public: explicit Command(DeformableInfantryOmni& deformableInfantry) : deformableInfantry(deformableInfantry) {} @@ -121,10 +122,201 @@ class DeformableInfantryOmni DeformableInfantryOmni& deformableInfantry; }; + struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { + public: + explicit TopBoard( + DeformableInfantryOmni& status, Component& command, + const std::string& serial_filter = {}) + : status_{status} + , tf_{status.tf_} + , bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = + Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} + .toRotationMatrix()}} + , gimbal_pitch_motor_(status, command, "/gimbal/pitch") + , gimbal_left_friction_(status, command, "/gimbal/left_friction") + , gimbal_right_friction_(status, command, "/gimbal/right_friction") { + + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() + .set_encoder_zero_point( + static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); + + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} + .set_reduction_ratio(1.) + .set_reversed()); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( + 1.)); + + status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); + status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + + auto options = librmcs::board::AdvancedOptions{}; + options.dangerously_skip_version_checks = true; + board_ = std::make_unique(*this, serial_filter, options); + + board_->start_transmit().gpio_digital_read( + Spec::kGpios.kUart1Rx, // + { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); + + board_->start_transmit().uart_config(Spec::kUarts.kUart0, {.baudrate = 921600}); + + status_.remote_control_->register_vt13(&vt13_); + } + + ~TopBoard() override = default; + + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_bmi088_; + } + + void request_hard_sync_read() { + // RMCS-lite top board variant currently has no GPIO hard-sync request + // path. + } + + void update() { + vt13_.update_status(); + gimbal_pitch_motor_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + + if (auto snapshot = bmi088_.snapshot()) { + *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); + *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); + tf_->set_transform( + snapshot->orientation.conjugate()); + } + + tf_->set_state( + pitch_encoder_angle); + } + + void command_update() { + auto builder = board_->start_transmit(); + { + auto packet = gimbal_pitch_motor_.generate_torque_command(); + builder.can_transmit( + Spec::kCans.kCan0, // + { + .can_id = 0x141, + .can_data = packet.as_bytes(), + }); + } + { + auto packet = device::CanPacket8{uint64_t{0}}; + packet << gimbal_left_friction_; + builder.can_transmit( + Spec::kCans.kCan1, // + { + .can_id = gimbal_left_friction_.send_id(), + .can_data = packet.as_bytes(), + }); + } + { + auto packet = device::CanPacket8{uint64_t{0}}; + packet << gimbal_right_friction_; + builder.can_transmit( + Spec::kCans.kCan2, // + { + .can_id = gimbal_right_friction_.send_id(), + .can_data = packet.as_bytes(), + }); + } + } + + void can_receive_callback(const Spec::Can& can, const View::Can& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (can == Spec::kCans.kCan0) { + if (data.can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + monitor_.tick("Top::Can0", data.can_id); + } else if (can == Spec::kCans.kCan1) { + gimbal_left_friction_.match_then_store_status(data.can_id, data.can_data); + monitor_.tick("Top::Can1", data.can_id); + } else if (can == Spec::kCans.kCan2) { + gimbal_right_friction_.match_then_store_status(data.can_id, data.can_data); + monitor_.tick("Top::Can2", data.can_id); + } + } + + void uart_receive_callback(const Spec::Uart& uart, const View::Uart& data) override { + if (uart == Spec::kUarts.kUart0) + vt13_.store_status(data.uart_data); + } + + void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); + monitor_.tick("Top::Imu", "Acc"); + } + + void gyroscope_receive_callback(const View::ImuGyroscope& data) override { + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + monitor_.tick("Top::Imu", "Gyr"); + if (!timestamp.has_value()) + return; + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (snapshot) + imu_snapshot_output_.emit(*snapshot); + } + + void gpio_digital_read_result_callback( + const Spec::Gpio& gpio, const View::GpioDigital& data) override { + if (gpio != Spec::kGpios.kUart1Rx) + return; + if (!data.timestamp_quarter_us) + return; + + const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + camera_signal_output_.emit(*timestamp); + monitor_.tick("Top::CameraSync", "Active"); + } + + auto status() const -> std::vector { return monitor_.text(); } + + DeformableInfantryOmni& status_; + OutputInterface& tf_; + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_bmi088_; + + EventOutputInterface imu_snapshot_output_; + EventOutputInterface camera_signal_output_; + + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; + device::Vt13 vt13_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + + StatusMonitor monitor_{}; + std::unique_ptr board_; + }; + struct BottomBoard final : public librmcs::board::RmcsBoardLite::Callback { public: explicit BottomBoard( - DeformableInfantryOmni& status, rmcs_executor::Component& command, + DeformableInfantryOmni& status, Component& command, const std::string& serial_filter = {}) : status_{status} , command_{command} @@ -189,11 +381,6 @@ class DeformableInfantryOmni status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); status.register_output("/chassis/radius", radius_, kDefaultRadius); - status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); - status.get_parameter_or( - "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - auto options = librmcs::board::AdvancedOptions{}; options.dangerously_skip_version_checks = true; board_ = std::make_unique(*this, serial_filter, options); @@ -235,15 +422,11 @@ class DeformableInfantryOmni i, joint_physical_angle_[i], joint_physical_velocity_[i]); update_geometry_feedback_(); - if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) - log_chassis_feedback_once_per_second_(); dr16_.update_status(); gimbal_yaw_motor_.update_status(); if (supercap_status_received_.load(std::memory_order_relaxed)) supercap_.update_status(); - if (debug_log_supercap_) - log_supercap_feedback_once_per_second_(); gimbal_bullet_feeder_.update_status(); tf_->set_state( @@ -279,18 +462,17 @@ class DeformableInfantryOmni } .as_bytes(), }); + auto packet_can2_200 = device::CanPacket8{ + chassis_wheel_motors_[kRightBack].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + }; builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[kRightBack].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), + .can_data = packet_can2_200.as_bytes(), }); builder.can_transmit( Spec::kCans.kCan3, // @@ -305,11 +487,12 @@ class DeformableInfantryOmni } .as_bytes(), }); + auto packet_can2_142 = gimbal_yaw_motor_.generate_command(); builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + .can_data = packet_can2_142.as_bytes(), }); builder.can_transmit( Spec::kCans.kCan1, // @@ -343,14 +526,16 @@ class DeformableInfantryOmni .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), }); break; - case kRightBack: + case kRightBack: { + auto packet = chassis_joint_motors_[i].generate_command(); builder.can_transmit( Spec::kCans.kCan2, // { .can_id = 0x141, - .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), + .can_data = packet.as_bytes(), }); break; + } case kRightFront: builder.can_transmit( Spec::kCans.kCan3, // @@ -394,25 +579,17 @@ class DeformableInfantryOmni // State - std::atomic wheel_status_received_[4] = {false, false, false, false}; std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - const double kChassisRadiusBase; const double kRodLength; const double kDefaultRadius; - Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - // Device device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{}; + device::Dr16 dr16_; device::DjiMotor chassis_wheel_motors_[4]{ device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, @@ -427,7 +604,6 @@ class DeformableInfantryOmni device::LkMotor{status_, command_, "/chassis/right_front_joint"}, }; - std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; std::atomic supercap_status_received_{false}; device::Supercap supercap_{status_, command_}; @@ -438,7 +614,6 @@ class DeformableInfantryOmni return; if (data.can_id == 0x201) { chassis_wheel_motors_[index].store_status(data.can_data); - wheel_status_received_[index].store(true, std::memory_order_relaxed); } else if (data.can_id == 0x141) { chassis_joint_motors_[index].store_status(data.can_data); joint_status_received_[index].store(true, std::memory_order_relaxed); @@ -488,91 +663,6 @@ class DeformableInfantryOmni *radius_ = (kChassisRadiusBase + kRodLength * alpha_rad.array().cos()).mean(); } - void log_chassis_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_chassis_feedback_log_time_) - return; - - const auto wheel_rx = [this](size_t index) { - return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - const auto joint_rx = [this](size_t index) { - return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - - if (debug_log_wheel_motor_) { - std::string wheel_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - wheel_rx_str.push_back(' '); - wheel_rx_str.push_back(wheel_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%s]", - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), - chassis_wheel_motors_[kLeftFront].angle(), - chassis_wheel_motors_[kLeftBack].angle(), - chassis_wheel_motors_[kRightBack].angle(), - chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); - } - - if (debug_log_deformable_joint_motor_) { - std::string joint_rx_str; - for (size_t i = 0; i < 4; ++i) { - if (i > 0) - joint_rx_str.push_back(' '); - joint_rx_str.push_back(joint_rx(i)); - } - RCLCPP_INFO( - status_.get_logger(), - "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%s]", - *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], - *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], - *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], - *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], - joint_rx_str.c_str()); - } - - next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void log_supercap_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_supercap_feedback_log_time_) - return; - - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); - auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); - const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); - - RCLCPP_INFO( - status_.get_logger(), - "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " - "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : kNaN, - supercap_rx ? supercap_.chassis_voltage() : kNaN, - supercap_rx ? supercap_.chassis_power() : kNaN, - std::to_integer(supercap_raw_bytes[0]), - std::to_integer(supercap_raw_bytes[1]), - std::to_integer(supercap_raw_bytes[2]), - std::to_integer(supercap_raw_bytes[3]), - std::to_integer(supercap_raw_bytes[4]), - std::to_integer(supercap_raw_bytes[5]), - std::to_integer(supercap_raw_bytes[6]), - std::to_integer(supercap_raw_bytes[7])); - - next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); - } - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { if (data.is_extended_can_id || data.is_remote_transmission) return; @@ -583,9 +673,6 @@ class DeformableInfantryOmni process_chassis_can_receive_(1, data); if (!data.is_extended_can_id && !data.is_remote_transmission && data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); supercap_.store_status(data.can_data); supercap_status_received_.store(true, std::memory_order_relaxed); } @@ -594,9 +681,9 @@ class DeformableInfantryOmni process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x142) + if (data.can_id == 0x142) { gimbal_yaw_motor_.store_status(data.can_data); - else if (data.can_id == 0x203) + } else if (data.can_id == 0x203) gimbal_bullet_feeder_.store_status(data.can_data); monitor_.tick("Bottom::Can2", data.can_id); } else if (can == Spec::kCans.kCan3) { @@ -633,188 +720,6 @@ class DeformableInfantryOmni StatusMonitor monitor_{}; }; - struct TopBoard final : public librmcs::board::RmcsBoardLite::Callback { - public: - explicit TopBoard( - DeformableInfantryOmni& status, rmcs_executor::Component& command, - const std::string& serial_filter = {}) - : tf_{status.tf_} - , bmi088_{device::Bmi088Ekf::Config{ - .body_to_sensor = - Eigen::AngleAxisd{std::numbers::pi / 2.0, Eigen::Vector3d::UnitX()} - .toRotationMatrix()}} - , gimbal_pitch_motor_(status, command, "/gimbal/pitch") - , gimbal_left_friction_(status, command, "/gimbal/left_friction") - , gimbal_right_friction_(status, command, "/gimbal/right_friction") { - - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .set_reversed() - .set_encoder_zero_point( - static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 1} - .set_reduction_ratio(1.) - .set_reversed()); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508, 2}.set_reduction_ratio( - 1.)); - - status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - status.register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); - status.register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); - - auto options = librmcs::board::AdvancedOptions{}; - options.dangerously_skip_version_checks = true; - board_ = std::make_unique(*this, serial_filter, options); - - board_->start_transmit().gpio_digital_read( - Spec::kGpios.kUart1Rx, { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, - .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, - }); - } - - ~TopBoard() override = default; - - [[nodiscard]] auto gimbal_yaw_velocity() const -> double { - return *gimbal_yaw_velocity_bmi088_; - } - - void request_hard_sync_read() { - // RMCS-lite top board variant currently has no GPIO hard-sync request - // path. - } - - void update() { - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - - if (auto snapshot = bmi088_.snapshot()) { - *gimbal_pitch_velocity_bmi088_ = snapshot->gyro_body.y(); - *gimbal_yaw_velocity_bmi088_ = snapshot->gyro_body.z(); - tf_->set_transform( - snapshot->orientation.conjugate()); - } - - tf_->set_state( - pitch_encoder_angle); - } - - void command_update() const { - auto builder = board_->start_transmit(); - builder.can_transmit( - Spec::kCans.kCan0, // - { - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan1, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_left_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can_transmit( - Spec::kCans.kCan2, // - { - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - gimbal_right_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - void can_receive_callback(const Spec::Can& can, const View::Can& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (can == Spec::kCans.kCan0) { - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - monitor_.tick("Top::Can0", data.can_id); - } else if (can == Spec::kCans.kCan1) { - if (data.can_id == 0x201) - gimbal_left_friction_.store_status(data.can_data); - monitor_.tick("Top::Can1", data.can_id); - } else if (can == Spec::kCans.kCan2) { - if (data.can_id == 0x202) - gimbal_right_friction_.store_status(data.can_data); - monitor_.tick("Top::Can2", data.can_id); - } - } - - void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); - bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); - monitor_.tick("Top::Imu", "Acc"); - } - - void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); - monitor_.tick("Top::Imu", "Gyr"); - if (!timestamp.has_value()) - return; - auto snapshot = - bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); - if (snapshot) - imu_snapshot_output_.emit(*snapshot); - } - - void gpio_digital_read_result_callback( - const Spec::Gpio& gpio, const View::GpioDigital& data) override { - if (gpio != Spec::kGpios.kUart1Rx) - return; - if (!data.timestamp_quarter_us) - return; - - const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); - if (!timestamp.has_value()) - return; - - camera_signal_output_.emit(*timestamp); - monitor_.tick("Top::CameraSync", "Active"); - } - - auto status() const -> std::vector { return monitor_.text(); } - - OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; - - EventOutputInterface imu_snapshot_output_; - EventOutputInterface camera_signal_output_; - - device::Bmi088Ekf bmi088_; - device::BoardClockLifter board_clock_lifter_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - - StatusMonitor monitor_{}; - std::unique_ptr board_; - }; - auto status_service_callback(const std::shared_ptr& response) -> void { response->success = true; @@ -865,4 +770,4 @@ class DeformableInfantryOmni } // namespace rmcs_core::hardware #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmni, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmni, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp index 83c1b2e32..33ae9bef4 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp @@ -84,7 +84,7 @@ class Bmi088Ekf { ekf_state_time_ = accel_sample_time; const auto correction = ekf_.prepare_correction(pending_accel_sample_->accel_g); - if (!correction || correction->chi_square() >= 3.0) + if (!correction || correction->chi_square() >= 16.0) break; if (!ekf_.correct(*correction)) break; diff --git a/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp b/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp index e537da8e8..29008bf6f 100644 --- a/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/identification/static_torque_test_controller.cpp @@ -9,10 +9,14 @@ #include #include #include +#include + +#include #include #include #include +#include #include #include #include @@ -25,7 +29,6 @@ namespace { using Clock = std::chrono::steady_clock; -constexpr auto kCenterInfoInterval = std::chrono::duration(0.5); constexpr double kRangeTolerance = 1e-9; template @@ -93,17 +96,14 @@ double wrap_to_pi(double angle) { enum class RemoteMode { kMeasureRange, - kCenterHold, kTestCommand, kIdle, }; RemoteMode decode_remote_mode(rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) { using rmcs_msgs::Switch; - if (switch_left == Switch::MIDDLE && switch_right == Switch::DOWN) - return RemoteMode::kMeasureRange; if (switch_left == Switch::MIDDLE && switch_right == Switch::MIDDLE) - return RemoteMode::kCenterHold; + return RemoteMode::kMeasureRange; if (switch_left == Switch::MIDDLE && switch_right == Switch::UP) return RemoteMode::kTestCommand; return RemoteMode::kIdle; @@ -149,6 +149,7 @@ class StaticTorqueTestController register_input("/predefined/timestamp", timestamp_); register_input("/remote/switch/left", switch_left_); register_input("/remote/switch/right", switch_right_); + register_input("/tf", tf_); register_input(measured_torque_name_, measured_torque_); register_input(measured_velocity_name_, measured_velocity_); register_input(measured_angle_name_, measured_angle_); @@ -167,7 +168,6 @@ class StaticTorqueTestController angle_tracking_initialized_ = false; range_initialized_ = false; remote_mode_initialized_ = false; - next_center_info_time_ = Clock::time_point{}; } void update() override { @@ -178,7 +178,6 @@ class StaticTorqueTestController switch (remote_mode) { case RemoteMode::kMeasureRange: handle_measure_range(); break; - case RemoteMode::kCenterHold: handle_center_hold(mode_changed); break; case RemoteMode::kTestCommand: handle_test_command(mode_changed); break; case RemoteMode::kIdle: handle_idle(); break; } @@ -224,56 +223,11 @@ class StaticTorqueTestController range_max_ = std::max(range_max_, current_continuous_angle_); } - void handle_center_hold(bool mode_changed) { - stop_test(true); - - if (!range_initialized_) { - *control_torque_ = nan_; - return; - } - - if (mode_changed) { - position_pid_.reset(); - velocity_pid_.reset(); - next_center_info_time_ = *timestamp_; - } - - active_setpoint_ = 0.5 * (range_min_ + range_max_); - *control_torque_ = calculate_pid_output(active_setpoint_); - - maybe_log_center_hold_info(); - } - - void maybe_log_center_hold_info() { - if (*timestamp_ < next_center_info_time_) - return; - - const double control_error = active_setpoint_ - current_continuous_angle_; - RCLCPP_INFO( - get_logger(), - "Center hold error=%.6f rad, setpoint=%.6f rad, angle=%.6f rad, range=[%.6f, %.6f] rad", - control_error, wrap_to_pi(active_setpoint_), current_wrapped_angle_, - wrap_to_pi(range_min_), wrap_to_pi(range_max_)); - - do { - next_center_info_time_ += - std::chrono::duration_cast(kCenterInfoInterval); - } while (*timestamp_ >= next_center_info_time_); - } - void handle_test_command(bool mode_changed) { if (mode_changed) { - if (last_remote_mode_ == RemoteMode::kCenterHold) { - if (!start_test()) { - *control_torque_ = nan_; - return; - } - } else if (!test_setpoint_valid_) { + if (!start_test()) { *control_torque_ = nan_; return; - } else { - position_pid_.reset(); - velocity_pid_.reset(); } } @@ -334,7 +288,8 @@ class StaticTorqueTestController csv_writer_.open(path); csv_writer_.write_row( "update_count", "elapsed_s", control_torque_name_, measured_torque_name_, - measured_velocity_name_, measured_angle_name_); + measured_velocity_name_, measured_angle_name_, "imu_pitch_angle", + "imu_yaw_angle"); csv_writer_.flush(); } catch (const std::exception& exception) { const auto path_string = path.string(); @@ -364,12 +319,23 @@ class StaticTorqueTestController const double elapsed_s = std::max(0.0, std::chrono::duration(*timestamp_ - test_start_time_).count()); + const auto [imu_yaw, imu_pitch] = imu_yaw_pitch(); + csv_writer_.write_row( *update_count_, elapsed_s, *control_torque_, *measured_torque_, *measured_velocity_, - current_wrapped_angle_); + current_wrapped_angle_, imu_pitch, imu_yaw); csv_writer_.flush(); } + std::pair imu_yaw_pitch() const { + auto dir = fast_tf::cast( + rmcs_description::PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + const double pitch = std::asin(std::clamp(dir->z(), -1.0, 1.0)); + if (std::abs(dir->x()) < 1e-6 && std::abs(dir->y()) < 1e-6) + return {nan_, pitch}; + return {std::atan2(dir->y(), dir->x()), pitch}; + } + void finish_test_sequence() { test_active_ = false; close_csv(); @@ -430,6 +396,7 @@ class StaticTorqueTestController InputInterface timestamp_; InputInterface switch_left_; InputInterface switch_right_; + InputInterface tf_; InputInterface measured_torque_; InputInterface measured_velocity_; InputInterface measured_angle_; @@ -450,7 +417,6 @@ class StaticTorqueTestController bool remote_mode_initialized_ = false; RemoteMode last_remote_mode_ = RemoteMode::kIdle; - Clock::time_point next_center_info_time_{}; double active_setpoint_ = 0.0; bool test_setpoint_valid_ = false; diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index c0a7db62e..2357bc672 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -103,6 +104,9 @@ class Status register_output("/referee/map_command/event/source", map_command_event_source_, 0); register_output("/referee/map_command/event/timestamp", map_command_event_timestamp_, 0.0); register_output("/referee/map_command/event/sequence", map_command_event_sequence_, 0); + register_output( + "/referee/multi_robot_communication/lidar_msg_broadcast", lidar_msg_broadcast_, + LidarMsgBroadcast{}); robot_status_watchdog_.reset(5'000); } @@ -182,6 +186,8 @@ class Status update_sentry_info(); else if (command_id == 0x0303) update_map_command(); + else if (command_id == 0x0301) + update_robot_interaction_data(); } void update_game_status() { @@ -326,6 +332,29 @@ class Status *map_command_event_timestamp_ = *map_command_received_timestamp_; *map_command_event_sequence_ += 1; } + + void update_robot_interaction_data() { + if (frame_.header.data_length < sizeof(RobotInteractionData)) { + RCLCPP_WARN( + logger_, "Robot interaction data length invalid: %u", + static_cast(frame_.header.data_length)); + return; + } + + RobotInteractionData data; + std::memcpy(&data, frame_.body.data, sizeof(data)); + + if (data.sender_id != 9 && data.sender_id != 109) + return; + if (data.data_cmd_id < 0x0200 || data.data_cmd_id > 0x02ff) + return; + + LidarMsgBroadcast lidar_msg{}; + static_assert(sizeof(data.user_data) == std::tuple_size_v); + std::memcpy(lidar_msg.data(), data.user_data, sizeof(data.user_data)); + *lidar_msg_broadcast_ = lidar_msg; + } + // When referee system loses connection unexpectedly, // use these indicators make sure the robot safe. // Muzzle: Cooling priority with level 1 @@ -403,6 +432,9 @@ class Status OutputInterface map_command_event_sequence_; MapCommand last_map_command_{}; bool has_last_map_command_ = false; + + using LidarMsgBroadcast = std::array; + OutputInterface lidar_msg_broadcast_; }; } // namespace rmcs_core::referee diff --git a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp index ad5e21619..6bde951cf 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -158,4 +158,12 @@ struct __attribute__((packed)) SentryInfo { }; static_assert(sizeof(SentryInfo) == 14); +struct __attribute__((packed)) RobotInteractionData { + uint16_t data_cmd_id; + uint16_t sender_id; + uint16_t receiver_id; + uint8_t user_data[112]; +}; +static_assert(sizeof(RobotInteractionData) == 118); + } // namespace rmcs_core::referee::status