std::vector<LaserData> simulateLaserScan(const Robot& robot, const Map& map) { std::vector<LaserData> laser_data; const double start_angle = robot.theta - robot.laser_fov / 2; const double end_angle = robot.theta + robot.laser_fov / 2; for (double angle = start_angle; angle <= end_angle; angle += 0.1) { double x = robot.x; double y = robot.y; double range = robot.laser_range; while (range > 0) { x += range * cos(angle); y += range * sin(angle); if (x < 0 || y < 0 || x >= MAP_WIDTH || y >= MAP_HEIGHT) { range = 0; // 超出地图范围 } else if (map.data[(int)x][(int)y] == 1) { range = 0; // 障碍物 } else { range -= 1; // 每次减少1 } } laser_data.push_back({robot.laser_range - range, angle}); } return laser_data; }

这段代码是一个模拟激光雷达扫描的函数。函数接受一个机器人对象和地图对象作为输入，返回一个激光数据的向量。函数首先计算出激光雷达扫描的起始角度和结束角度，然后从起始角度到结束角度以0.1度为间隔进行循环。在循环内部，函数通过机器人的位置和当前角度计算出激光射线的终点位置，并检查终点是否超出地图范围或者是否与障碍物相撞。如果超出地图范围或者与障碍物相撞，激光射线的长度就是机器人到终点的距离；否则，激光射线的长度每次减少1，直到终点超出激光雷达的最大量程或者与障碍物相撞。最后，函数将每个角度上的激光数据加入到向量中，并返回向量。