YashsharmaPhD/SLAM_simulation

1import gradio as gr2import matplotlib.pyplot as plt3import matplotlib.image as mpimg4import numpy as np5import random6import threading7import time8 9# Global state10pose = {"x": 0, "z": 0, "angle": 0}11trajectory = [(0, 0)]12obstacle_hits = []13color_index = 014rgb_colors = ['red', 'green', 'blue']15noise_enabled = True16obstacles = []17auto_mode = False18 19def generate_obstacles(count=10):20    return [{21        "x": random.uniform(-8, 8),22        "z": random.uniform(-8, 8),23        "radius": random.uniform(0.5, 1.2)24    } for _ in range(count)]25 26obstacles = generate_obstacles(10)27 28def toggle_noise():29    global noise_enabled30    noise_enabled = not noise_enabled31    return "Noise: ON" if noise_enabled else "Noise: OFF"32 33def reset_sim(count):34    global pose, trajectory, obstacles, obstacle_hits, color_index35    pose = {"x": 0, "z": 0, "angle": 0}36    trajectory = [(0, 0)]37    obstacle_hits.clear()38    color_index = 039    obstacles[:] = generate_obstacles(int(count))40    return render_env(), render_slam_map(), f"Simulation Reset with {count} obstacles"41 42def check_collision(x, z):43    for obs in obstacles:44        dist = np.sqrt((obs["x"] - x)**2 + (obs["z"] - z)**2)45        if dist <= obs["radius"] + 0.2:46            return True47    return False48 49def move_robot(direction):50    global pose, trajectory51    step = 152    direction = direction.upper()53 54    if direction == "W":55        new_x, new_z = pose["x"], pose["z"] + step56        pose["angle"] = 9057    elif direction == "S":58        new_x, new_z = pose["x"], pose["z"] - step59        pose["angle"] = -9060    elif direction == "A":61        new_x, new_z = pose["x"] - step, pose["z"]62        pose["angle"] = 18063    elif direction == "D":64        new_x, new_z = pose["x"] + step, pose["z"]65        pose["angle"] = 066    else:67        return render_env(), render_slam_map(), "❌ Invalid Key"68 69    if check_collision(new_x, new_z):70        return render_env(), render_slam_map(), "🚫 Collision detected!"71 72    pose["x"], pose["z"] = new_x, new_z73 74    if noise_enabled:75        noisy_x = pose["x"] + random.uniform(-0.1, 0.1)76        noisy_z = pose["z"] + random.uniform(-0.1, 0.1)77        trajectory.append((noisy_x, noisy_z))78    else:79        trajectory.append((pose["x"], pose["z"]))80 81    return render_env(), render_slam_map(), f"Moved {direction}"82 83def render_env():84    global obstacle_hits85    fig, ax = plt.subplots(figsize=(5,5))86    ax.set_xlim(-10, 10)87    ax.set_ylim(-10, 10)88    ax.set_title("SLAM Environment View")89 90    try:91        bg = mpimg.imread("map.png")92        ax.imshow(bg, extent=(-10, 10, -10, 10), alpha=0.2)93    except FileNotFoundError:94        pass95 96    for obs in obstacles:97        circ = plt.Circle((obs["x"], obs["z"]), obs["radius"], color="gray", alpha=0.6)98        ax.add_patch(circ)99 100    ax.plot(pose["x"], pose["z"], 'ro', markersize=8)101 102    # Clear previous hits to avoid infinite growth103    obstacle_hits.clear()104 105    angles = np.linspace(0, 2*np.pi, 24)106    for ang in angles:107        for r in np.linspace(0, 3, 30):108            scan_x = pose["x"] + r * np.cos(ang)109            scan_z = pose["z"] + r * np.sin(ang)110            if check_collision(scan_x, scan_z):111                ax.plot([pose["x"], scan_x], [pose["z"], scan_z], 'g-', linewidth=0.5)112                obstacle_hits.append((scan_x, scan_z))113                break114 115    plt.close(fig)116    return fig117 118def render_slam_map():119    global color_index120    fig, ax = plt.subplots(figsize=(5,5))121    ax.set_title("SLAM Trajectory Map")122    x_vals = [x for x, z in trajectory]123    z_vals = [z for x, z in trajectory]124    ax.plot(x_vals, z_vals, 'bo-', markersize=3)125    ax.grid(True)126 127    if obstacle_hits:128        current_color = rgb_colors[color_index % len(rgb_colors)]129        for hit in obstacle_hits[-20:]:130            ax.plot(hit[0], hit[1], 'o', color=current_color, markersize=6)131        color_index += 1132 133    plt.close(fig)134    return fig135 136def handle_text_input(direction):137    return move_robot(direction.strip().upper())138 139def auto_movement(update_callback):140    global auto_mode141    directions = ['W', 'A', 'S', 'D']142    while auto_mode:143        direction = random.choice(directions)144        env, slam, msg = move_robot(direction)145        update_callback(env, slam, msg)146        time.sleep(1)147 148def toggle_auto_mode(env_plot, slam_plot, status_text):149    global auto_mode150    auto_mode = not auto_mode151 152    if auto_mode:153        def update_ui(e, s, t):154            env_plot.update(value=e)155            slam_plot.update(value=s)156            status_text.update(value=t)157 158        thread = threading.Thread(target=auto_movement, args=(update_ui,), daemon=True)159        thread.start()160        return "🟢 Auto Mode: ON"161    else:162        return "⚪ Auto Mode: OFF"163 164# Gradio UI165with gr.Blocks() as demo:166    gr.Markdown("## 🤖 SLAM Simulation with Auto Mode + Collision Status")167 168    obstacle_slider = gr.Slider(1, 20, value=10, step=1, label="Number of Obstacles")169    direction_input = gr.Textbox(label="Type W / A / S / D and press Enter", placeholder="e.g., W")170    status_text = gr.Textbox(label="Status", interactive=False)171 172    with gr.Row():173        with gr.Column():174            env_plot = gr.Plot(label="Robot View")175        with gr.Column():176            slam_plot = gr.Plot(label="SLAM Map")177 178    with gr.Row():179        w = gr.Button("⬆️ W")180        a = gr.Button("⬅️ A")181        s = gr.Button("⬇️ S")182        d = gr.Button("➡️ D")183        reset = gr.Button("🔄 Reset")184        toggle = gr.Button("🔀 Toggle Noise")185        auto = gr.Button("🤖 Toggle Auto")186 187    w.click(fn=lambda: move_robot("W"), outputs=[env_plot, slam_plot, status_text])188    a.click(fn=lambda: move_robot("A"), outputs=[env_plot, slam_plot, status_text])189    s.click(fn=lambda: move_robot("S"), outputs=[env_plot, slam_plot, status_text])190    d.click(fn=lambda: move_robot("D"), outputs=[env_plot, slam_plot, status_text])191 192    reset.click(fn=reset_sim, inputs=[obstacle_slider], outputs=[env_plot, slam_plot, status_text])193    toggle.click(fn=lambda: (None, None, toggle_noise()), outputs=[env_plot, slam_plot, status_text])194    auto.click(fn=toggle_auto_mode, inputs=[env_plot, slam_plot, status_text], outputs=status_text)195    direction_input.submit(fn=handle_text_input, inputs=direction_input, outputs=[env_plot, slam_plot, status_text])196 197demo.launch()198