危险检测与急停：60° 扇区 / 20cm / 0.5s 判定
N3 安全节点订阅激光雷达 /scan，在前方 60° 扇区（±30°，跨 0° 环绕）内检测 <20cm 障碍， 持续 0.5s 才判危险（防瞬时误报）→ 同一回调内发布 danger=true 并释放 6 舵机力矩（机械臂变软）； 解除后恢复力矩 + go_work 回 work。打断分层：雷达危险 > 语音打断（R9 / R9.1）。
01问题引入：机器人伸手时，人手也在旁边
前三章的任务链路都默认"环境是空的"，但真实工位上，操作者的手随时可能伸进机械臂工作区。 需求 R9 给出精确红线：激光雷达前方 60° 扇区内、障碍距离 <20cm、持续 0.5s 三个条件同时满足即判危险。为什么需要三个条件而不是一个？——扇区限定"正前方"（只管最可能撞人的方向）， 距离阈值是"近到会撞"，持续时间是"确实有人站着"而非手掠过。R9.1 进一步规定： 雷达危险打断 > 语音打断，且危险解除后自动恢复回 work、需重新唤醒下发任务。
02原理通俗讲解：保安的手电筒与"松手"哲学
保安手电只扫正前方（±30°），不回头看背后——雷达前方 60° 扇区同理。算法遍历每个扫描点：
角度归一化到 [0,2π)，判断是否落在 [-30°, +30°] 扇区内（含跨 0° 环绕的"或"分支），
过滤 inf/nan 与 <0.01m 无效点，取扇区内最小距离。min_range < 0.20m 即"有人贴近"。
注意检测基于雷达自身坐标系 laser_link——障碍必须放在雷达正前方，不是机械臂前方。
扫过的手、飞过的纸屑只出现一瞬。_scan_cb 首见障碍记 _danger_since，
仅当 now - _danger_since ≥ persist_time(0.5s) 才 _enter_danger()；
中途消失则清零计时。宁可慢半拍，不可狼来了——误报会让工人对报警脱敏，比晚报更危险。
危险时硬刹车保持姿态，机械臂会与推它的人"较劲"。本项目反着来：同一 scan 回调内直接发布
/car1_base_driver/bus_servo/set_state enable_torque=[0]*6
（ID0-5：5 关节＋1 夹爪，ARM_SERVO_IDS=range(6)，duration=0.2），机械臂瞬间变软、可被推离。
一次 DDS 发布无服务往返：雷达 10Hz 最坏 100ms 检测 + 舵机释放 ~150-220ms。
障碍移开 → 清计时 → _exit_danger()：发 danger=false、status=recover、
enable_torque=[1]*6 恢复力矩，再异步调 /arm_controller/go_work
回 work 安全姿态（顺序不能反：没力矩就动不了）。同时 N-grasp 收 danger 立即 emergency_stop 停抓、
N-conveyor 停带停臂、N2 暂停任务——恢复后需人工重新触发（安全策略）。
03关联知识点
front_start_deg=-30 / front_end_deg=30 / danger_dist=0.20 / persist_time=0.5；
启动日志 front=-30°..30° dist=0.20m persist=0.5s。
/eai_factory/safety/danger（Bool）、/eai_factory/safety/status（safe→danger→recover）、 /car1_base_driver/bus_servo/set_state（力矩发布）。
start_base:=false ... start_radar:=true 只拉 lidar.launch + safety 节点（launch L85-94），
安全模块可独立调试；完整联动时保持 start_base:=true。
/scan 用 Best Effort QoS 订阅（sensor_msgs/LaserScan，约 10Hz）； 力矩真正生效需 car1_base_driver 在跑，回 work 需 arm_controller 在跑。
04实验目标
- 理解扇区检测算法：60° 扇区、跨 0° 环绕判定、20cm 阈值、0.5s 持续计时的防误报原理。
- 掌握危险触发时经 /car1_base_driver/bus_servo/set_state 释放 6 舵机力矩的机制（变软可手推）。
- 掌握解除恢复流程（恢复力矩 → go_work），理解"雷达打断 > 语音打断"的分层设计。
- 完成任务中急停联动验证（grasp/conveyor 订阅 danger 立即停），按 docs/11 验收 TS1~TS5。
05实验环境与准备
- 激光雷达 USB 已接（/dev/lidar → ttyUSB1），发布 /scan 约 10Hz；障碍物用书本/纸盒（安全测试用）。
- 低内存单测启动（只起雷达 + safety）：
ros2 launch eai_factory eai_factory.launch.py \ start_base:=false start_voice:=false start_vision:=false start_grasp:=false \ start_conveyor:=false start_radar:=true
- 确认接口：
ros2 node list有 eai_factory_safety；ros2 topic hz /scan约 10Hz；/eai_factory/safety/status初始为 safe。 - 力矩释放步骤需完整模式（base_driver 在跑）：改
start_voice:=false start_vision:=false start_grasp:=false start_viz:=false完整启动， 并先确认机械臂在安全位置（work 姿态、远离雷达扇区）。 - 预备验证命令（TS5 手动路径，与 N3 自动路径同一话题同一消息）。
06动画讲解：扇区触发与三节点联动
07互动讲解：急停三参数实验台
调整判定参数组合，观察"手掠过"与"真人驻留"两种场景下是否触发危险：
08实操步骤
eai_factory_safety started | front=-30°..30° dist=0.20m persist=0.5s。ros2 topic echo /eai_factory/safety/status # 初始 safe
ros2 topic echo /eai_factory/safety/danger
ros2 topic echo /car1_base_driver/bus_servo/set_statedanger: true、
status: danger、enable_torque: [0,0,0,0,0,0]、日志
DANGER: obstacle 0.20m in front sector；轻推关节确认可被推动。danger: false、status: recover、enable_torque: [1,1,1,1,1,1]、
日志 SAFE: front sector clear — torque restored, go_work；
机臂自动回 work 并重新锁死。再触发→解除一次，验证循环可重复。ros2 launch eai_factory eai_factory.launch.py start_viz:=false
ros2 service call /eai_factory/start_sort std_srvs/srv/Trigger
# 机械臂运动中，雷达正前方放障碍 ≥0.5s
grasp 日志 SAFETY DANGER — stopping grasp；conveyor 日志
safety danger detected — belt + arm stopped；移开后任务需重新触发（安全策略，
非缺陷）。装货/码垛两任务重复一遍。ros2 topic pub -1 /car1_base_driver/bus_servo/set_state car1_base_driver_msgs/msg/SetBusServoState \
"{state: [{target_id: [0,1,2,3,4,5], enable_torque: [0,0,0,0,0,0]}], duration: 0.2}"
# 释放后务必恢复：
ros2 topic pub -1 /car1_base_driver/bus_servo/set_state car1_base_driver_msgs/msg/SetBusServoState \
"{state: [{target_id: [0,1,2,3,4,5], enable_torque: [1,1,1,1,1,1]}], duration: 0.2}"09重点与易错点
| 现象 | 根因 | 处理 |
|---|---|---|
| 放障碍不触发 | 超出 ±30° 扇区 / <10cm 盲区 / 持续 <0.5s | 对准雷达正前方 10-20cm 保持 ≥0.5s |
| /scan 无数据、safety 静默 | 雷达未接或 start_radar 未开 | 查 /dev/lidar（ttyUSB1）；ros2 topic hz /scan |
| 释放后机械臂推不动 | 力矩释放命令未生效（驱动 cmd） | TS5 手动发 enable_torque:[0]*6 验证（释放=0x0B） |
| 解除后不回 work | go_work 服务未就绪 | ros2 service list | grep go_work，确认 arm_controller 在跑 |
| 危险解除后任务不继续 | 安全策略：人工确认后重触发 | 重新调 /eai_factory/start_xxx，不是缺陷 |
| 抓取状态卡 busy | _danger_cb 已置 idle 但残留状态 | /eai_factory/grasp/abort 复位 |
| TS5 手动释放后忘了恢复 | 机械臂保持软连接 | 测完立即发 [1,1,1,1,1,1]；两数组必须同为 6 元素 |
- 评分自测（源讲义 15/30/35/10/10）：原理 15、步骤 30、四步验证 35、TS1~TS5 表 10、思考题 10。
- 安全红线：释放力矩前确认机械臂安全位；触发/解除时手离开机械臂，让 go_work 自主回位。
- 测完
bash scripts/stop_eai_factory.sh清理，防残留进程占用总线舵机。
10总结与思考
方向、距离、时间三重过滤，缺一不可。
无服务往返，最坏 100ms+150-220ms 内机械臂变软。
顺序颠倒则无力回位；状态 safe→danger→recover 闭环。
危险时不强行运动，恢复后人工重新触发。