Skill: 33-code/radar-collision-warning (version 1.0.0)

--- name: radar-collision-warning description: 雷达防撞预警 — 调用树莓派上 ROS2 节点的雷达预警服务。当用户说"雷达防撞预警"、"开启雷达监控"、"检查障碍物"等时使用。通过 rosbridge WebSocket 向树莓派发送 ROS2 Service 请求，获取最近障碍物距离并给出预警。依赖 Node.js 18+（内置 WebSocket，无需安装任何包）。 metadata: {"openclaw": {"emoji": "📡", "requires": {"bins": ["node"]}}} --- # 雷达防撞预警 Skill ## 配置 在 `~/.openclaw/workspace/TOOLS.md` 中添加树莓派的 IP 地址： ```markdown ### 雷达预警 - 树莓派 IP: <你的树莓派IP> - rosbridge 端口: 8080（默认） ``` 或通过环境变量传入： ```bash export RADAR_HOST=192.168.137.100 # 树莓派 IP export RADAR_PORT=8080 # rosbridge 端口（默认） ``` ## 架构概览 ``` OpenClaw (本机) └── skill: call_radar_service.js └── rosbridge WebSocket (ws://<树莓派IP>:8080) └── 树莓派 rosbridge_server └── ROS2 Service: /radar_collision_warning (std_srvs/Trigger) └── radar_collision_warning_node └── 订阅 /scan (LaserScan) ← 乐动激光雷达 ``` **rosbridge WebSocket 端口**: `8080` **ROS2 Service 名称**: `/radar_collision_warning` **碰撞阈值**: 默认 0.05m（5cm），调用时可用 `-t/--threshold` 覆盖 ## 使用方式 **依赖**: Node.js 18+（`globalThis.WebSocket` 为内置，无需安装任何包） ### 单次查询 当用户说"雷达防撞预警"、"检查障碍物距离"、"开启雷达监控"时，调用： ```bash node ~/.openclaw/workspace/skills/radar-collision-warning/scripts/call_radar_service.js ``` 可加参数覆盖阈值（单位：米）： ```bash node ~/.openclaw/workspace/skills/radar-collision-warning/scripts/call_radar_service.js --threshold 0.10 ``` ### 持续监控模式 当用户要求"持续监控"、"实时雷达监控"时，在 **后台** 启动轮询： ```bash node ~/.openclaw/workspace/skills/radar-collision-warning/scripts/call_radar_service.js --monitor --interval 2.0 ``` 使用 `exec` 工具的 `background:true` 模式运行，后台运行时按 Ctrl+C 或发信号停止。 ## 返回格式（JSON stdout） ```json { "success": true, "distance_m": 0.032, "distance_cm": 3.2, "warning": true, "message": "⚠️ 碰撞危险！距离仅 3.2cm", "threshold_m": 0.05 } ``` 或安全状态： ```json { "success": true, "distance_m": 1.234, "distance_cm": 123.4, "warning": false, "message": "✅ 安全，最近障碍物距离 1.23m", "threshold_m": 0.05 } ``` 连接失败时： ```json { "success": false, "error": "无法连接到 rosbridge 服务器", "message": "⚠️ 雷达服务不可用，请检查树莓派是否开机" } ``` ## 树莓派侧准备（首次配置时参考） ### 1. 安装 rosbridge ```bash sudo apt install ros-jazzy-rosbridge-suite ``` ### 2. ROS2 包配置 在树莓派上创建雷达预警包： ```bash cd ~/ros_ws/src ros2 pkg create radar_pkg --build-type ament_python # 将 radar_collision_warning_node.py 复制到: # ~/ros_ws/src/radar_pkg/radar_pkg/ # 添加 setup.py 入口点: # 'radar_collision_warning_node = radar_pkg.radar_collision_warning_node:main' cd ~/ros_ws && colcon build && source install/setup.bash ``` ### 3. 一键启动 ```bash bash ~/ros_ws/start_radar_rosbridge.sh ``` ## 返回值解读 - `warning: true` → 距离 < 阈值，应提醒用户"注意防撞" - `warning: false` → 距离正常 - `success: false` → 连接失败或服务异常

