comsim.py 文件源码

def red_pixels(self):
        pixels = np.zeros(self._red_pixel_sensors)
        for i in range(self._red_pixel_sensors):
            rotation = self._robot.rotation - self._fov / 2 + i * self._fov / (self._red_pixel_sensors-1)
            ray = affinity.rotate(
                LineString([np.array(self._robot.pos.coords)[0],
                            np.array(self._robot.pos.coords)[0] + (self._height*2, 0)]),
                rotation, origin=self._robot.pos, use_radians=True)
            dists_red = np.min(np.array([self._robot.pos.distance(point) if not point.is_empty else np.inf
                                for point in (ray.intersection(cube) for cube in self._red_cubes)]))
            dists_obs = np.min(np.array([self._robot.pos.distance(point) if not point.is_empty else np.inf
                                for point in (ray.intersection(cube) for cube in
                                    #filter(lambda o: np.array([self._robot.pos.distance(Point(p)) < dists_red for p in o.exterior.coords]).any(),
                                           self._obstacles)]))
            pixels[i] = dists_red < dists_obs
        return pixels