304 lines
10 KiB
Python
304 lines
10 KiB
Python
|
# main.py (Complete Structure)
|
|||
|
|
|
|||
|
|
import sys
|
|||
|
|
import os
|
|||
|
|
import time
|
|||
|
|
RUNNING_IN_GUI = os.getenv("RUNNING_IN_GUI") == "0"
|
|||
|
|
import numpy as np
|
|||
|
|
import matplotlib.pyplot as plt
|
|||
|
|
#if os.getenv("RUNNING_IN_GUI") == "1":
|
|||
|
|
# plt.switch_backend('Agg')
|
|||
|
|
from matplotlib.colors import LinearSegmentedColormap
|
|||
|
|
import subprocess # Ensure subprocess is imported if LidarReader might need it indirectly (though it's self-contained)
|
|||
|
|
|
|||
|
|
|
|||
|
|
from lidar_reader import LidarReader
|
|||
|
|
from imu_reader import IMUReader
|
|||
|
|
|
|||
|
|
# Import necessary components from BreezySLAM
|
|||
|
|
from breezyslam.algorithms import RMHC_SLAM
|
|||
|
|
from breezyslam.sensors import Laser # <--- Import Laser base class
|
|||
|
|
|
|||
|
|
# Add the parent of TurboPi to the path
|
|||
|
|
sys.path.append('/home/poebot/RPLidar/libs/rplidar_sdk/app/ultra_simple/TurboPi/HiwonderSDK/')
|
|||
|
|
from mecanum import MecanumChassis
|
|||
|
|
|
|||
|
|
from power_monitor import PowerMonitor
|
|||
|
|
power = PowerMonitor()
|
|||
|
|
|
|||
|
|
|
|||
|
|
# Import logger if you have a separate logger setup
|
|||
|
|
# from logger import get_logger
|
|||
|
|
# logger = get_logger()
|
|||
|
|
# If not using a separate logger.py, configure basic logging:
|
|||
|
|
from logger import get_logger
|
|||
|
|
logger = get_logger()
|
|||
|
|
# Set higher level for noisy BreezySLAM logs if needed
|
|||
|
|
# logging.getLogger('breezyslam').setLevel(logging.WARNING)
|
|||
|
|
|
|||
|
|
chassis = MecanumChassis()
|
|||
|
|
|
|||
|
|
from wall_follower import WallFollower
|
|||
|
|
|
|||
|
|
# --- Constants ---
|
|||
|
|
MAP_SIZE_PIXELS = 800
|
|||
|
|
MAP_SIZE_METERS = 8
|
|||
|
|
# Calculate MM_TO_PIXELS correctly
|
|||
|
|
MM_TO_PIXELS = MAP_SIZE_PIXELS / (MAP_SIZE_METERS * 1000.0)
|
|||
|
|
CENTER = MAP_SIZE_PIXELS // 2
|
|||
|
|
LIDAR_UPDATE_INTERVAL = 0.1 # Optional: control loop frequency
|
|||
|
|
|
|||
|
|
# --- Sensor model ---
|
|||
|
|
# Describes the LIDAR characteristics to BreezySLAM
|
|||
|
|
class RPLidarSensor(Laser):
|
|||
|
|
def __init__(self):
|
|||
|
|
super().__init__(
|
|||
|
|
scan_size=360, # Number of measurements per scan
|
|||
|
|
scan_rate_hz=5, # Scan rate (adjust if known, affects some SLAM aspects)
|
|||
|
|
detection_angle_degrees=360, # Angular range of the scan
|
|||
|
|
distance_no_detection_mm=6000, # Max range in mm or value for no detection, prev 6k
|
|||
|
|
offset_mm=0 # Offset from robot center (if any)
|
|||
|
|
)
|
|||
|
|
# Should be a list of floats/ints from 0 to 359
|
|||
|
|
self.scan_angles_degrees = list(range(360))
|
|||
|
|
|
|||
|
|
# --- IMU Initialization ---
|
|||
|
|
try:
|
|||
|
|
imu = IMUReader()
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"❌ Failed to initialize IMU: {e}")
|
|||
|
|
imu = None # Allow running without for testing
|
|||
|
|
|
|||
|
|
# --- LIDAR Initialization ---
|
|||
|
|
try:
|
|||
|
|
# Use the correct device path for your system
|
|||
|
|
lidar = LidarReader(device="/dev/lidar")
|
|||
|
|
|
|||
|
|
logger.debug("🌀 Letting LIDAR warm up for 1 second...")
|
|||
|
|
time.sleep(1.0)
|
|||
|
|
|
|||
|
|
logger.debug("⏳ Waiting for first valid LIDAR scan...")
|
|||
|
|
while True:
|
|||
|
|
scan, _ = lidar.get_scan()
|
|||
|
|
logger.debug(f"👀 Current scan length: {len(scan) if scan else 0}")
|
|||
|
|
if scan and len(scan) == 360:
|
|||
|
|
logger.debug("✅ First full LIDAR scan received. Proceeding with SLAM.")
|
|||
|
|
break
|
|||
|
|
else:
|
|||
|
|
logger.debug(f"🕰️ Incomplete scan, retrying...")
|
|||
|
|
time.sleep(0.1)
|
|||
|
|
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"❌ Failed to initialize LIDAR: {e}")
|
|||
|
|
exit() # Exit if LIDAR fails
|
|||
|
|
|
|||
|
|
|
|||
|
|
# --- SLAM Initialization ---
|
|||
|
|
try:
|
|||
|
|
# Create an instance of sensor description class
|
|||
|
|
sensor_description = RPLidarSensor()
|
|||
|
|
# Initialize SLAM with the sensor description, map size in pixels, and map size in meters
|
|||
|
|
slam = RMHC_SLAM(sensor_description, MAP_SIZE_PIXELS, MAP_SIZE_METERS)
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"❌ Failed to initialize SLAM: {e}")
|
|||
|
|
exit()
|
|||
|
|
|
|||
|
|
# --- Visualization setup ---
|
|||
|
|
try:
|
|||
|
|
cmap = LinearSegmentedColormap.from_list("redmap", ["#FFD700", "#4C566A"]) # Black to red
|
|||
|
|
|
|||
|
|
plt.ion()
|
|||
|
|
fig, ax = plt.subplots(figsize=(8, 8))
|
|||
|
|
fig.patch.set_facecolor('#0a0a0a')
|
|||
|
|
ax.set_facecolor('#0a0a0a')
|
|||
|
|
|
|||
|
|
map_im = ax.imshow(np.zeros((MAP_SIZE_PIXELS, MAP_SIZE_PIXELS), dtype=np.uint8),
|
|||
|
|
cmap=cmap, origin='lower', vmin=0, vmax=255)
|
|||
|
|
robot_dot, = ax.plot([], [], 'co', markersize=5)
|
|||
|
|
path_line, = ax.plot([], [], 'c-', linewidth=1, alpha=0.7)
|
|||
|
|
ax.set_title("SLAM Map (Press Ctrl+C to save & exit)", color="white")
|
|||
|
|
ax.set_aspect('equal', adjustable='box')
|
|||
|
|
|
|||
|
|
# 🚫 Remove this if running from the GUI
|
|||
|
|
if not RUNNING_IN_GUI:
|
|||
|
|
plt.show() # Only show in standalone
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
# Show the plot window
|
|||
|
|
logger.info("✅ Visualization initialized.")
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"❌ Failed to initialize visualization: {e}")
|
|||
|
|
visualization_enabled = False
|
|||
|
|
logger.warning("⚠️ Running without visualization due to error.")
|
|||
|
|
else:
|
|||
|
|
visualization_enabled = True
|
|||
|
|
|
|||
|
|
wall_follower = WallFollower(chassis)
|
|||
|
|
|
|||
|
|
# --- Main Loop ---
|
|||
|
|
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
|
|||
|
|
pose = [0.0, 0.0, 0.0] # [x_mm, y_mm, theta_degrees]
|
|||
|
|
path = []
|
|||
|
|
viz_center_x = CENTER
|
|||
|
|
viz_center_y = CENTER
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
print("🗺️ Mapping started. Move the bot around to map.")
|
|||
|
|
logger.info("🗺️ Mapping started.")
|
|||
|
|
last_map_update_time = time.time()
|
|||
|
|
|
|||
|
|
while True:
|
|||
|
|
loop_start_time = time.time()
|
|||
|
|
|
|||
|
|
voltage = power.get_voltage()
|
|||
|
|
if voltage is not None:
|
|||
|
|
logger.debug(f"🔋 Voltage = {voltage:.2f} V")
|
|||
|
|
if not power.voltage_ok():
|
|||
|
|
logger.warning("🛑 Voltage too low! Stopping motors.")
|
|||
|
|
chassis.reset_motors()
|
|||
|
|
time.sleep(5)
|
|||
|
|
continue
|
|||
|
|
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
scan_data, scan_angles = lidar.get_scan()
|
|||
|
|
|
|||
|
|
if scan_data is None or scan_angles is None:
|
|||
|
|
logger.warning("⚠️ LIDAR scan failed or incomplete, skipping iteration.")
|
|||
|
|
time.sleep(0.1)
|
|||
|
|
continue
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"❌ Error getting LIDAR scan: {e}")
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
continue
|
|||
|
|
|
|||
|
|
if not scan_data or len(scan_data) != sensor_description.scan_size:
|
|||
|
|
logger.warning(f"Received invalid scan data (length {len(scan_data)}), skipping SLAM update.")
|
|||
|
|
time.sleep(0.1)
|
|||
|
|
continue
|
|||
|
|
|
|||
|
|
heading = None
|
|||
|
|
if imu:
|
|||
|
|
try:
|
|||
|
|
heading = imu.get_heading()
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.warning(f"⚠️ Could not get IMU heading: {e}")
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
slam.update(scan_data, scan_angles_degrees=scan_angles)
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"❌ Error updating SLAM: {e}")
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
continue
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
pose = slam.getpos()
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"❌ Error getting SLAM pose: {e}")
|
|||
|
|
|
|||
|
|
#Let the wall follower take control
|
|||
|
|
try:
|
|||
|
|
logger.debug("📡 About to call wall_follower.step()")
|
|||
|
|
wall_follower.step(scan_data)
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"⚠️ Wall follower error: {e}")
|
|||
|
|
|
|||
|
|
current_time = time.time()
|
|||
|
|
if current_time - last_map_update_time > 0.1:
|
|||
|
|
try:
|
|||
|
|
slam.getmap(mapbytes)
|
|||
|
|
map_np = np.reshape(mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
|
|||
|
|
last_map_update_time = current_time
|
|||
|
|
|
|||
|
|
if visualization_enabled:
|
|||
|
|
x_pix = int(pose[0] * MM_TO_PIXELS + viz_center_x)
|
|||
|
|
y_pix = int(pose[1] * MM_TO_PIXELS + viz_center_y)
|
|||
|
|
|
|||
|
|
path.append((x_pix, y_pix))
|
|||
|
|
max_path_points = 2000
|
|||
|
|
if len(path) > max_path_points:
|
|||
|
|
path = path[-max_path_points:]
|
|||
|
|
path_x, path_y = zip(*path) if path else ([], [])
|
|||
|
|
|
|||
|
|
map_im.set_data(map_np)
|
|||
|
|
robot_dot.set_data([x_pix], [y_pix])
|
|||
|
|
path_line.set_data(path_x, path_y)
|
|||
|
|
|
|||
|
|
plt.draw()
|
|||
|
|
plt.pause(0.001)
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"❌ Error updating map/visualization: {e}")
|
|||
|
|
visualization_enabled = False
|
|||
|
|
|
|||
|
|
loop_duration = time.time() - loop_start_time
|
|||
|
|
sleep_time = LIDAR_UPDATE_INTERVAL - loop_duration
|
|||
|
|
if sleep_time > 0:
|
|||
|
|
time.sleep(sleep_time)
|
|||
|
|
|
|||
|
|
except KeyboardInterrupt:
|
|||
|
|
print("\n🛑 Mapping stopped by user.")
|
|||
|
|
logger.info("🛑 KeyboardInterrupt received, stopping mapping.")
|
|||
|
|
|
|||
|
|
# Stop motors FIRST
|
|||
|
|
try:
|
|||
|
|
if 'wall_follower' in locals():
|
|||
|
|
wall_follower.stop()
|
|||
|
|
elif 'chassis' in locals() and chassis:
|
|||
|
|
chassis.reset_motors()
|
|||
|
|
logger.info("✅ Motors stopped.")
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.warning(f"⚠️ Could not stop motors: {e}")
|
|||
|
|
|
|||
|
|
# Save the map
|
|||
|
|
try:
|
|||
|
|
slam.getmap(mapbytes)
|
|||
|
|
final_map = np.reshape(mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
|
|||
|
|
np.save("slam_occupancy_grid.npy", final_map)
|
|||
|
|
plt.imsave("slam_map_final.png", final_map, cmap=cmap, origin='lower')
|
|||
|
|
print("💾 Map saved to slam_map_final.png and slam_occupancy_grid.npy.")
|
|||
|
|
logger.info("💾 Map saved successfully.")
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"❌ Failed to save final map: {e}")
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.exception(f"🚨 An unexpected error occurred in the main loop: {e}")
|
|||
|
|
|
|||
|
|
finally:
|
|||
|
|
print("🧹 Cleaning up...")
|
|||
|
|
logger.info("🧹 Cleaning up resources...")
|
|||
|
|
|
|||
|
|
# Ensure motors are stopped
|
|||
|
|
try:
|
|||
|
|
if 'wall_follower' in locals():
|
|||
|
|
wall_follower.stop()
|
|||
|
|
elif 'chassis' in locals() and chassis:
|
|||
|
|
chassis.reset_motors()
|
|||
|
|
logger.info("✅ Motors stopped (from finally block).")
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.warning(f"⚠️ Could not stop motors in finally: {e}")
|
|||
|
|
|
|||
|
|
# Stop LIDAR
|
|||
|
|
if 'lidar' in locals() and lidar:
|
|||
|
|
try:
|
|||
|
|
lidar.stop()
|
|||
|
|
logger.info("✅ LIDAR stopped.")
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"❌ Error stopping LIDAR: {e}")
|
|||
|
|
|
|||
|
|
# Shutdown visualization
|
|||
|
|
if visualization_enabled and 'plt' in locals():
|
|||
|
|
try:
|
|||
|
|
if not RUNNING_IN_GUI:
|
|||
|
|
plt.ioff()
|
|||
|
|
logger.info("ℹ️ Visualization ended cleanly.")
|
|||
|
|
# Avoid showing the plot interactively
|
|||
|
|
except Exception as e:
|
|||
|
|
logger.error(f"❌ Error handling plot closure: {e}")
|
|||
|
|
|
|||
|
|
|