From 2fb482161a1dad8d10fd93cc9b6310cba1932628 Mon Sep 17 00:00:00 2001 From: 22268 <2226804607@qq.com> Date: Thu, 18 Dec 2025 23:41:09 +0800 Subject: [PATCH 1/2] =?UTF-8?q?=E5=B0=8F=E8=BD=A6=E5=BC=80=E5=A7=8B?= =?UTF-8?q?=E5=89=8D=E8=BF=9B=EF=BC=8C=E5=B9=B6=E4=B8=94=E8=83=BD=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E9=80=9F=E5=BA=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/minicarsim/main.py | 7 ++ src/minicarsim/models/simple_car.xml | 102 +++++++++++++++++++++++++++ 2 files changed, 109 insertions(+) create mode 100644 src/minicarsim/models/simple_car.xml diff --git a/src/minicarsim/main.py b/src/minicarsim/main.py index 7a002bf72e..797db47ffb 100644 --- a/src/minicarsim/main.py +++ b/src/minicarsim/main.py @@ -201,6 +201,8 @@ def run_simulation(self): # 查找车辆的驱动关节 rear_left_idx = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "rear_left_wheel_motor") rear_right_idx = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "rear_right_wheel_motor") + front_left_steer_idx = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "front_left_steering") + front_right_steer_idx = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "front_right_steering") if rear_left_idx >= 0 and rear_right_idx >= 0: print("找到了车辆驱动关节") @@ -213,6 +215,11 @@ def run_simulation(self): self.data.ctrl[rear_left_idx] = 5.0 # 左后轮速度 if rear_right_idx >= 0: self.data.ctrl[rear_right_idx] = 5.0 # 右后轮速度 + # 设置前轮转向 + if front_left_steer_idx >= 0: + self.data.ctrl[front_left_steer_idx] = np.sin(i * 0.05) * 10 # 正弦变化转向 + if front_right_steer_idx >= 0: + self.data.ctrl[front_right_steer_idx] = np.sin(i * 0.05) * 10 # 正弦变化转向 # 执行仿真步长 mujoco.mj_step(self.model, self.data) diff --git a/src/minicarsim/models/simple_car.xml b/src/minicarsim/models/simple_car.xml new file mode 100644 index 0000000000..cb6aa99be0 --- /dev/null +++ b/src/minicarsim/models/simple_car.xml @@ -0,0 +1,102 @@ + + + + \ No newline at end of file From 80a7018267fdbdef89f93c83e50b424a8402c674 Mon Sep 17 00:00:00 2001 From: 22268 <2226804607@qq.com> Date: Fri, 19 Dec 2025 11:42:01 +0800 Subject: [PATCH 2/2] =?UTF-8?q?=E5=B0=8F=E8=BD=A6=E8=83=BD=E5=A4=9F?= =?UTF-8?q?=E8=AF=86=E5=88=AB=E5=91=A8=E5=9B=B4=E7=89=A9=E4=BD=93=E5=B9=B6?= =?UTF-8?q?=E7=94=9F=E6=88=90=E6=95=88=E6=9E=9C=E5=9B=BE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/minicarsim/main.py | 103 ++++++++++++++++++++++++++++++++++++++++- 1 file changed, 101 insertions(+), 2 deletions(-) diff --git a/src/minicarsim/main.py b/src/minicarsim/main.py index 797db47ffb..8b6f3adee6 100644 --- a/src/minicarsim/main.py +++ b/src/minicarsim/main.py @@ -8,6 +8,8 @@ import mujoco from mujoco import viewer import time +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D # -------------------------- 配置参数 -------------------------- # 场景文件路径 @@ -35,6 +37,7 @@ def __init__(self, xml_path, output_dir): os.makedirs(output_dir, exist_ok=True) os.makedirs(f"{output_dir}/lidar", exist_ok=True) os.makedirs(f"{output_dir}/annotations", exist_ok=True) + os.makedirs(f"{output_dir}/visualization", exist_ok=True) # 新增可视化目录 # 加载MuJoCo模型和数据 self.model = mujoco.MjModel.from_xml_path(xml_path) @@ -149,21 +152,36 @@ def detect_objects(self): # 获取物体类型(根据名称) obj_type = "box" + # 获取物体的几何信息用于更好的可视化 + geom_id = self.model.body_geomadr[i] + if geom_id >= 0: + size = self.model.geom_size[geom_id][:3].copy() + else: + size = [0.5, 0.5, 0.5] # 默认大小 + detected_objects.append({ "id": i, "name": body_name, "type": obj_type, "position": pos.tolist(), - "distance": distance + "distance": distance, + "size": size.tolist() }) except ValueError: # 如果找不到车辆,跳过距离检测 + geom_id = self.model.body_geomadr[i] + if geom_id >= 0: + size = self.model.geom_size[geom_id][:3].copy() + else: + size = [0.5, 0.5, 0.5] # 默认大小 + detected_objects.append({ "id": i, "name": body_name, "type": "box", "position": pos.tolist(), - "distance": 0 + "distance": 0, + "size": size.tolist() }) return detected_objects @@ -192,6 +210,84 @@ def save_data(self, lidar_data, annotations): self.frame_count += 1 + def visualize_detection(self, lidar_data, annotations): + """生成物体识别效果图""" + fig = plt.figure(figsize=(12, 10)) + ax = fig.add_subplot(111, projection='3d') + + # 绘制LiDAR点云数据 + if len(lidar_data) > 0: + ax.scatter(lidar_data[:, 0], lidar_data[:, 1], lidar_data[:, 2], + c='blue', s=0.5, alpha=0.6, label='LiDAR点云') + + # 绘制检测到的物体 + colors = ['red', 'green', 'orange', 'purple', 'brown'] + for i, obj in enumerate(annotations['objects']): + pos = np.array(obj['position']) + size = np.array(obj['size']) + + # 绘制物体中心点 + ax.scatter(pos[0], pos[1], pos[2], + c=colors[i % len(colors)], s=100, marker='o', + label=f"{obj['name']}") + + # 绘制物体边界框 + corners = self._generate_bounding_box_corners(pos, size) + self._plot_bounding_box(ax, corners, colors[i % len(colors)]) + + # 尝试绘制小车 + try: + vehicle_pos, _ = self.get_world_pose("vehicle") + ax.scatter(vehicle_pos[0], vehicle_pos[1], vehicle_pos[2], + c='cyan', s=200, marker='s', label='小车') + except ValueError: + # 如果无法获取小车位置,则不绘制 + pass + + ax.set_xlabel('X (m)') + ax.set_ylabel('Y (m)') + ax.set_zlabel('Z (m)') + ax.set_title(f'物体识别效果图 - 帧 {self.frame_count:04d}') + ax.legend() + + # 保存可视化图像 + plt.savefig(f"{self.output_dir}/visualization/frame_{self.frame_count:04d}.png", + dpi=300, bbox_inches='tight') + plt.close() + + print(f"已生成识别效果图: frame_{self.frame_count:04d}.png") + + def _generate_bounding_box_corners(self, position, size): + """生成包围盒的8个顶点""" + x, y, z = position + sx, sy, sz = size + + corners = np.array([ + [x-sx, y-sy, z-sz], [x+sx, y-sy, z-sz], [x+sx, y+sy, z-sz], [x-sx, y+sy, z-sz], # 底面 + [x-sx, y-sy, z+sz], [x+sx, y-sy, z+sz], [x+sx, y+sy, z+sz], [x-sx, y+sy, z+sz] # 顶面 + ]) + return corners + + def _plot_bounding_box(self, ax, corners, color): + """绘制包围盒""" + # 底面和顶面 + for i in range(2): + # 四条边 + ax.plot(corners[i*4:(i+1)*4, 0], corners[i*4:(i+1)*4, 1], corners[i*4:(i+1)*4, 2], + c=color, alpha=0.7) + # 连接首尾 + ax.plot([corners[i*4+3, 0], corners[i*4, 0]], + [corners[i*4+3, 1], corners[i*4, 1]], + [corners[i*4+3, 2], corners[i*4, 2]], + c=color, alpha=0.7) + + # 连接顶面和底面 + for i in range(4): + ax.plot([corners[i, 0], corners[i+4, 0]], + [corners[i, 1], corners[i+4, 1]], + [corners[i, 2], corners[i+4, 2]], + c=color, alpha=0.7) + def run_simulation(self): """运行MuJoCo仿真并生成数据""" print("开始仿真...") @@ -244,6 +340,9 @@ def run_simulation(self): # 保存数据 self.save_data(lidar_data, annotations) + # 生成识别效果图 + self.visualize_detection(lidar_data, annotations) + print(f"已仿真 {i}/{SIMULATION_FRAMES} 帧") # 控制仿真速度以便观察