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..e5afe5a --- /dev/null +++ b/examples/dynamic_mapping_motion.py @@ -0,0 +1,144 @@ +import math +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from rplidar import RPLidar +import serial +import time +import rplidar +import threading + +# LIDAR and IMU setup +LIDAR_PORT = '/dev/ttyUSB0' +IMU_PORT = '/dev/ttyACM0' +IMU_BAUDRATE = 9600 + +lidar = RPLidar(LIDAR_PORT) +imu_serial = serial.Serial(IMU_PORT, IMU_BAUDRATE, timeout=1) + +# Parameters +DMAX = 4000 # Maximum distance in mm to include points +POINT_SIZE = 1 # Size of points in the plot + +# Global map for 3D points +global_map = [] + +# 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 get_tilt_angle(): + """Fetch tilt angle (pitch) from IMU.""" + try: + line = imu_serial.readline().decode('utf-8').strip() + if line.startswith("Accel:"): # Clean up prefix if present + line = line.replace("Accel: ", "").strip() + if line.count(",") == 2: # Ensure correct format + ax, ay, gz = map(float, line.split(",")) + else: + return 0.0 # Default to no tilt on malformed data + # Use gyroscope Z-axis (gz) for tilt angle (pitch) + time.sleep(0.05) # Adjust delay as needed + return gz # In degrees or radians as needed + except Exception as e: + print(f"Error reading IMU data: {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) # Project y-axis distance to z-axis + y_proj = y * math.cos(tilt_rad) # Adjust y-axis for tilt + 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(): + start_time = time.time() + + # LIDAR data collection + local_points = [] + 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) + local_points.append((x, y)) + + # IMU data collection + tilt_angle = get_tilt_angle() + + # Transform to 3D points + global_points = transform_to_3d(local_points, tilt_angle) + global_map.extend(global_points) + + # Update scatter plot + 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) + + # Maintain consistent timing + elapsed = time.time() - start_time + time.sleep(max(0, 0.1 - elapsed)) # Adjust interval as needed + plt.pause(0.01) # Adjust for smoother animation + +# Main execution +def main(): + try: + print("Starting LIDAR and IMU...") + + # Clear LIDAR buffer + lidar.stop() + lidar.disconnect() + lidar.connect() + lidar.clean_input() + + lidar._serial.reset_input_buffer() # Clear any old data + imu_serial.reset_input_buffer() + + + # Clear IMU buffer + imu_serial.reset_input_buffer() # Clear IMU input buffer + imu_serial.reset_output_buffer() # Clear IMU output buffer + + # Start LIDAR motor + lidar.start_motor() + lidar._set_pwm(60) + lidar.clean_input + print(lidar.motor_speed)# Ensure the motor is running + # Reduce from default 660 RPM to 200 RPM + + # Set up the 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) + + # Begin dynamic mapping + 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/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("