Skip to content
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
151 changes: 101 additions & 50 deletions src/minicarsim/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -68,13 +68,23 @@ def generate_realistic_lidar_data(self):
try:
# 获取车辆位置和朝向
vehicle_pos, vehicle_quat = self.get_world_pose("vehicle")

# 获取LiDAR传感器的位置和朝向
lidar_site_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "lidar_site")
if lidar_site_id >= 0:
lidar_pos = self.data.site_xpos[lidar_site_id].copy()
# 获取LiDAR的旋转矩阵
lidar_mat = self.data.site_xmat[lidar_site_id].reshape(3, 3)
else:
# 如果找不到LiDAR站点,使用默认位置
lidar_offset = np.array(LIDAR_PARAMS["pos"])
lidar_pos = vehicle_pos + lidar_offset
lidar_mat = np.eye(3) # 单位矩阵表示无旋转
except ValueError:
vehicle_pos = np.array([0, 0, 0.5])

# LiDAR相对位置
lidar_offset = np.array(LIDAR_PARAMS["pos"])
lidar_pos = vehicle_pos + lidar_offset

lidar_pos = vehicle_pos + np.array(LIDAR_PARAMS["pos"])
lidar_mat = np.eye(3)

# 生成角度范围
azimuth_angles = np.arange(0, 360, LIDAR_PARAMS["azimuth_res"]) # 方位角:0~360°
elevation_angles = np.arange(
Expand Down Expand Up @@ -102,6 +112,9 @@ def generate_realistic_lidar_data(self):
# 归一化方向向量
dir_local = dir_local / np.linalg.norm(dir_local)

# 将方向向量从LiDAR坐标系转换到世界坐标系
dir_world = lidar_mat @ dir_local

# 创建参数
geom_group = np.array([1, 1, 1, 1, 1, 1], dtype=np.uint8)
geom_id = np.zeros(1, dtype=np.int32)
Expand All @@ -110,7 +123,7 @@ def generate_realistic_lidar_data(self):
distance = mujoco.mj_ray(
self.model, self.data,
lidar_pos, # 射线起点
dir_local, # 射线方向
dir_world, # 射线方向(世界坐标系)
geom_group, # 几何体组
1, # flg_static: 检测静态几何体
-1, # bodyexclude: 不排除任何body
Expand All @@ -120,7 +133,7 @@ def generate_realistic_lidar_data(self):
# 记录点云数据
if distance >= 0 and distance <= LIDAR_PARAMS["range"]: # 如果检测到碰撞且在范围内
# 计算交点位置
hit_pos = lidar_pos + dir_local * distance
hit_pos = lidar_pos + dir_world * distance
point_cloud.append(hit_pos)

# 转换为numpy数组
Expand All @@ -136,56 +149,79 @@ def detect_objects(self):
"""检测环境中的物体"""
detected_objects = []

# 获取车辆位置
try:
vehicle_pos, _ = self.get_world_pose("vehicle")
except ValueError:
vehicle_pos = np.array([0, 0, 0.5])

# 遍历所有物体
for i in range(self.model.nbody):
body_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_BODY, i)
if body_name and body_name.startswith("obstacle"):
# 获取物体位置
pos = self.data.xpos[i].copy()

# 简单的距离检测(10米内认为可检测到)
try:
vehicle_pos, _ = self.get_world_pose("vehicle")
distance = np.linalg.norm(pos - vehicle_pos)
# 计算与车辆的距离
distance = np.linalg.norm(pos - vehicle_pos)

# 只有在检测范围内才记录
if distance <= 15.0: # 扩大检测范围
# 获取物体类型(根据名称)
obj_type = "box"

if distance <= 10.0:
# 获取物体类型(根据名称)
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,
"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",
"type": obj_type,
"position": pos.tolist(),
"distance": 0,
"distance": float(distance),
"size": size.tolist()
})

return detected_objects

def calculate_avoidance_control(self, lidar_data, detected_objects):
"""基于传感器数据计算避障控制指令"""
# 初始化控制指令
left_speed = 5.0
right_speed = 5.0
steering_angle = 0.0

if len(detected_objects) > 0:
# 找到最近的障碍物
closest_obj = min(detected_objects, key=lambda x: x['distance'])

if closest_obj['distance'] < 5.0: # 如果障碍物很近
obj_pos = np.array(closest_obj['position'])
try:
vehicle_pos, _ = self.get_world_pose("vehicle")
# 计算障碍物相对于车辆的方向
direction = obj_pos[:2] - vehicle_pos[:2] # 只考虑XY平面
angle_to_obstacle = np.arctan2(direction[1], direction[0])

# 简单避障策略:向相反方向转弯
if angle_to_obstacle > 0: # 障碍物在左侧
steering_angle = -5.0 # 向右转
else: # 障碍物在右侧
steering_angle = 5.0 # 向左转

# 如果非常接近,减速
if closest_obj['distance'] < 3.0:
left_speed = 2.0
right_speed = 2.0
except ValueError:
pass

return left_speed, right_speed, steering_angle

def generate_annotations(self):
"""生成物体检测标注数据"""
# 检测到的物体
Expand Down Expand Up @@ -306,26 +342,17 @@ def run_simulation(self):
prev_detected_count = 0

for i in range(SIMULATION_FRAMES):
# 设置控制输入(使车辆向前移动并轻微转向)
if rear_left_idx >= 0:
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)

# 每20帧生成和保存一次数据
if i % 20 == 0:
# 生成传感器数据和标注
lidar_data = self.generate_realistic_lidar_data()
annotations = self.generate_annotations()

# 基于传感器数据计算控制指令
left_speed, right_speed, steering_angle = self.calculate_avoidance_control(
lidar_data, annotations["objects"]
)

# 显示检测到的物体数量
detected_count = len(annotations["objects"])
if detected_count != prev_detected_count:
Expand All @@ -344,9 +371,33 @@ def run_simulation(self):
self.visualize_detection(lidar_data, annotations)

print(f"已仿真 {i}/{SIMULATION_FRAMES} 帧")
else:
# 使用上一帧的控制指令
try:
left_speed, right_speed, steering_angle
except NameError:
left_speed, right_speed, steering_angle = 5.0, 5.0, 0.0

# 设置控制输入
if rear_left_idx >= 0:
self.data.ctrl[rear_left_idx] = left_speed # 左后轮速度
if rear_right_idx >= 0:
self.data.ctrl[rear_right_idx] = right_speed # 右后轮速度
# 设置前轮转向
if front_left_steer_idx >= 0:
self.data.ctrl[front_left_steer_idx] = steering_angle
if front_right_steer_idx >= 0:
self.data.ctrl[front_right_steer_idx] = steering_angle

# 执行仿真步长
mujoco.mj_step(self.model, self.data)

# 更新可视化
if hasattr(self, 'viewer') and self.viewer is not None:
self.viewer.sync()

# 控制仿真速度以便观察
time.sleep(0.02)
time.sleep(0.01)

print(f"仿真完成!数据已保存到:{self.output_dir}")

Expand Down
Loading