Skip to content

Commit 992a9f3

Browse files
explorer1009explorer1009
authored
Drone 233 (#3055)
* 完成选题 * 给代码添加注释 * 优化airsim控制代码 * 优化车辆控制代码提高可读性 * Update hello_car.py * 实现在pycharm和airsim的连接 * 删除多余文件 * 1 * 按要求修改 * 按要求修改 * Update README.md * 实现无人机定点飞行 * 配置setitngs文件打开激光雷达 * 激光雷达避障 * 更新settings文件 * Update README.md * 规范readme文件 * Update Radar_obstacle_avoidance.py * 添加3D点云生成功能 * Update 3D_point_cloud.py * 点云坐标系修正
1 parent c1dd3f1 commit 992a9f3

File tree

2 files changed

+91
-55
lines changed

2 files changed

+91
-55
lines changed

‎src/airsim_control/3D_point_cloud.py‎

Lines changed: 90 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -4,121 +4,159 @@
44
import time
55
import os
66

7-
# --- 配置 ---
7+
# --- 1. 配置参数 ---
88
VEHICLE_NAME = "Drone_1"
99
LIDAR_NAME = "lidar_1"
10-
H_SPEED = 2.0 # 水平移动速度
11-
V_SPEED = 1.0 # 垂直移动速度
12-
YAW_SPEED = 30.0 # 旋转速度
10+
H_SPEED = 2.0
11+
V_SPEED = 1.0
12+
YAW_SPEED = 30.0
1313

14-
# 保存路径
15-
OUTPUT_FILE = r"D:\Others\map_output.asc"
14+
# 最小记录阈值 防止悬停时产生大量冗余数据
15+
MIN_MOVE_DIST = 0.05 # 至少移动 5厘米 才记录
16+
MIN_ROT_ANGLE = 1.0 # 至少旋转 1度 才记录
17+
18+
OUTPUT_FILE = r"D:\Others\map_output_pro.asc"
1619

1720
# 检查目录
1821
output_dir = os.path.dirname(OUTPUT_FILE)
1922
if not os.path.exists(output_dir):
20-
print(f"错误: 找不到文件夹 '{output_dir}'，请先创建。")
23+
print(f"错误: 找不到文件夹 '{output_dir}'")
2124
exit()
2225

2326

24-
# --- 数学工具 ---
27+
# --- 2. 数学工具 ---
2528
def get_rotation_matrix(q):
2629
w, x, y, z = q.w_val, q.x_val, q.y_val, q.z_val
30+
# 归一化四元数 (防止数值漂移)
31+
norm = np.sqrt(w * w + x * x + y * y + z * z)
32+
w, x, y, z = w / norm, x / norm, y / norm, z / norm
33+
2734
return np.array([
2835
[1 - 2 * y * y - 2 * z * z, 2 * x * y - 2 * z * w, 2 * x * z + 2 * y * w],
2936
[2 * x * y + 2 * z * w, 1 - 2 * x * x - 2 * z * z, 2 * y * z - 2 * x * w],
3037
[2 * x * z - 2 * y * w, 2 * y * z + 2 * x * w, 1 - 2 * x * x - 2 * y * y]
3138
])
3239

3340

34-
# --- 初始化 ---
41+
def calc_distance(p1, p2):
42+
return np.sqrt((p1.x_val - p2.x_val) ** 2 + (p1.y_val - p2.y_val) ** 2 + (p1.z_val - p2.z_val) ** 2)
43+
44+
45+
# --- 3. 初始化 ---
3546
client = airsim.MultirotorClient()
3647
client.confirmConnection()
3748
client.enableApiControl(True, vehicle_name=VEHICLE_NAME)
3849
client.armDisarm(True, vehicle_name=VEHICLE_NAME)
3950
client.takeoffAsync(vehicle_name=VEHICLE_NAME).join()
4051
client.moveToPositionAsync(0, 0, -2, 3, vehicle_name=VEHICLE_NAME).join()
4152

42-
print("\n3D 扫描系统 手动控制版 ")
43-
print("飞行控制: [WASD]移动 [QE]旋转 [↑↓]升降")
44-
print("扫描开关: 按 [R] 键开启/停止录制")
45-
print(f"文件路径: {OUTPUT_FILE}")
53+
print("\n=== 3D 扫描系统 (专业版) ===")
54+
print("控制: [WASD]移动 [QE]旋转 [↑↓]升降")
55+
print("开关: [R] 键")
56+
print("优化: 已启用坐标修正(ENU)与智能去重")
4657

47-
# 清空/初始化文件
4858
with open(OUTPUT_FILE, "w") as f:
4959
f.write("")
5060

5161
try:
52-
total_points_captured = 0
62+
total_points = 0
5363
last_save_time = time.time()
5464
points_buffer = []
65+
is_recording = False
5566

56-
# --- 新增：扫描状态标记 ---
57-
is_scanning = False
67+
# 用于智能去重的变量
68+
last_rec_pos = airsim.Vector3r(0, 0, 0)
69+
last_rec_yaw = 0.0
5870

5971
while True:
60-
# --- 1. 监听开关按键 [R] ---
72+
# --- 开关逻辑 ---
6173
if keyboard.is_pressed('r'):
62-
is_scanning = not is_scanning # 切换状态
63-
if is_scanning:
64-
print(f"\n>>>开始录制数据... (当前总点数: {total_points_captured})")
74+
is_recording = not is_recording
75+
if is_recording:
76+
# 开启瞬间重置一下记录位置，确保能立刻记录
77+
last_rec_pos = airsim.Vector3r(999, 999, 999)
78+
print(f"\n>>> 录制开始...")
6579
else:
66-
print(f"\n>>>暂停录制数据。")
67-
68-
time.sleep(0.3) # 简单的防抖动，防止按一下触发多次
80+
print(f"\n>>> 录制暂停。")
81+
time.sleep(0.3)
6982

70-
# --- 2. 仅在开启状态下处理数据 ---
71-
if is_scanning:
72-
# 获取位姿
83+
# --- 录制逻辑 ---
84+
if is_recording:
85+
# 1. 获取状态
7386
state = client.simGetVehiclePose(vehicle_name=VEHICLE_NAME)
7487
pos = state.position
7588
orientation = state.orientation
7689

77-
# 获取雷达
78-
lidar_data = client.getLidarData(lidar_name=LIDAR_NAME, vehicle_name=VEHICLE_NAME)
90+
# 计算当前偏航角 (简单估算)
91+
current_yaw = airsim.to_eularian_angles(orientation)[2]
92+
93+
# 2. 智能去重判断 (核心优化)
94+
# 只有当位置移动超过阈值，或者旋转超过阈值时，才处理数据
95+
dist_moved = calc_distance(pos, last_rec_pos)
96+
rot_moved = abs(current_yaw - last_rec_yaw)
97+
98+
if dist_moved > MIN_MOVE_DIST or rot_moved > np.deg2rad(MIN_ROT_ANGLE):
99+
100+
# 更新上一次记录的位置
101+
last_rec_pos = pos
102+
last_rec_yaw = current_yaw
103+
104+
# 获取雷达
105+
lidar_data = client.getLidarData(lidar_name=LIDAR_NAME, vehicle_name=VEHICLE_NAME)
79106

80-
if lidar_data and len(lidar_data.point_cloud) >= 3:
81-
raw_points = np.array(lidar_data.point_cloud, dtype=np.float32)
82-
local_points = np.reshape(raw_points, (int(raw_points.shape[0] / 3), 3))
107+
if lidar_data and len(lidar_data.point_cloud) >= 3:
108+
raw_points = np.array(lidar_data.point_cloud, dtype=np.float32)
109+
local_points = np.reshape(raw_points, (int(raw_points.shape[0] / 3), 3))
83110

84-
# 坐标转换
85-
R = get_rotation_matrix(orientation)
86-
rotated_points = np.dot(local_points, R.T)
87-
t_vec = np.array([pos.x_val, pos.y_val, pos.z_val])
88-
global_points = rotated_points + t_vec
111+
# 坐标转换
112+
R = get_rotation_matrix(orientation)
113+
rotated_points = np.dot(local_points, R.T)
114+
t_vec = np.array([pos.x_val, pos.y_val, pos.z_val])
115+
global_points = rotated_points + t_vec
89116

90-
points_buffer.extend(global_points)
91-
total_points_captured += len(global_points)
117+
# --- 3. 坐标系修正 (NED -> ENU) ---
118+
# AirSim Z是向下，我们把它取反，变成向上，这样在软件里看房子就是正的了
119+
global_points[:, 2] = -global_points[:, 2]
92120

93-
# 写入文件 (每0.5秒)
121+
# --- 4. 增加强度信息 (Intensity) ---
122+
# 我们用高度(Z)作为第4列强度值，这样导入时可以直接按颜色显示
123+
# 也可以用距离作为强度: intensity = np.linalg.norm(local_points, axis=1)
124+
intensity = global_points[:, 2]
125+
126+
# 拼接数据: X Y Z I
127+
data_to_save = np.column_stack((global_points, intensity))
128+
129+
points_buffer.extend(data_to_save)
130+
total_points += len(global_points)
131+
132+
# 写入硬盘
94133
if time.time() - last_save_time > 0.5:
95134
if points_buffer:
96135
with open(OUTPUT_FILE, "a") as f:
97136
for p in points_buffer:
98-
f.write(f"{p[0]:.4f} {p[1]:.4f} {p[2]:.4f}\n")
137+
# 保存4位小数，第4列是强度
138+
f.write(f"{p[0]:.3f} {p[1]:.3f} {p[2]:.3f} {p[3]:.3f}\n")
99139

100-
# 使用 \r 动态刷新同一行，不刷屏
101-
print(f"\r[录制中] 已采集: {total_points_captured} 点 | 正在写入...", end="")
140+
print(f"\r[REC] 点数: {total_points} | 状态: 移动扫描中...", end="")
102141
points_buffer = []
103142
last_save_time = time.time()
143+
else:
144+
# 如果没有新数据(因为没移动)，打印静止状态
145+
print(f"\r[REC] 点数: {total_points} | 状态: 等待移动... ", end="")
146+
104147
else:
105-
# 暂停状态下，稍微sleep一下减少CPU占用，且不打印刷屏
106148
time.sleep(0.05)
107149

108-
# --- 3. 飞行控制 (始终有效) ---
150+
# --- 飞行控制 ---
109151
vx, vy, vz, yaw_rate = 0.0, 0.0, 0.0, 0.0
110-
111152
if keyboard.is_pressed('w'): vx = H_SPEED
112153
if keyboard.is_pressed('s'): vx = -H_SPEED
113154
if keyboard.is_pressed('a'): vy = -H_SPEED
114155
if keyboard.is_pressed('d'): vy = H_SPEED
115-
116-
if keyboard.is_pressed('up'): vz = -V_SPEED
156+
if keyboard.is_pressed('up'): vz = -V_SPEED # 注意：AirSim里负数是向上，但我们上面保存时已经反转了Z，这里控制依然遵循AirSim逻辑
117157
if keyboard.is_pressed('down'): vz = V_SPEED
118-
119158
if keyboard.is_pressed('q'): yaw_rate = -YAW_SPEED
120159
if keyboard.is_pressed('e'): yaw_rate = YAW_SPEED
121-
122160
if keyboard.is_pressed('esc'): break
123161

124162
# 发送控制指令
@@ -136,8 +174,6 @@ def get_rotation_matrix(q):
136174
if points_buffer:
137175
with open(OUTPUT_FILE, "a") as f:
138176
for p in points_buffer:
139-
f.write(f"{p[0]:.4f} {p[1]:.4f} {p[2]:.4f}\n")
140-
141-
print(f"\n\n任务结束！最终点数: {total_points_captured}")
142-
print(f"结果已保存: {OUTPUT_FILE}")
177+
f.write(f"{p[0]:.3f} {p[1]:.3f} {p[2]:.3f} {p[3]:.3f}\n")
178+
print(f"\n结束。文件: {OUTPUT_FILE}")
143179
client.reset()

‎src/airsim_control/settings.json‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@
1414
"Enabled": true,
1515
"Range": 100,
1616
"NumberOfChannels": 16,
17-
"PointsPerSecond": 50000,
17+
"PointsPerSecond": 15000,
1818
"VerticalFOVUpper": 10,
1919
"VerticalFOVLower": -15,
2020
"HorizontalFOVStart": -90,

0 commit comments

Comments
 (0)