feat: Sentry support (#60)

* style: update clang-format config and fix missing newline

* feat(device): add make_pid_calculator() and last_raw_angle() interface

- pid_calculator: add make_pid_calculator() factory function with parameter loading
- dji_motor: add int last_raw_angle() public getter
- lk_motor: add int64_t last_raw_angle() public getter

* feat(description): add sentry TF link tree with dual-yaw gimbal joints

* feat(sentry): add sentry hardware driver with build dependencies

- Add sentry hardware component (GimbalBoard, Topboard, BottomBoard)
- Support dual-yaw gimbal, chassis steering, supercap, DR16 remote, referee serial
- Update CMakeLists.txt: bump librmcs to v3.1.0 for RmcsBoardLite support
- Add std_srvs dependency for robot status service

* feat(sentry): add EccentricDualYaw manual gimbal controller and launch config

- Add eccentric dual-yaw gimbal controller with manual joystick/mouse input
- Cascaded angle-velocity PID with friction and gravity feedforward
- Top-bottom yaw coupling compensation via k_top_to_bottom
- Launch config with tuned PID/Ff parameters for sentry robot

* build(docker): add navigation2 and lua deps for rmcs-navigation, remove dotnet

* refactor(sentry): streamline command component and can packet assembly

* feat: tool script for status checking

* chore: update sentry config

Author: creeper5820

.clang-format

@@ -13,27 +13,25 @@ MaxEmptyLinesToKeep: 1
 
 AlignAfterOpenBracket: AlwaysBreak
 
-AlignArrayOfStructures: Right
-
 AlignConsecutiveAssignments:
-    Enabled: false
+  Enabled: false
 AlignConsecutiveBitFields:
-    Enabled: true
-    AcrossEmptyLines: false
-    AcrossComments: false
+  Enabled: true
+  AcrossEmptyLines: false
+  AcrossComments: false
 AlignConsecutiveDeclarations:
-    Enabled: false
+  Enabled: false
 AlignConsecutiveMacros:
-    Enabled: true
-    AcrossEmptyLines: false
-    AcrossComments: false
+  Enabled: true
+  AcrossEmptyLines: false
+  AcrossComments: false
 # AlignConsecutiveShortCaseStatements:
 #     Enabled: false
 AlignEscapedNewlines: Left
 AlignOperands: AlignAfterOperator
 AlignTrailingComments:
-    Kind: Always
-    OverEmptyLines: 64
+  Kind: Always
+  OverEmptyLines: 64
 PointerAlignment: Left
 
 AllowAllArgumentsOnNextLine: true

.script/remote-status

@@ -0,0 +1,65 @@
+#!/bin/env bash
+
+set -euo pipefail
+
+# 定义需要查询的服务列表
+services=(
+    "/rmcs/service/robot_status"
+)
+
+# 使用 bash 远程执行，并传递服务列表作为参数
+ssh remote bash -s -- "${services[@]}" <<'EOF'
+set -euo pipefail
+
+# 1. 环境准备
+set +u
+if [ -f ~/env_setup.bash ]; then
+    source ~/env_setup.bash
+fi
+set -u
+
+# 获取所有可用服务列表
+all_services="$(ros2 service list 2>/dev/null || true)"
+input_services=("${@}")
+
+# 2. 定义单个服务查询函数
+call_status_service() {
+    local service="$1"
+
+    if ! printf "%s\n" "$all_services" | grep -Fxq "$service"; then
+        printf "[warn] service not found: %s\n\n" "$service"
+        return
+    fi
+
+    printf "=== %s ===\n" "$service"
+    local raw
+    raw="$(ros2 service call "$service" std_srvs/srv/Trigger "{}" 2>&1 || true)"
+
+    local msg
+    msg="$(printf "%s\n" "$raw" | sed -n "s/.*message='\(.*\)'.*/\1/p")"
+
+    if [[ -z "$msg" ]]; then
+        printf "[warn] failed to parse message: %s\n%s\n" "$service" "$raw"
+    else
+        printf "%b\n" "$msg"
+    fi
+    printf "\n"
+}
+
+# 3. 并发执行逻辑
+tmp_dir=$(mktemp -d)
+trap 'rm -rf "${tmp_dir}"' EXIT
+
+for i in "${!input_services[@]}"; do
+    call_status_service "${input_services[$i]}" > "${tmp_dir}/${i}" 2>&1 &
+done
+
+wait
+
+# 4. 按原始顺序汇总输出
+for i in "${!input_services[@]}"; do
+    if [ -f "${tmp_dir}/${i}" ]; then
+        cat "${tmp_dir}/${i}"
+    fi
+done
+EOF

Dockerfile

@@ -30,8 +30,9 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
     libceres-dev \
     ros-$ROS_DISTRO-rviz2 \
     ros-$ROS_DISTRO-foxglove-bridge \
-    dotnet-sdk-8.0 \
     ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pcl-conversions ros-$ROS_DISTRO-pcl-msgs && \
+    ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-msgs \
+    lua5.4 liblua5.4-0 liblua5.4-dev && \
     apt-get autoremove -y && apt-get clean && \
     rm -rf /var/lib/apt/lists/* /tmp/*
 

rmcs_ws/src/rmcs_bringup/config/sentry.yaml

@@ -0,0 +1,164 @@
+rmcs_executor:
+  ros__parameters:
+    update_rate: 1000.0
+    components:
+      - rmcs_core::hardware::Sentry -> sentry_hardware
+
+      - 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::controller::gimbal::EccentricDualYaw -> gimbal_controller
+
+      # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster
+
+      - 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::ChassisController -> chassis_controller
+      - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller
+      - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller
+
+      # - rmcs::navigation::Navigation -> rmcs_navigation
+
+# The positive direction is the one that battery exists
+sentry_hardware:
+  ros__parameters:
+    board_serial_top_board: "af-da30"
+    board_serial_bottom_board: "d4-2184"
+    board_serial_gimbal_board: "d4-1d2b"
+    bottom_yaw_motor_zero_point: 26163
+    top_yaw_motor_zero_point: 4540
+    pitch_motor_zero_point: 1446
+    left_front_zero_point: 7131
+    left_back_zero_point: 3740
+    right_back_zero_point: 1327
+    right_front_zero_point: 5147
+
+rmcs_navigation:
+  ros__parameters:
+    # 策略名称：
+    # - fast-push-output "速推前哨站"
+    # - kill-robots "杀伤优先"
+    decision: "fast-push-output"
+    command_vel_name: "/cmd_vel"
+    mock_context: false
+    endpoint: "test"
+    enable_goal_topic_forward: true
+
+gimbal_controller:
+  ros__parameters:
+    upper_limit: -0.39518
+    lower_limit: 0.36
+
+    top_yaw_angle_kp: 30.0
+    top_yaw_angle_ki: 0.0
+    top_yaw_angle_kd: 0.0
+    top_yaw_velocity_kp: 2.160
+    top_yaw_velocity_ki: 0.0
+    top_yaw_velocity_kd: 0.0
+
+    bottom_yaw_angle_kp: 8.8
+    bottom_yaw_angle_ki: 0.0
+    bottom_yaw_angle_kd: 0.0
+    bottom_yaw_velocity_kp: 22.49
+    bottom_yaw_velocity_ki: 0.0
+    bottom_yaw_velocity_kd: 0.0
+
+    pitch_angle_kp: 40.0
+    pitch_angle_ki: 0.0
+    pitch_angle_kd: 0.01
+    pitch_velocity_kp: 2.5
+    pitch_velocity_ki: 0.0
+    pitch_velocity_kd: 0.0
+
+    k_top_to_bottom: -1.0
+
+    bottom_yaw_viscous_ff_gain: 0.002495
+    # bottom_yaw_coulomb_ff_gain: 0.457343
+    # bottom_yaw_coulomb_ff_tanh_gain: 100.0
+    top_yaw_viscous_ff_gain: 0.231
+    # top_yaw_coulomb_ff_gain: 1.12
+    # top_yaw_coulomb_ff_tanh_gain: 100.0
+    pitch_viscous_ff_gain: 0.33
+    # pitch_coulomb_ff_gain: 0.95
+    # pitch_coulomb_ff_tanh_gain: 100.0
+    pitch_gravity_ff_gain: 2.128
+    pitch_gravity_ff_phase: 1.438
+
+chassis_controller:
+  ros__parameters:
+    navigation_velocity_scale: 1.0
+
+friction_wheel_controller:
+  ros__parameters:
+    friction_wheels:
+      - /gimbal/left_friction
+      - /gimbal/right_friction
+    friction_velocities:
+      - 630.0
+      - 630.0
+    friction_soft_start_stop_time: 1.0
+
+heat_controller:
+  ros__parameters:
+    heat_per_shot: 10000
+    reserved_heat: 10000
+
+bullet_feeder_controller:
+  ros__parameters:
+    bullets_per_feeder_turn: 9.0
+    shot_frequency: 28.0
+    safe_shot_frequency: 10.0
+    eject_frequency: 15.0
+    eject_time: 0.15
+    deep_eject_frequency: 15.0
+    deep_eject_time: 0.20
+    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: 0.283
+    ki: 0.0
+    kd: 0.0
+
+steering_wheel_controller:
+  ros__parameters:
+    mess: 22.0
+    moment_of_inertia: 0.77852676
+    vehicle_radius: 0.26870058
+    wheel_radius: 0.055
+    friction_coefficient: 0.666
+    k1: 2.958580e+00
+    k2: 3.082190e-03
+    no_load_power: 11.37
+    chassis_translation_kp: 8.0
+    chassis_translation_ki: 0.0
+    chassis_translation_kd: 0.0

rmcs_ws/src/rmcs_core/CMakeLists.txt

@@ -17,8 +17,8 @@ include(FetchContent)
 set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE)
 FetchContent_Declare(
   librmcs
-  URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.0.0/librmcs-sdk-src-3.0.0.zip
-  URL_HASH SHA256=b39f51c21baacdcbf3f0176119b8850137a108b88a67e12395d37d89e5ef53e8
+  URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0/librmcs-sdk-src-3.1.0.zip
+  URL_HASH SHA256=07107e251745ddb23f7b3e39edec5d6910be1a197025d167ec9849c5c80dd954
   DOWNLOAD_EXTRACT_TIMESTAMP TRUE
 )
 FetchContent_MakeAvailable(librmcs)

rmcs_ws/src/rmcs_core/package.xml

@@ -11,6 +11,7 @@
 
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>pluginlib</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>

rmcs_ws/src/rmcs_core/plugins.xml

@@ -2,6 +2,7 @@
   <class type="rmcs_core::hardware::MecanumHero" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::hardware::SteeringHero" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::hardware::OmniInfantry" base_class_type="rmcs_executor::Component" />
+  <class type="rmcs_core::hardware::Sentry" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::hardware::SteeringInfantry" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::chassis::ChassisController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::chassis::ChassisPowerController" base_class_type="rmcs_executor::Component" />
@@ -10,6 +11,7 @@
   <class type="rmcs_core::controller::gimbal::SimpleGimbalController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::gimbal::HeroGimbalController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::gimbal::DualYawController" base_class_type="rmcs_executor::Component" />
+  <class type="rmcs_core::controller::gimbal::EccentricDualYaw" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::gimbal::PlayerViewer" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::shooting::FrictionWheelController" base_class_type="rmcs_executor::Component" />
   <class type="rmcs_core::controller::shooting::HeatController" base_class_type="rmcs_executor::Component" />

rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp

@@ -145,4 +145,4 @@ class DualYawController
 
 #include <pluginlib/class_list_macros.hpp>
 
-PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::DualYawController, rmcs_executor::Component)
+PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::DualYawController, rmcs_executor::Component)