Lidar Integration

# Install RPLidar driver
pip install Adafruit_CircuitPython_RPLIDAR

# Install dependencies
pip install glob2 pyserial numpy pillow

# For YDLidar
pip install PyLidar3

# Add udev rule for lidar
echo 'SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", MODE="0666"' | sudo tee /etc/udev/rules.d/99-lidar.rules

# Reload udev
sudo udevadm control --reload-rules
sudo udevadm trigger

# Verify device
ls -l /dev/ttyUSB*

# Enable lidar
USE_LIDAR = True

# Lidar type
LIDAR_TYPE = 'RP'  # 'RP' for RPLidar, 'YD' for YDLidar

# Angle filtering (degrees)
LIDAR_LOWER_LIMIT = 90   # Block rear of vehicle
LIDAR_UPPER_LIMIT = 270  # Front 180° view

# Note: For RPLidar A1M8, 0° is at motor direction
# Adjust limits based on mounting orientation

from donkeycar.parts.lidar import RPLidar2, CLOCKWISE, COUNTER_CLOCKWISE

# Create lidar part
lidar = RPLidar2(
    min_angle=90.0,        # degrees
    max_angle=270.0,       # degrees  
    min_distance=100.0,    # millimeters
    max_distance=4000.0,   # millimeters
    forward_angle=0.0,     # degrees (direction of 0°)
    angle_direction=CLOCKWISE,  # or COUNTER_CLOCKWISE
    batch_ms=50,           # polling duration
    debug=False
)

# Add to vehicle
V.add(lidar, 
      outputs=['lidar/measurements'],
      threaded=True)

(distance, angle, timestamp, scan_id, index)

# RPLidar spins clockwise
# 0° is at motor housing

# If car forward is at motor (0°):
forward_angle = 0.0
min_angle = 270.0  # left side
max_angle = 90.0   # right side (wraps through 0)

# If car forward is opposite motor (180°):
forward_angle = 180.0  
min_angle = 90.0   # left side
max_angle = 270.0  # right side

def angle_in_bounds(angle, min_angle, max_angle):
    """Determine if angle is between two bounds"""
    if min_angle <= max_angle:
        return min_angle <= angle <= max_angle
    else:
        # Range crosses 0°
        return (min_angle <= angle <= 360) or (max_angle >= angle >= 0)

from donkeycar.parts.lidar import RPLidar

# Create simple lidar
lidar = RPLidar(
    lower_limit=90,   # degrees
    upper_limit=270,  # degrees
    debug=False
)

# Add to vehicle
V.add(lidar,
      outputs=['lidar/distances'],
      threaded=True)

from donkeycar.parts.lidar import YDLidar

# Create YDLidar
lidar = YDLidar(port='/dev/ttyUSB0')

# Add to vehicle  
V.add(lidar,
      outputs=['lidar/distances', 'lidar/angles'],
      threaded=True)

from donkeycar.parts.lidar import LidarPlot2

# Create plotter
plotter = LidarPlot2(
    resolution=(500, 500),
    plot_type=LidarPlot2.PLOT_TYPE_CIRCLE,  # or PLOT_TYPE_LINE
    mark_px=3,                # point size
    max_dist=4000,            # millimeters
    angle_direction=COUNTER_CLOCKWISE,
    rotate_plot=0,            # degrees to rotate display
    background_color=(32, 32, 32),
    border_color=(128, 128, 128),
    point_color=(64, 255, 64)
)

# Add to vehicle
V.add(plotter,
      inputs=['lidar/measurements'],
      outputs=['lidar/image'])

# Add camera output for lidar image
from donkeycar.parts.network import TCPServeValue
from donkeycar.parts.image import ImgArrToJpg

if cfg.USE_LIDAR:
    pub = TCPServeValue("lidar")
    V.add(ImgArrToJpg(), 
          inputs=['lidar/image'],
          outputs=['lidar/jpg'])
    V.add(pub, inputs=['lidar/jpg'])