path: references/radar_collision_warning_node.py
content: #!/usr/bin/env python3
"""
雷达防撞预警节点 — ROS2 Python 节点

订阅 /scan (LaserScan)，计算最近有效距离，通过 Service 供外部调用。

使用方法：
  # 赋予执行权限
  chmod +x radar_collision_warning_node.py
  
  # 运行节点
  ros2 run radar_collision_warning radar_collision_warning_node

Service:
  名称: /radar_collision_warning
  类型: std_srvs/srv/Trigger
  请求: 空（Trigger 无参数）
  响应:
    success: bool   — true=危险, false=安全
    message: str    — 中文描述信息

话题:
  订阅: /scan (sensor_msgs/LaserScan) — 激光雷达数据
  发布（预留）: /flight_control_cmd (std_msgs/String) — 飞控指令
"""

import math
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from std_srvs.srv import Trigger
from std_msgs.msg import String


class RadarCollisionWarning(Node):
    def __init__(self):
        super().__init__('radar_collision_warning_node')

        # ========== 参数 ==========
        self.declare_parameter('collision_threshold', 0.05)  # 5cm
        self.collision_threshold = self.get_parameter('collision_threshold').value
        # ==========================

        # 订阅激光雷达 /scan 话题
        self.scan_sub = self.create_subscription(
            LaserScan,
            '/scan',
            self._scan_callback,
            qos_profile=10
        )

        # 当前最近有效距离（米）和对应角度
        self.latest_min_distance = float('inf')
        self.latest_min_angle = 0.0

        # Service 调用计数（用于调试）
        self._call_count = 0

        # 创建 ROS2 Service（供外部 WebSocket/rosbridge 调用）
        self.warning_service = self.create_service(
            Trigger,
            'radar_collision_warning',   # → 全局名 /radar_collision_warning
            self._service_callback
        )

        # 预留：飞控指令发布器
        self.flight_pub = self.create_publisher(String, 'flight_control_cmd', qos_profile=10)

        self.get_logger().info(
            f"雷达防撞预警节点已启动，碰撞阈值: {self.collision_threshold * 100:.1f}cm"
        )

    # ──────────────────────────────────────────────
    def _scan_callback(self, msg: LaserScan):
        """ LaserScan 回调：过滤无效值，找到最近有效距离及角度 """
        range_min = msg.range_min
        range_max = msg.range_max

        # 角度分辨率
        angle_increment = msg.angle_increment
        angle_min = msg.angle_min

        min_dist = float('inf')
        min_angle = 0.0

        for i, r in enumerate(msg.ranges):
            # 过滤: inf, nan, 超出物理范围
            if math.isfinite(r) and range_min <= r <= range_max:
                if r < min_dist:
                    min_dist = r
                    # 计算该点的角度（弧度转度）
                    min_angle = math.degrees(angle_min + i * angle_increment)

        self.latest_min_distance = min_dist
        self.latest_min_angle = min_angle

        # DEBUG: 每次 scan 回调打印一次
        if min_dist < float('inf'):
            self.get_logger().debug(
                f"scan 回调: 最近障碍物 {min_dist*100:.1f}cm @ {min_angle:.1f}°"
            )
    # ──────────────────────────────────────────────

    # ──────────────────────────────────────────────
    def _service_callback(self, request, response):
        """ Service 回调：被外部调用时返回当前预警状态 """
        self._call_count += 1
        dist = self.latest_min_distance
        threshold = self.collision_threshold

        if dist < threshold:
            response.success = True
            response.message = (
                f"⚠️ 碰撞危险！距离: {dist*100:.1f}cm "
                f"@ {self.latest_min_angle:.1f}° "
                f"(< {threshold*100:.1f}cm)"
            )

            # 预留：发布飞控避障指令（如"stop"）
            # cmd = String()
            # cmd.data = "stop"
            # self.flight_pub.publish(cmd)

            self.get_logger().warn(
                f"[#{self._call_count}] {response.message} | "
                f"scan累积最小: {dist*100:.1f}cm"
            )
        else:
            response.success = False
            if dist < float('inf'):
                response.message = f"✅ 安全，最近障碍物: {dist*100:.1f}cm @ {self.latest_min_angle:.1f}°"
            else:
                response.message = "⚠️ 尚未收到有效 scan 数据"
            self.get_logger().info(f"[#{self._call_count}] {response.message}")

        return response
    # ──────────────────────────────────────────────


def main(args=None):
    rclpy.init(args=args)
    node = RadarCollisionWarning()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()


path: references/start_radar_rosbridge.sh
content: #!/bin/bash
# 树莓派 ROS2 雷达预警 + rosbridge 启动脚本
# 用法: bash start_radar_rosbridge.sh
# 确保已复制 radar_collision_warning_node.py 到 ros_ws/src/radar_pkg/src/

