Initial import of FaRui Campus ADS v3.2

2026-06-05 14:20:30 +08:00
commit 2839d34fdb
6548 changed files with 1335203 additions and 0 deletions
@@ -0,0 +1,124 @@
+#!/usr/bin/env bash
+# 诊断从板 control、system 状态、ECAR CAN driver 和 socketcan 链路。
+
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+WORKSPACE_ROOT="$(cd "${SCRIPT_DIR}/.." && pwd)"
+ROS_SETUP_FILE="${SLAVE_ROS_SETUP_FILE:-/opt/ros/${ROS_DISTRO:-humble}/setup.bash}"
+
+info() { printf '\n\033[0;32m[INFO]\033[0m %s\n' "$*"; }
+warn() { printf '\n\033[1;33m[WARN]\033[0m %s\n' "$*"; }
+run() {
+  printf '\n$ %s\n' "$*"
+  "$@" || true
+}
+
+echo_once() {
+  local topic="$1"
+  local timeout_sec="${2:-2}"
+
+  printf '\n$ timeout %ss ros2 topic echo --once %s\n' "${timeout_sec}" "${topic}"
+  timeout "${timeout_sec}"s ros2 topic echo --once "${topic}" || true
+}
+
+set +u
+if [[ -f "${ROS_SETUP_FILE}" ]]; then
+  source "${ROS_SETUP_FILE}"
+fi
+if [[ -f "${WORKSPACE_ROOT}/install/setup.bash" ]]; then
+  source "${WORKSPACE_ROOT}/install/setup.bash"
+else
+  warn "Workspace setup not found: ${WORKSPACE_ROOT}/install/setup.bash"
+fi
+set -u
+
+info "环境变量"
+printf 'ROS_DOMAIN_ID=%s\n' "${ROS_DOMAIN_ID:-}"
+printf 'RMW_IMPLEMENTATION=%s\n' "${RMW_IMPLEMENTATION:-}"
+printf 'CYCLONEDDS_URI=%s\n' "${CYCLONEDDS_URI:-}"
+
+info "相关节点"
+run ros2 node list
+
+info "Control/system 服务"
+run ros2 service list
+run ros2 service type /control/control_mode_request
+run ros2 service type /system/operation_mode/change_autoware_control
+run ros2 service type /system/operation_mode/change_operation_mode
+
+info "Topic 端点检查"
+topics=(
+  /socket_can/from_can_bus
+  /socket_can/to_can_bus
+  /vehicle/status/control_mode
+  /vehicle/status/velocity_status
+  /vehicle/status/steering_status
+  /vehicle/status/gear_status
+  /vehicle/status/turn_indicators_status
+  /vehicle/status/hazard_lights_status
+  /vehicle/status/actuation_status
+  /vehicle/status/steering_wheel_status
+  /vehicle/status/door_status
+  /localization/slip_angle
+  /sensing/vehicle_velocity_converter/twist_with_covariance
+  /planning/scenario_planning/trajectory
+  /control/trajectory_follower/control_cmd
+  /control/command/control_cmd
+  /control/command/gear_cmd
+  /control/vehicle_cmd_gate/operation_mode
+  /system/operation_mode/state
+  /system/operation_mode/availability
+  /system/fail_safe/mrm_state
+  /system/emergency/hazard_status
+  /ecar_can_sender/command/adcu_drive_cmd
+  /ecar_can_sender/command/adcu_brake_cmd
+  /ecar_can_sender/command/adcu_steer_cmd
+)
+
+for topic in "${topics[@]}"; do
+  run ros2 topic info -v "${topic}"
+done
+
+info "单次消息采样"
+echo_once /vehicle/status/control_mode 2
+echo_once /vehicle/status/velocity_status 2
+echo_once /vehicle/status/steering_status 2
+echo_once /vehicle/status/gear_status 2
+echo_once /vehicle/status/actuation_status 2
+echo_once /vehicle/status/steering_wheel_status 2
+echo_once /localization/slip_angle 2
+echo_once /control/vehicle_cmd_gate/operation_mode 2
+echo_once /system/operation_mode/state 2
+echo_once /system/operation_mode/availability 2
+echo_once /system/fail_safe/mrm_state 2
+echo_once /system/emergency/hazard_status 2
+echo_once /control/command/control_cmd 2
+echo_once /ecar_can_sender/command/adcu_drive_cmd 2
+echo_once /socket_can/to_can_bus 2
+
+info "结果解读"
+cat <<'EOF'
+从板状态链路:
+  /socket_can/from_can_bus
+    -> /ecar_can_receiver
+    -> /vehicle/status/control_mode、/vehicle/status/velocity_status 等 Autoware 车辆状态
+    -> /autoware_operation_mode_transition_manager + /vehicle_cmd_gate
+    -> /system/operation_mode/state
+    -> /system/operation_mode/availability、/system/fail_safe/mrm_state
+    -> 主板 planning scenario_selector 和 velocity_smoother
+
+控制命令链路:
+  /planning/scenario_planning/trajectory
+    -> /control/trajectory_follower/control_cmd
+    -> /control/command/control_cmd
+    -> /ecar_can_sender/command/adcu_drive_cmd
+    -> /socket_can/to_can_bus
+
+如果 /vehicle/status/control_mode 或 /control/vehicle_cmd_gate/operation_mode 缺失，
+/system/operation_mode/state 就可能不发布。主板 planning 会卡在
+"Waiting for operation mode state"，最终不输出 trajectory。
+
+如果 /control/command/control_cmd 有消息，但 /ecar_can_sender/command/adcu_drive_cmd
+不变化，说明驱动命令输入或命令超时路径仍然有问题。
+EOF