lidar.py

#LIDAR Sensor
#Fires rays from the robot's position
#in a fan shape and measures how far each ray travels before hitting an obstacle or:
#(if noise is enabled) either ignoring hitting an obstacle (false negative) or hitting thin air (false positive)
import math
import random
from config_parameters import *
from geometry import ray_segment_intersection, ray_circle_intersection


class Lidar:

    def __init__(self):
        self.fov=LIDAR_FOV
        self.resolution=LIDAR_RESOLUTION
        self.max_range=LIDAR_MAX_RANGE

        #Noise (off by default)
        self.negative_false=0.0
        self.positive_false=0.0

        #Performance optimization: Precompute ray angles by dividing FOV by resolution of LIDAR sensor
        half=self.fov / 2.0
        self.ray_angles=[]
        a=-half

        #angles include: -95, -93, -1, 1, 93, 95 (assuming 190 degree FOV)
        while a < half:
            self.ray_angles.append(a)
            a += self.resolution
        self.num_rays=len(self.ray_angles)


    #Returns list of (angle_rad, distance, hit_x, hit_y, did_hit)
    #misses will have the max Lidar Range and can be identified that way
    def scan(self, robot_x, robot_y, robot_heading, map):

        all_segments=map.get_all_segments()
        results=[]

        for i in range(self.num_rays):

            robot_relative_angle_rad=math.radians(self.ray_angles[i])
            #Absolute angle relative to the world
            world_angle=robot_heading + robot_relative_angle_rad

            #Projection in x and y with cos and sin
            ray_dx=math.cos(world_angle)
            ray_dy=math.sin(world_angle)

            #Max range is a placeholder, looking for a lower hit distance to replace with
            #If not, max range signifies "no hit"
            min_dist=self.max_range
            hit=False

            #Note: ray_dx and ray_dy are a unit vector, therefore, t=ray_segment_intersection will
            #return a real distance, not requiring further normalization.

            #Checks all segments of an object
            for seg in all_segments:

                t=ray_segment_intersection(robot_x,robot_y,ray_dx,ray_dy,
                                           seg[0],seg[1],seg[2],seg[3])
                if t is not None and t < min_dist:
                    if self.negative_false > 0 and random.random()*100 < self.negative_false:
                        #The continue causes setting the hit to be true and min_dist=t to be skipped
                        #simulating a false negative
                        #But the remaining segments are still checked
                        continue
                    min_dist=t
                    hit=True

            #Circles use their own formula as segmentation could be more computationally expensive
            for circ in map.circles:
                t=ray_circle_intersection(robot_x,robot_y,ray_dx,ray_dy,
                                          circ[0],circ[1],circ[2])

                if t is not None and t < min_dist:

                    if self.negative_false > 0 and random.random()*100 < self.negative_false:
                        #The continue causes setting the hit to be true and min_dist=t to be skipped
                        #simulating a false negative
                        continue

                    min_dist=t
                    hit=True

            #False positive hit detection
            if not hit and self.positive_false > 0 and random.random()*100 < self.positive_false:
                min_dist=random.random() * self.max_range * 0.8
                hit=True


            #Hits are saved here and converted back into world coordinates using cos and sin.
            # If there is a miss, min_distance will have the max value of lidar range (tested with 120)
            # and can be identified that way as misses or using the hit flag
            hx=robot_x + min_dist * math.cos(world_angle)
            hy=robot_y + min_dist * math.sin(world_angle)

            results.append((robot_relative_angle_rad, min_dist, hx, hy, hit))

        return results