set -e

ROS_WS=~/ros_ws
LIDAR_PKG_NAME=ldlidar
RADAR_PKG_NAME=radar_pkg
RADAR_NODE_NAME=radar_collision_warning_node

echo "🔹 加载 ROS2 Jazzy 环境..."
source /opt/ros/jazzy/setup.bash
source $ROS_WS/install/setup.bash

echo "🔹 检查雷达预警节点包..."
if [ ! -d "$ROS_WS/src/$RADAR_PKG_NAME/src/$RADAR_NODE_NAME.py" ] && \
   [ ! -f "$ROS_WS/src/$RADAR_PKG_NAME/src/radar_collision_warning_node.py" ]; then
  echo "⚠️  未找到雷达预警节点，请先将 references/radar_collision_warning_node.py 复制过来"
  echo "    cp references/radar_collision_warning_node.py ~/ros_ws/src/$RADAR_PKG_NAME/src/"
fi

echo "🔹 启动 rosbridge_server（WebSocket :8080）..."
pkill -f rosbridge_websocket || true
ros2 launch rosbridge_server rosbridge_websocket_launch.py \
  > $ROS_WS/rosbridge.log 2>&1 &
echo "✅ rosbridge 已启动，PID=$! 日志: tail -f $ROS_WS/rosbridge.log"

echo "🔹 启动激光雷达节点..."
pkill -f "$LIDAR_PKG_NAME" || true
ros2 run $LIDAR_PKG_NAME $LIDAR_PKG_NAME \
  > $ROS_WS/ldlidar.log 2>&1 &
echo "✅ 激光雷达已启动，PID=$!"

echo "🔹 启动雷达防撞预警节点..."
pkill -f "$RADAR_NODE_NAME" || true
ros2 run $RADAR_PKG_NAME $RADAR_NODE_NAME \
  > $ROS_WS/radar_node.log 2>&1 &
echo "✅ 雷达预警节点已启动，PID=$!"

echo ""
echo "🎉 服务启动完成！"
echo "  rosbridge: ws://$(hostname -I | awk '{print $1}'):8080"
echo "  Service:  /radar_collision_warning"
echo "  Topic:    /scan"
echo ""
echo "🛑 停止: pkill -f rosbridge_websocket && pkill -f $LIDAR_PKG_NAME && pkill -f $RADAR_NODE_NAME"


path: scripts/call_radar_service.js
content: #!/usr/bin/env node
/**
 * 雷达防撞预警 — 通过 rosbridge WebSocket 调用 ROS2 Service
 *
 * 架构:
 *   本机 -> ws://192.168.137.100:8080 (rosbridge_server)
 *   -> ROS2 Service: /radar_collision_warning (std_srvs/srv/Trigger)
 *
 * 用法:
 *   node call_radar_service.js              # 单次查询
 *   node call_radar_service.js --monitor     # 持续监控（Ctrl+C 停止）
 *   node call_radar_service.js -t 0.10      # 自定义阈值（米）
 *   node call_radar_service.js -i 2.0       # 监控间隔（秒）
 *
 * 依赖: Node.js 18+ 内置 WebSocket（globalThis.WebSocket），无需安装任何包
 */

'use strict';

const WS = globalThis.WebSocket;

// ============ 配置 ============
const ROSBRIDGE_HOST = process.env.RADAR_HOST || '192.168.137.100';
const ROSBRIDGE_PORT = process.env.RADAR_PORT  || '8080';
const SERVICE_NAME   = '/radar_collision_warning';
const DEFAULT_THRESHOLD = 0.05; // 5cm
// ==============================

// ── 命令行参数解析 ──
const args     = process.argv.slice(2);
const isMonitor = args.includes('--monitor') || args.includes('-m');
let threshold   = DEFAULT_THRESHOLD;
let interval    = 1.0;

for (let i = 0; i < args.length; i++) {
  if ((args[i] === '--threshold' || args[i] === '-t') && args[i + 1]) {
    threshold = parseFloat(args[i + 1]); i++;
  }
  if ((args[i] === '--interval' || args[i] === '-i') && args[i + 1]) {
    interval = parseFloat(args[i + 1]); i++;
  }
}

const wsUrl = `ws://${ROSBRIDGE_HOST}:${ROSBRIDGE_PORT}`;

