diff --git a/examples/dynamic2.py b/examples/dynamic2.py new file mode 100644 index 0000000..d48758a --- /dev/null +++ b/examples/dynamic2.py @@ -0,0 +1,117 @@ +import math +import time +import serial +import threading +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from rplidar import RPLidar + +# Configuration +LIDAR_PORT = '/dev/ttyUSB0' +IMU_PORT = '/dev/ttyACM0' +IMU_BAUDRATE = 9600 +DMAX = 4000 # Maximum distance in mm +POINT_SIZE = 1 + +# Initialize LIDAR and IMU +lidar = RPLidar(LIDAR_PORT) +imu_serial = serial.Serial(IMU_PORT, IMU_BAUDRATE, timeout=1) +global_map = [] + +# Shared variables for IMU data +tilt_angle = 0.0 +tilt_lock = threading.Lock() + +def polar_to_cartesian(angle_deg, distance_mm): + """Convert polar coordinates to Cartesian.""" + angle_rad = math.radians(angle_deg) + x = distance_mm * math.cos(angle_rad) + y = distance_mm * math.sin(angle_rad) + return x, y + +def read_imu_data(): + """Read IMU data in a separate thread.""" + global tilt_angle + while True: + try: + line = imu_serial.readline().decode('utf-8').strip() + if line.startswith("GY:"): + gy_raw = float(line.replace("GY:", "").strip()) + with tilt_lock: # Safely update shared variable + tilt_angle += gy_raw * 0.01 # Example integration + except Exception as e: + print(f"IMU read error: {e}") + +def transform_to_3d(local_points): + """Transform 2D LiDAR points to 3D using the latest tilt angle.""" + global tilt_angle + global_points = [] + with tilt_lock: # Safely read shared variable + tilt_rad = math.radians(tilt_angle) + for x, y in local_points: + z = y * math.sin(tilt_rad) + y_proj = y * math.cos(tilt_rad) + global_points.append((x, y_proj, z)) + return global_points + +def update_map(lidar, ax, scatter): + global global_map + scan_count = 0 + + for scan in lidar.iter_scans(): + local_points = [] + for _, angle, distance in scan: + if distance > 0 and distance <= DMAX: + x, y = polar_to_cartesian(angle, distance) + local_points.append((x, y)) + + global_points = transform_to_3d(local_points) + global_map.extend(global_points) + + # Update plot every 10 scans + scan_count += 1 + if scan_count % 10 == 0: + # Downsample points for faster rendering + x_coords = [p[0] for p in global_map] + y_coords = [p[1] for p in global_map] + z_coords = [p[2] for p in global_map] + scatter._offsets3d = (x_coords, y_coords, z_coords) + plt.pause(0.01) + + +def main(): + try: + print("Starting LIDAR and IMU...") + status, error_code = lidar.get_health() + print(f"LIDAR health status: {status}, Error code: {error_code}") + + # Start IMU thread + imu_thread = threading.Thread(target=read_imu_data, daemon=True) + imu_thread.start() + + # Set up 3D plot + plt.ion() + fig = plt.figure(figsize=(10, 10)) + ax = fig.add_subplot(111, projection='3d') + ax.set_title("3D Mapping with Tilted LIDAR") + ax.set_xlim(-DMAX, DMAX) + ax.set_ylim(-DMAX, DMAX) + ax.set_zlim(-DMAX, DMAX) + ax.set_xlabel("X (mm)") + ax.set_ylabel("Y (mm)") + ax.set_zlabel("Z (mm)") + scatter = ax.scatter([], [], [], s=POINT_SIZE) + + update_map(lidar, ax, scatter) + + except KeyboardInterrupt: + print("Stopping LIDAR and IMU...") + + finally: + lidar.stop() + lidar.disconnect() + imu_serial.close() + print("LIDAR and IMU stopped") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/examples/dynamic_2d_map.py b/examples/dynamic_2d_map.py new file mode 100644 index 0000000..2eb049a --- /dev/null +++ b/examples/dynamic_2d_map.py @@ -0,0 +1,68 @@ +import math +import matplotlib.pyplot as plt +from rplidar import RPLidar + +# LIDAR setup +PORT_NAME = '/dev/ttyUSB0' +lidar = RPLidar(PORT_NAME) + +# Parameters for dynamic mapping +DMAX = 4000 # Maximum distance in mm to include points +POINT_SIZE = 1 # Size of points in the plot + +# Functions +def polar_to_cartesian(angle_deg, distance_mm): + """Convert polar coordinates to Cartesian coordinates.""" + angle_rad = math.radians(angle_deg) + x = distance_mm * math.cos(angle_rad) + y = distance_mm * math.sin(angle_rad) + return x, y + +def update_map(lidar, ax, scatter): + """Fetch data from LIDAR and update the map dynamically.""" + points = [] + for scan in lidar.iter_scans(): + for _, angle, distance in scan: + if distance > 0 and distance <= DMAX: # Filter out invalid or out-of-range points + x, y = polar_to_cartesian(angle, distance) + points.append((x, y)) + + # Update scatter plot + if points: + x_coords = [p[0] for p in points] + y_coords = [p[1] for p in points] + scatter.set_offsets(list(zip(x_coords, y_coords))) + plt.pause(0.01) # Adjust for smoother animation + + points.clear() # Clear points for the next frame + +# Main execution +def main(): + try: + print("Starting LIDAR...") + lidar.connect() + + # Set up the plot + plt.ion() + fig, ax = plt.subplots(figsize=(10, 10)) + ax.set_title("Dynamic 2D Mapping") + ax.set_xlim(-DMAX, DMAX) + ax.set_ylim(-DMAX, DMAX) + ax.set_xlabel("X (mm)") + ax.set_ylabel("Y (mm)") + ax.grid(True) + scatter = ax.scatter([], [], s=POINT_SIZE) + + # Dynamic mapping + update_map(lidar, ax, scatter) + + except KeyboardInterrupt: + print("Stopping LIDAR...") + + finally: + lidar.stop() + lidar.disconnect() + print("LIDAR stopped.") + +if __name__ == "__main__": + main() diff --git a/examples/dynamic_mapping_motion.py b/examples/dynamic_mapping_motion.py new file mode 100644 index 0000000..b94f7a1 --- /dev/null +++ b/examples/dynamic_mapping_motion.py @@ -0,0 +1,142 @@ +import math +import time +import serial +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from rplidar import RPLidar + +# Configuration +LIDAR_PORT = '/dev/ttyUSB0' +IMU_PORT = '/dev/ttyACM0' +IMU_BAUDRATE = 9600 +DMAX = 4000 # Maximum distance in mm +POINT_SIZE = 1 + +# Initialize LIDAR and IMU +lidar = RPLidar(LIDAR_PORT) +imu_serial = serial.Serial(IMU_PORT, IMU_BAUDRATE, timeout=2) +global_map = [] + + +def polar_to_cartesian(angle_deg, distance_mm): + """Convert polar coordinates to Cartesian.""" + angle_rad = math.radians(angle_deg) + x = distance_mm * math.cos(angle_rad) + y = distance_mm * math.sin(angle_rad) + return x, y + + +def get_tilt_angle(): + """Calculate tilt angle by integrating gyroscope data.""" + tilt_angle = 0 + last_time = time.time() + + while True: + try: + # Read a line from the IMU + line = imu_serial.readline().decode('utf-8').strip() + if line.startswith("GY:"): # Check for the prefix "GY:" + gz_raw = float(line.replace("GY:", "").strip()) # Extract numeric part + gz = gz_raw # Convert raw value to degrees/sec (scale factor for MPU9250) + + # Compute time interval + current_time = time.time() + dt = current_time - last_time + last_time = current_time + + # Integrate to get tilt angle + tilt_angle += gz * dt + return tilt_angle + else: + print(f"Ignoring non-gyroscope data: {line}") + except ValueError as ve: + print(f"Error reading tilt angle: {ve} (raw data: {line})") + return 0.0 + except Exception as e: + print(f"Unexpected error: {e}") + return 0.0 + + + +def transform_to_3d(local_points, tilt_angle): + """Transform 2D LIDAR points to 3D based on tilt angle.""" + global_points = [] + tilt_rad = math.radians(tilt_angle) + for x, y in local_points: + z = y * math.sin(tilt_rad) + y_proj = y * math.cos(tilt_rad) + global_points.append((x, y_proj, z)) + return global_points + + +def update_map(lidar, ax, scatter): + global global_map + for scan in lidar.iter_scans(): + lidar.clean_input() + local_points = [] + for _, angle, distance in scan: + lidar.clean_input() + if distance > 0 and distance <= DMAX: + x, y = polar_to_cartesian(angle, distance) + local_points.append((x, y)) + + tilt_angle = get_tilt_angle() # Get current tilt angle + global_points = transform_to_3d(local_points, tilt_angle) + global_map.extend(global_points) + + if global_map: + x_coords = [p[0] for p in global_map] + y_coords = [p[1] for p in global_map] + z_coords = [p[2] for p in global_map] + scatter._offsets3d = (x_coords, y_coords, z_coords) + lidar.clean_input() + plt.pause(0.01) + + +def main(): + try: + print("Starting LIDAR and IMU...") + print(lidar.get_info) + status, error_code = lidar.get_health() + print(f"LIDAR health status: {status}, Error code: {error_code}") + + print(lidar.express_data) + print(lidar.motor_running) + print(lidar.motor_speed) + lidar.stop() + lidar.disconnect() + lidar.connect() + lidar.clean_input() + lidar.start_motor() + lidar.motor_speed=60 + + + imu_serial.reset_input_buffer() + + # Set up 3D plot + plt.ion() + fig = plt.figure(figsize=(5, 5)) + ax = fig.add_subplot(111, projection='3d') + ax.set_title("3D Mapping with Tilted LIDAR") + ax.set_xlim(-DMAX, DMAX) + ax.set_ylim(-DMAX, DMAX) + ax.set_zlim(-DMAX, DMAX) + ax.set_xlabel("X (mm)") + ax.set_ylabel("Y (mm)") + ax.set_zlabel("Z (mm)") + scatter = ax.scatter([], [], [], s=POINT_SIZE) + + update_map(lidar, ax, scatter) + + except KeyboardInterrupt: + print("Stopping LIDAR and IMU...") + + finally: + lidar.stop() + lidar.disconnect() + imu_serial.close() + print("LIDAR and IMU stopped") + + +if __name__ == "__main__": + main() diff --git a/examples/generate_2d_map.py b/examples/generate_2d_map.py new file mode 100644 index 0000000..2308701 --- /dev/null +++ b/examples/generate_2d_map.py @@ -0,0 +1,52 @@ +import math +import matplotlib.pyplot as plt + +# File containing raw data (output from record_measurements.py) +DATA_FILE = "output.txt" + +def polar_to_cartesian(angle_deg, distance_mm): + """Convert polar coordinates to Cartesian coordinates.""" + angle_rad = math.radians(angle_deg) + x = distance_mm * math.cos(angle_rad) + y = distance_mm * math.sin(angle_rad) + return x, y + +def load_data(file_path): + """Load raw LIDAR data from the file.""" + points = [] + with open(file_path, "r") as f: + for line in f: + parts = line.split() + if len(parts) != 4: + continue # Skip invalid lines + + _, quality, angle, distance = parts + quality = int(quality) + angle = float(angle) + distance = float(distance) + + # Filter out invalid or low-quality data + if distance > 0 and quality > 0: + x, y = polar_to_cartesian(angle, distance) + points.append((x, y)) + return points + +def plot_map(points): + """Plot the 2D map using the processed points.""" + x_coords = [p[0] for p in points] + y_coords = [p[1] for p in points] + plt.figure(figsize=(10, 10)) + plt.scatter(x_coords, y_coords, s=1) + plt.title("2D Map from LIDAR Data") + plt.xlabel("X (mm)") + plt.ylabel("Y (mm)") + plt.axis("equal") + plt.grid(True) + plt.show() + +if __name__ == "__main__": + # Load and process LIDAR data + points = load_data(DATA_FILE) + + # Plot the 2D map + plot_map(points) diff --git a/examples/read_imu_serial.py b/examples/read_imu_serial.py new file mode 100644 index 0000000..de19cfa --- /dev/null +++ b/examples/read_imu_serial.py @@ -0,0 +1,29 @@ +import serial + +def read_imu_data(port='/dev/ttyACM0', baudrate=9600): + """Reads IMU data from Arduino via Serial.""" + ser = serial.Serial(port, baudrate, timeout=1) + try: + while True: + line = ser.readline().decode('utf-8').strip() # Read and clean data + print(f"Raw data: {line}") # Debug: Print raw data + if line.startswith("Accel:"): # Handle lines with 'Accel:' prefix + line = line.replace("Accel: ", "").strip() # Remove the prefix + + if line.count(",") == 2: # Ensure correct format (two commas) + try: + ax, ay, gz = map(float, line.split(",")) + print(f"AccelX: {ax}, AccelY: {ay}, GyroZ: {gz}") + except ValueError: + print("Malformed data, skipping...") + else: + print("Incomplete or malformed data, skipping...") + except KeyboardInterrupt: + print("Stopping IMU read...") + finally: + ser.close() + + + +if __name__ == "__main__": + read_imu_data() diff --git a/examples/record_measurments.py b/examples/record_measurments.py index f2880f0..0e6c82a 100755 --- a/examples/record_measurments.py +++ b/examples/record_measurments.py @@ -15,7 +15,7 @@ def run(path): outfile = open(path, 'w') try: print('Recording measurments... Press Crl+C to stop.') - for measurment in lidar.iter_measurments(): + for measurment in lidar.iter_measures(): line = '\t'.join(str(v) for v in measurment) outfile.write(line + '\n') except KeyboardInterrupt: diff --git a/examples/temp.py b/examples/temp.py new file mode 100644 index 0000000..49cd56f --- /dev/null +++ b/examples/temp.py @@ -0,0 +1,16 @@ +#!/usr/bin/env python3 +'''Measures sensor scanning speed''' +from rplidar import RPLidar +import time + +PORT_NAME = '/dev/ttyUSB0' + +def run(): + '''Main function''' + lidar = RPLidar(PORT_NAME) + health = lidar.get_health() + print(health) + + +if __name__ == '__main__': + run() \ No newline at end of file diff --git a/examples/view_raw_data.py b/examples/view_raw_data.py new file mode 100644 index 0000000..5e7f76e --- /dev/null +++ b/examples/view_raw_data.py @@ -0,0 +1,22 @@ +from rplidar import RPLidar + +# Specify your LiDAR port (adjust if necessary) +LIDAR_PORT = '/dev/ttyUSB0' + +def view_raw_data(): + """View raw data from the RPLidar.""" + lidar = RPLidar(LIDAR_PORT) + try: + print("Starting LiDAR and printing raw packets...") + for new_scan, quality, angle, distance in lidar.iter_measures(): + # Print each raw packet's details + print(f"New Scan: {new_scan}, Quality: {quality}, Angle: {angle}, Distance: {distance}") + except KeyboardInterrupt: + print("Stopping LiDAR.....") + finally: + lidar.stop() + lidar.disconnect() + print("LiDAR stopped.") + +if __name__ == "__main__": + view_raw_data() diff --git a/rplidar.py b/rplidar.py index 89ebd5c..46233ed 100644 --- a/rplidar.py +++ b/rplidar.py @@ -55,7 +55,7 @@ # Constants & Command to start A2 motor MAX_MOTOR_PWM = 1023 -DEFAULT_MOTOR_PWM = 660 +DEFAULT_MOTOR_PWM = 60 SET_PWM_BYTE = b'\xF0' _HEALTH_STATUSES = { @@ -157,6 +157,12 @@ def disconnect(self): def _set_pwm(self, pwm): payload = struct.pack("