from donkeycar.parts.lidar import BreezySLAM, BreezyMap, MapToImage

# Create map buffer
MAP_SIZE_PIXELS = 500
MAP_SIZE_METERS = 10

map_buffer = BreezyMap(MAP_SIZE_PIXELS)
V.add(map_buffer, outputs=['slam/map_bytes'])

# Create SLAM
slam = BreezySLAM(MAP_SIZE_PIXELS, MAP_SIZE_METERS)
V.add(slam,
      inputs=['lidar/distances', 'lidar/angles', 'slam/map_bytes'],
      outputs=['slam/x', 'slam/y', 'slam/theta'])

# Convert map to image
map_to_img = MapToImage(resolution=(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
V.add(map_to_img,
      inputs=['slam/map_bytes'],
      outputs=['slam/map_image'])

pip install breezyslam

class ObstacleDetector:
    def __init__(self, min_distance=500, angle_range=30):
        """Detect obstacles within angle_range of forward"""
        self.min_distance = min_distance  # mm
        self.angle_range = angle_range     # degrees
        
    def run(self, measurements):
        """Return True if obstacle detected"""
        for distance, angle, _, _, _ in measurements:
            # Check if measurement is in front
            if abs(angle) < self.angle_range or abs(angle - 360) < self.angle_range:
                if 0 < distance < self.min_distance:
                    return True
        return False

# Add to vehicle
obstacle = ObstacleDetector(min_distance=500, angle_range=45)
V.add(obstacle,
      inputs=['lidar/measurements'],
      outputs=['obstacle/detected'])

# Emergency stop on obstacle
class SafetyStop:
    def run(self, throttle, obstacle_detected):
        if obstacle_detected:
            return 0.0  # Stop
        return throttle

V.add(SafetyStop(),
      inputs=['pilot/throttle', 'obstacle/detected'],
      outputs=['pilot/throttle'])

# Add lidar data to tub inputs
inputs = [
    'cam/image_array',
    'user/angle', 'user/throttle',
    'lidar/measurements'  # Will be stored as array
]

types = [
    'image_array',
    'float', 'float',
    'vector'  # Vector type for arrays
]

tub_writer = TubWriter(tub_path, inputs=inputs, types=types)
V.add(tub_writer, inputs=inputs, outputs=["tub/num_records"], 
      run_condition='recording')

python -m donkeycar.parts.lidar \
    --rate 10 \
    --number 100 \
    --min-angle 90 \
    --max-angle 270 \
    --max-distance 4000 \
    --forward-angle 0 \
    --angle-direction -1 \
    --threaded

# More frequent updates (25ms)
lidar = RPLidar2(batch_ms=25)

# Less frequent (100ms)
lidar = RPLidar2(batch_ms=100)

# Only front hemisphere
min_angle = 270.0
max_angle = 90.0

# Only mid-range
min_distance = 200.0   # Ignore very close
max_distance = 3000.0  # Ignore far away

V.add(lidar, outputs=['lidar/measurements'], threaded=True)

# Check USB connection
ls -l /dev/ttyUSB*

# Check permissions
sudo chmod 666 /dev/ttyUSB0

# Test serial port
python -c "import serial; s=serial.Serial('/dev/ttyUSB0'); print('OK')"

# Increase timeout
self.lidar = RPLidar(None, self.port, timeout=5)

# Check baud rate (RPLidar uses 115200)

# Detect parking space
class ParkingDetector:
    def run(self, measurements):
        # Find gap in obstacles
        # Return True if space found
        pass

# Maintain distance from wall
class WallFollower:
    def run(self, measurements):
        # Calculate distance to right wall
        # Return steering to maintain offset
        pass

# Avoid obstacles ahead  
class CollisionAvoidance:
    def run(self, measurements, steering, throttle):
        # Check for obstacles in direction of travel
        # Modify steering to avoid
        pass