diff --git a/README.md b/README.md index 9f9fd56..30d4a53 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,100 @@ -# route_TLset +以下是为两个 Python 脚本编写的 `README.md` 文件，包含红绿灯控制脚本 (`cyber_trafficlight.py`) 和车道跟随命令脚本 (`cyber_routing.py`) 的详细说明。 +```markdown +# Apollo 外部命令控制脚本 + +本仓库提供两个用于 Apollo 9.0 仿真环境的 Python 脚本，分别用于手动控制红绿灯状态和发送车道跟随命令，以测试规划与控制模块的响应。 + +## 环境要求 + +- Apollo 9.0 Docker 容器环境（已安装 Cyber RT 及相关模块） +- Python 3.6+ +- 已启动 Dreamview 或相关 Cyber 节点（`Planning`、`Control`、`external_command_processor` 等） + +## 脚本说明 + +### 1. 红绿灯控制脚本 (`cyber_trafficlight.py`) + +**功能** +向 `/apollo/perception/traffic_light` 话题周期性发送红绿灯检测消息，手动切换红绿灯状态，用于仿真中测试车辆在红灯前的停车及绿灯后的起步行为。 + +**消息协议** +- 使用 `modules/common_msgs/perception_msgs/traffic_light_detection.proto` +- 每个 `TrafficLight` 消息包含 `id`（地图中的信号灯 ID）、`color`、`tracking_time` 等字段 +- 支持同时控制多个红绿灯 ID + +**使用方法** +```bash +python cyber_trafficlight.py +``` +运行后按提示输入数字： +- `1` → 将所有指定红绿灯设为红灯，车辆应停车 +- `2` → 设为绿灯，车辆应通行 +- `3` → 退出程序 + +**配置** +在脚本开头的 `SIGNAL_IDS` 列表中，可修改需要控制的红绿灯 ID（必须与地图 `base_map` 中的 `signal.id` 一致）。 + +--- + +### 2. 车道跟随命令脚本 (`cyber_routing.py`) + +**功能** +通过 Cyber 客户端向 `/apollo/external_command/lane_follow` 服务发送 `LaneFollowCommand`，命令车辆从指定起点行驶至终点。不手动设置车辆朝向（heading），由 Apollo 自动匹配最近车道的切线方向。 + +**消息协议** +- 使用 `modules/common_msgs/external_command_msgs/lane_follow_command.proto` +- 必需的字段：`command_id`、`is_start_pose_set`、`way_point`（起点）、`end_pose`（终点） + +**使用方法** +1. 确认 `external_command_processor` 已运行（通常跟随 Planning 模块启动）。 +2. 执行脚本： +```bash +python cyber_routing.py +``` +3. 脚本会输出发送的命令及服务响应状态，若响应 `status=1`（RUNNING），车辆即开始沿车道行驶。 + +**坐标输入** +脚本中使用 `parse_coordinate()` 函数解析形如 `"(x,y)"` 的字符串，可方便地更换起点/终点坐标。示例： +```python +start_x, start_y = parse_coordinate("(587107.2299980448,4141578.018806449)") +end_x, end_y = parse_coordinate("(587010.5463351277,4141603.8543796046)") +``` + +--- + +## 常见问题 + +### 红绿灯脚本发送后车辆无响应 +- 检查话题名称是否正确：`/apollo/perception/traffic_light` +- 确认 Planning/Control 模块已启动，且红绿灯 ID 与地图中的 `signal.id` 完全匹配（注意前缀 `signal_`） +- 查看 Planning 日志中是否有 `stop by TL_xxx` 的决策输出 + +### 车道跟随命令返回 ERROR +- 最常见原因：缺少 `routing_map` 文件。执行以下命令生成（替换为实际地图路径）： + ```bash + ./scripts/generate_routing_topo_graph.sh --map_dir /path/to/map/directory + ``` +- 检查起点/终点坐标是否落在可行驶车道上（可使用 `query_position.py` 工具验证） +- 确认 `external_command_processor` 模块已运行： + ```bash + cyber_channel list | grep external_command + ``` + +### 响应状态码含义 +- `0` → STATUS_UNKNOWN（未知） +- `1` → STATUS_RUNNING（执行中） +- `2` → STATUS_FINISHED（已完成） +- `3` → STATUS_ERROR（失败，查看日志） + +## 日志查看 + +- 红绿灯相关日志：`tail -f /apollo/data/log/planning.INFO` +- 外部命令日志：`tail -f /apollo/data/log/external_command.INFO` + +## 许可证 + +本脚本仅用于 Apollo 仿真学习与测试，遵循 Apollo 项目所使用的 Apache 2.0 许可证。 +``` + +该 README 文件可直接保存为 `README.md`，与两个 Python 脚本放在同一目录下。 diff --git a/cyber_routing.py b/cyber_routing.py new file mode 100644 index 0000000..e0ca78f --- /dev/null +++ b/cyber_routing.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +from cyber.python.cyber_py3 import cyber +from cyber.python.cyber_py3 import cyber_time +from modules.common_msgs.external_command_msgs import lane_follow_command_pb2 +from modules.common_msgs.external_command_msgs import command_status_pb2 +import time +import random + +def parse_coordinate(coord_str): + cleaned = coord_str.strip().strip('()') + parts = cleaned.split(',') + if len(parts) != 2: + raise ValueError(f"Invalid coordinate format: {coord_str}") + x = float(parts[0].strip()) + y = float(parts[1].strip()) + return x, y + +def main(): + cyber.init() + node = cyber.Node("lane_follow_requester") + + cmd = lane_follow_command_pb2.LaneFollowCommand() + cmd.header.timestamp_sec = cyber_time.Time.now().to_sec() + cmd.header.module_name = "manual_command" + cmd.header.sequence_num = 1 + cmd.command_id = random.randint(1, 2**31 - 1) + cmd.is_start_pose_set = True + + # 使用解析函数设置起点和终点坐标 + start_x, start_y = parse_coordinate("(587107.2299980448,4141578.018806449)") + end_x, end_y = parse_coordinate("(587010.5463351277,4141603.8543796046)") + + start_pose = cmd.way_point.add() + start_pose.x = start_x + start_pose.y = start_y + + cmd.end_pose.x = end_x + cmd.end_pose.y = end_y + + client = node.create_client( + '/apollo/external_command/lane_follow', + lane_follow_command_pb2.LaneFollowCommand, + command_status_pb2.CommandStatus + ) + + time.sleep(1.0) + print("Sending LaneFollowCommand:") + print(cmd) + + response = client.send_request(cmd) + if response is not None: + print("Response status:") + print(f" - command_id: {response.command_id}") + print(f" - status: {response.status}") + print(f" - message: {response.message}") + else: + print("No response received. Make sure external_command_processor is running.") + + cyber.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/cyber_trafficlight.py b/cyber_trafficlight.py new file mode 100644 index 0000000..b73ab78 --- /dev/null +++ b/cyber_trafficlight.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +from cyber.python.cyber_py3 import cyber +from cyber.python.cyber_py3 import cyber_time +import modules.common_msgs.perception_msgs.traffic_light_detection_pb2 as traffic_light_detection_pb2 +import threading +import time + +# 需要控制的红绿灯ID列表（必须与地图中的signal.id一致） +SIGNAL_IDS = ["signal_14", "signal_13", "signal_0","signal_9"] + +def add_forward_lights(color, traffic_light_pb, signal_ids): + """为指定的多个红绿灯ID添加相同颜色的消息""" + for sid in signal_ids: + light = traffic_light_pb.traffic_light.add() + light.id = sid + light.color = color + light.tracking_time = 10.0 + +seq_num = 0 + +def add_header(msg): + global seq_num + msg.header.sequence_num = seq_num + msg.header.timestamp_sec = cyber_time.Time.now().to_sec() + msg.header.module_name = "manual_traffic_light" + seq_num = seq_num + 1 + +def pub_func(writer): + while not cyber.is_shutdown(): + global traffic_light_msg + add_header(traffic_light_msg) + writer.write(traffic_light_msg) + time.sleep(0.1) + +traffic_light_msg = None + +if __name__ == '__main__': + traffic_light_msg = traffic_light_detection_pb2.TrafficLightDetection() + cyber.init() + node = cyber.Node("traffic_light_command") + writer = node.create_writer( + "/apollo/perception/traffic_light", + traffic_light_detection_pb2.TrafficLightDetection) + thread = threading.Thread(target=pub_func, args=(writer,)) + thread.start() + + while not cyber.is_shutdown(): + m = input("1: forward red 2: forward green 3: exit\n") + if m == '1': + traffic_light_msg.ClearField('traffic_light') + add_forward_lights(traffic_light_detection_pb2.TrafficLight.RED, + traffic_light_msg, SIGNAL_IDS) + print("Set all signals to RED") + elif m == '2': + traffic_light_msg.ClearField('traffic_light') + add_forward_lights(traffic_light_detection_pb2.TrafficLight.GREEN, + traffic_light_msg, SIGNAL_IDS) + print("Set all signals to GREEN") + elif m == '3': + break + else: + print("Invalid input") + + cyber.shutdown() \ No newline at end of file