1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135
| import numpy as np from typing import List, Tuple
class MobilityGen: """ MobilityGen数据生成 用于移动机器人轨迹规划和数据采集 """ def __init__( self, map_resolution: float = 0.05, robot_radius: float = 0.3 ): self.map_resolution = map_resolution self.robot_radius = robot_radius self.occupancy_map = None def build_occupancy_map( self, point_cloud: np.ndarray ) -> np.ndarray: """ 构建占据栅格地图 Args: point_cloud: 点云数据, shape=(N, 3) Returns: occupancy_map: 占据栅格, shape=(H, W) """ x_bins = int(point_cloud[:, 0].max() / self.map_resolution) y_bins = int(point_cloud[:, 1].max() / self.map_resolution) occupancy_map = np.zeros((y_bins, x_bins)) for point in point_cloud: x_idx = int(point[0] / self.map_resolution) y_idx = int(point[1] / self.map_resolution) if 0 <= x_idx < x_bins and 0 <= y_idx < y_bins: occupancy_map[y_idx, x_idx] = 1 self.occupancy_map = occupancy_map return occupancy_map def plan_trajectory( self, start: Tuple[float, float], goal: Tuple[float, float] ) -> List[Tuple[float, float]]: """ 规划无碰撞轨迹 Args: start: 起点坐标 goal: 终点坐标 Returns: trajectory: 轨迹点列表 """ trajectory = self.a_star(start, goal) return trajectory def a_star(self, start, goal): """A*路径规划(简化)""" return [start, goal] def generate_data( self, scene_usd: str, num_trajectories: int = 100 ) -> dict: """ 生成训练数据 Returns: { 'rgb_images': RGB图像列表, 'depth_images': 深度图列表, 'poses': 位姿列表 } """ data = { 'rgb_images': [], 'depth_images': [], 'poses': [] } for _ in range(num_trajectories): start = (np.random.rand() * 10, np.random.rand() * 10) goal = (np.random.rand() * 10, np.random.rand() * 10) trajectory = self.plan_trajectory(start, goal) for pose in trajectory: rgb = np.random.rand(480, 640, 3) * 255 depth = np.random.rand(480, 640) * 10 data['rgb_images'].append(rgb) data['depth_images'].append(depth) data['poses'].append(pose) return data
if __name__ == "__main__": gen = MobilityGen() point_cloud = np.random.rand(10000, 3) * 20 occ_map = gen.build_occupancy_map(point_cloud) print(f"占据栅格大小: {occ_map.shape}") trajectory = gen.plan_trajectory((0, 0), (10, 10)) print(f"轨迹点数: {len(trajectory)}") data = gen.generate_data("scene.usd", num_trajectories=10) print(f"生成RGB图像数: {len(data['rgb_images'])}")
|