// ── 调用 ROS2 Service，返回 Promise ──
function callRadarService() {
  return new Promise((resolve) => {
    let ws;
    let done = false;
    let timer;

    const finish = (result) => {
      if (done) return;
      done = true;
      if (timer) clearTimeout(timer);
      if (ws) try { ws.close(); } catch (_) {}
      resolve(result);
    };

    timer = setTimeout(() => finish({
      success: false,
      error: `连接 rosbridge 超时（${ROSBRIDGE_HOST}:${ROSBRIDGE_PORT}）`,
      message: '⚠️ 连接树莓派雷达服务超时，请检查树莓派是否在线且 rosbridge 已启动'
    }), 5000);

    try {
      ws = new WS(wsUrl);
    } catch (e) {
      finish({ success: false, error: `WebSocket 创建失败: ${e.message}`, message: '⚠️ 无法创建连接' });
      return;
    }

    ws.onopen = () => {
      const callId = `c${Date.now()}${Math.random().toString(36).slice(2, 6)}`;
      // rosbridge JSON-RPC 2.0 — call_service
      ws.send(JSON.stringify({
        op: 'call_service',
        id: callId,
        service: SERVICE_NAME,
        args: {},
        type: 'std_srvs/srv/Trigger'
      }));

      // 监听本轮响应
      ws.addEventListener('message', function handler(ev) {
        try {
          const msg = JSON.parse(ev.data);
          // 匹配我们的 callId
          if (msg.id !== callId) return;
          ws.removeEventListener('message', handler);

          let success = false, message = '';
          if (msg.values) {
            success = msg.values.success ?? false;
            message  = msg.values.message  ?? '';
          } else if (msg.result !== undefined) {
            const v = msg.result ?? {};
            success = v.success ?? false;
            message  = v.message  ?? '';
          }

          // 从 message 中同时匹配 cm 和 m 单位
          const dmCm = message.match(/([\d.]+)\s*cm/i);
          const dmM  = message.match(/([\d.]+)\s*m\b/i);
          let distM;
          if (dmCm) {
            distM = parseFloat(dmCm[1]) / 100;  // cm → m
          } else if (dmM) {
            distM = parseFloat(dmM[1]);
          } else {
            distM = threshold;  // 解析失败用阈值
          }

          finish({
            success: true,
            distance_m: distM,
            distance_cm: parseFloat((distM * 100).toFixed(1)),
            warning: distM < threshold,
            message,
            threshold_m: threshold,
            service_response_success: success
          });
        } catch (_) {}
      });
    };

    ws.onerror = () => finish({
      success: false,
      error: 'WebSocket 连接失败',
      message: '⚠️ 连接树莓派失败，请检查网络和 rosbridge 服务状态'
    });

    ws.onclose = () => {
      if (!done) finish({
        success: false,
        error: 'WebSocket 连接关闭',
        message: '⚠️ 连接意外中断'
      });
    };
  });
}

// ── 持续监控模式 ──
async function monitorMode() {
  console.log(`[雷达监控] 阈值=${(threshold * 100).toFixed(0)}cm | 间隔=${interval}s | Ctrl+C 停止\n`);
  let consecutive = 0, lastWarned = false;

  while (true) {
    const r = await callRadarService();
    const now = new Date().toLocaleTimeString('zh-CN', { hour12: false });

    if (r.success) {
      if (r.warning) {
        consecutive++;
        if (!lastWarned || consecutive % 5 === 1) {
          console.log(
            `🚨 [${now}] ⚠️ 碰撞危险！距离: ${r.distance_cm}cm (< ${(threshold * 100).toFixed(0)}cm) | 连续: ${consecutive}次`
          );
        }
        lastWarned = true;
      } else {
        consecutive = 0;
        console.log(`✅ [${now}] 安全 | 距离: ${r.distance_cm}cm`);
        lastWarned = false;
      }
    } else {
      consecutive = 0;
      console.log(`❌ [${now}] ${r.message}`);
      lastWarned = false;
    }

    await new Promise(r => setTimeout(r, interval * 1000));
  }
}

// ── 主入口 ──
async function main() {
  if (isMonitor) {
    await monitorMode();
  } else {
    const r = await callRadarService();
    process.stdout.write(JSON.stringify(r, null, 2) + '\n');
  }
}

main().catch(e => {
  console.error(JSON.stringify({ success: false, error: e.message }));
  process.exit(1);
});


Security scan verdicts — ClawScan: clean | static analysis: clean | VirusTotal: clean | SkillSpector: suspicious (HIGH)