Skip to content

LiDAR

2D ray-casting sensor against the occupancy grid and circular obstacles.

Lidar 

Lidar(config)
Source code in src/hivebr/engine/lidar.py 
 8
 9
10
11
12
13
14
def __init__(self, config: dict):
    c = config["lidar"]
    self.enabled = bool(c.get("enabled", True))
    self.num_rays = int(c.get("num_rays", 41))
    self.range = float(c.get("range", 4.0))
    self.fov = math.radians(float(c.get("fov_deg", 180)))
    self.colors = config["colors"]

enabled instance-attribute 

enabled = bool(c.get('enabled', True))

num_rays instance-attribute 

num_rays = int(c.get('num_rays', 41))

range instance-attribute 

range = float(c.get('range', 4.0))

fov instance-attribute 

fov = math.radians(float(c.get('fov_deg', 180)))

colors instance-attribute 

colors = config['colors']

ray_circle_intersection 

ray_circle_intersection(ray_origin, ray_dir, obs)
Source code in src/hivebr/engine/lidar.py 
16
17
18
19
20
21
22
23
24
25
26
27
28
def ray_circle_intersection(self, ray_origin, ray_dir, obs):
    circle_radius = obs[2]
    circle_center = np.array(obs[:2])
    oc = ray_origin - circle_center
    a = np.dot(ray_dir, ray_dir)
    b = 2.0 * np.dot(oc, ray_dir)
    c = np.dot(oc, oc) - circle_radius ** 2
    disc = b*b - 4*a*c
    if disc < 0:
        return None
    sq = math.sqrt(disc)
    valid = [t for t in [(-b - sq)/(2*a), (-b + sq)/(2*a)] if 0 <= t <= self.range]
    return min(valid) if valid else None

cast_rays 

cast_rays(robot, world, other_robots=None)

Faz ray-casting a partir de robot.

other_robots permite tratar os demais B-bots como obstáculos móveis (círculos com o raio do robô). Passe None para ignorá-los.

Source code in src/hivebr/engine/lidar.py 
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
def cast_rays(self, robot, world, other_robots=None):
    """Faz ray-casting a partir de ``robot``.

    ``other_robots`` permite tratar os demais B-bots como obstáculos
    móveis (círculos com o raio do robô). Passe ``None`` para ignorá-los.
    """
    readings = []
    angles = np.linspace(robot.theta - self.fov/2, robot.theta + self.fov/2, self.num_rays)

    # Check if grid map exists for ray casting
    grid_map = getattr(world, 'grid_map', None)

    # Círculos dos demais robôs (obstáculos móveis).
    robot_circles = []
    if other_robots:
        for other in other_robots:
            if other is robot:
                continue
            robot_circles.append(
                np.array([other.x, other.y, float(other.radius)])
            )

    for angle in angles:
        ray_dir = np.array([math.cos(angle), math.sin(angle)])
        min_d = self.range
        hit = robot.position + ray_dir * self.range

        # 1. Cast against grid map if available
        if grid_map is not None:
            d, h = grid_map.cast_ray(robot.x, robot.y, angle, self.range)
            if d < min_d:
                min_d = d
                hit = h

        # 2. Cast against circular obstacles
        for obs in world.obstacles:
            d = self.ray_circle_intersection(robot.position, ray_dir, obs)
            if d is not None and d < min_d:
                min_d = d
                hit = robot.position + ray_dir * d

        # 3. Cast against the other robots (moving obstacles)
        for circle in robot_circles:
            d = self.ray_circle_intersection(robot.position, ray_dir, circle)
            if d is not None and d < min_d:
                min_d = d
                hit = robot.position + ray_dir * d
        readings.append((angle, min_d, hit))
    return readings

draw 

draw(screen, camera, robot, world, other_robots=None)
Source code in src/hivebr/engine/lidar.py 
80
81
82
83
84
85
86
87
88
89
def draw(self, screen, camera, robot, world, other_robots=None):
    if not self.enabled:
        return
    origin = camera.world_to_screen(robot.x, robot.y)
    color = tuple(self.colors["lidar"])
    far = (80, 120, 120)
    for _, dist, hit in self.cast_rays(robot, world, other_robots):
        pygame.draw.line(screen, color if dist < self.range else far, origin, camera.world_to_screen(*hit), 1)
        if dist < self.range:
            pygame.draw.circle(screen, color, camera.world_to_screen(*hit), 3)