← ROS Nodes and Gazebo

TurtleBot State Estimation: From Software to Gazebo Simulation

In the TurtleBot ROS Deployment tutorial, we ran our navigation system using a software simulator node. Now we’ll integrate with Gazebo, replacing our simple simulator with a full physics engine.

Key Insight: Gazebo is not a visualization tool—it’s a ROS node ecosystem!

When you launch Gazebo with a robot model, it automatically creates:

• Physics simulation node

• Sensor publisher nodes (/odom, /scan, /imu, etc.)

• Robot state publisher

• Transform (TF) broadcaster

Our task: Integrate our control nodes with Gazebo’s existing nodes.

Architecture Evolution: Software → Gazebo

Software-Only Simulation (previous notebook):

waypoint_generator → /reference
         ↓
velocity_controller → /cmd_vel
         ↓
turtlebot_simulator → /odom  ← WE IMPLEMENTED THIS
         ↓
kalman_filter → /estimate

Gazebo Integration (this notebook):

waypoint_generator → /reference
         ↓
velocity_controller → /cmd_vel
         ↓
GAZEBO (physics engine) → /odom  ← GAZEBO PROVIDES THIS
         ↓
kalman_filter → /estimate

What Changed:

• ❌ Remove: turtlebot_simulator node

• ✓ Add: Gazebo launch

• ✓ Unchanged: waypoint generator, controller, Kalman filter

Note

The controller and observer don’t know or care whether /odom comes from our simulator, Gazebo, or a real robot!

Conceptual Foundation: From Dynamical Systems to Gazebo

Theory: Composition of Dynamical Systems

From the theory notebook, we modeled the system as a composition of dynamical systems:

Each block is a DynamicalSystem with state evolution f and observation h functions. The observer (Extended Kalman Filter) corrects odometry drift.

ROS Deployment: Distributed Nodes

From the ROS deployment notebook, we wrapped these systems as ROS nodes:

Each dynamical system became a ROS node communicating via topics. The TurtleBot simulator node provided /odom measurements.

Gazebo Integration: Same Architecture, Physics Engine

Now we replace the software simulator with Gazebo’s physics simulation:

Notice: The Gazebo node (in red) replaces the TurtleBot simulator node. Everything else remains identical—waypoint generator, controller, and Kalman filter don’t know whether /odom comes from our simulator or Gazebo!

This is the power of ROS: Interface-based modularity allows swapping implementations without changing dependent nodes.

Teleoperation with Gazebo

Just as in the ROS deployment notebook, teleoperation provides direct velocity control by publishing commands straight to /cmd_vel:

Architecture Modes:

• Autonomous Navigation (this notebook): Waypoint Generator → Velocity Controller → /cmd_vel → Gazebo

• Teleoperation: Keyboard Teleop → /cmd_vel → Gazebo

When using teleoperation with Gazebo, the teleop node bypasses the waypoint generator and position controller entirely, sending velocity commands directly to Gazebo’s /cmd_vel topic. The Kalman filter continues running to provide state estimates for monitoring.

Note

This tutorial implements the autonomous navigation architecture. For teleoperation with Gazebo, you would simply run a teleop node (e.g., teleop_twist_keyboard) that publishes to /cmd_vel, and the Gazebo simulation would respond exactly as it does to our controller commands!

What Gazebo Provides

When we launch Gazebo with the TurtleBot3 Waffle model:

Input Topics (Gazebo subscribes):

• /cmd_vel (Twist) - Velocity commands for the robot

Output Topics (Gazebo publishes):

• /odom (Odometry) - Wheel odometry with realistic noise

• /scan (LaserScan) - 360° LiDAR data

• /imu (Imu) - Inertial measurement unit

• /camera/image_raw (Image) - Camera feed

• /tf (TFMessage) - Coordinate frame transforms

For our state estimation, we only need /cmd_vel (input) and /odom (output).

Physics Simulation Features:

• Wheel slip and friction

• Robot inertia and dynamics

• Realistic sensor noise models

• Collision detection

• Same code works on real hardware!

Setup: Imports

import numpy as np
import matplotlib.pyplot as plt
from pykal import DynamicalSystem
from pykal.ros.ros_node import ROSNode
from pykal.algorithm_library.estimators.kf import KF
from pykal.gazebo import start_gazebo, stop_gazebo

# ROS message types
from geometry_msgs.msg import Twist, PoseStamped, Vector3
from nav_msgs.msg import Odometry
import rclpy
import time

print("Imports successful! Ready to integrate with Gazebo.")

Component Functions (Reuse from ROS Deployment)

# ============================================================================
# TurtleBot Dynamics (for Kalman filter prediction)
# ============================================================================

def turtlebot_f(xk: np.ndarray, uk: np.ndarray, dt: float) -> np.ndarray:
    """Unicycle kinematics: [x, y, theta, v, omega]."""
    x, y, theta, v, omega = xk.flatten()
    v_cmd, omega_cmd = uk.flatten()
    
    x_new = x + v * np.cos(theta) * dt
    y_new = y + v * np.sin(theta) * dt
    theta_new = np.arctan2(np.sin(theta + omega * dt), np.cos(theta + omega * dt))
    
    return np.array([[x_new], [y_new], [theta_new], [v_cmd], [omega_cmd]])


def turtlebot_h(xk: np.ndarray) -> np.ndarray:
    """Measurement: [x, y, theta] from odometry."""
    return xk[:3, :]


def compute_F_jacobian(xhat: np.ndarray, dt: float) -> np.ndarray:
    """Jacobian of dynamics."""
    _, _, theta, v, _ = xhat.flatten()
    return np.array([
        [1, 0, -v * np.sin(theta) * dt, np.cos(theta) * dt, 0],
        [0, 1,  v * np.cos(theta) * dt, np.sin(theta) * dt, 0],
        [0, 0,  1, 0, dt],
        [0, 0,  0, 1, 0],
        [0, 0,  0, 0, 1]
    ])


def compute_H_jacobian() -> np.ndarray:
    """Jacobian of measurement."""
    return np.array([
        [1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0],
        [0, 0, 1, 0, 0]
    ])


# Noise covariances
Q_turtlebot = np.diag([0.01, 0.01, 0.02, 0.1, 0.1])
R_turtlebot = np.diag([0.05, 0.05, 0.1])

print("TurtleBot dynamics and Jacobians defined!")

---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
Cell In[1], line 5
      1 # ============================================================================
      2 # TurtleBot Dynamics (for Kalman filter prediction)
      3 # ============================================================================
----> 5 def turtlebot_f(xk: np.ndarray, uk: np.ndarray, dt: float) -> np.ndarray:
      6     """Unicycle kinematics: [x, y, theta, v, omega]."""
      7     x, y, theta, v, omega = xk.flatten()

NameError: name 'np' is not defined

Step 1: Launch Gazebo with TurtleBot3

We use pykal.gazebo.start_gazebo() to launch Gazebo with the TurtleBot3 Waffle model.

This replaces our software simulator node!

# Launch Gazebo
print("Launching Gazebo with TurtleBot3 Waffle...")
print("This may take 10-20 seconds...\n")

gz = start_gazebo(
    robot='turtlebot3',
    world='empty_world',  # Simple empty world for testing
    headless=False,       # Set True to run without GUI (faster)
    x_pose=0.0,
    y_pose=0.0,
    z_pose=0.01,
    yaw=0.0
)

print("✓ Gazebo launched successfully!")
print(f"\nGazebo is now running and publishing:")
print(f"  - /odom (Odometry): Wheel odometry with noise")
print(f"  - /scan (LaserScan): LiDAR data")
print(f"  - /imu (Imu): IMU data")
print(f"  - /tf (TF): Coordinate transforms")
print(f"\nGazebo is listening on:")
print(f"  - /cmd_vel (Twist): Velocity commands")

# Give Gazebo time to initialize
import time as pytime
pytime.sleep(3)
print("\nGazebo initialization complete!")

Step 2: Node Architecture (Without Simulator)

We create the same control nodes as before, except the simulator:

Changes from software simulation:

1. ❌ Remove turtlebot_simulator node (Gazebo replaces it)

2. ✓ Keep waypoint_generator (unchanged)

3. ✓ Keep velocity_controller (unchanged)

4. ✓ Keep kalman_filter (unchanged, subscribes to Gazebo’s /odom)

Power of ROS: Same controller and observer code works with Gazebo!

Node 1: Waypoint Generator (Unchanged)

def create_waypoint_generator_node(waypoints, switch_time=15.0, rate_hz=10.0):
    """Same as before - generates reference poses."""
    current_idx = 0
    time_at_waypoint = 0.0
    last_tk = 0.0
    
    def waypoint_callback(tk):
        nonlocal current_idx, time_at_waypoint, last_tk
        
        dt = tk - last_tk if last_tk > 0 else 0.0
        last_tk = tk
        time_at_waypoint += dt
        
        if time_at_waypoint >= switch_time:
            current_idx = (current_idx + 1) % len(waypoints)
            time_at_waypoint = 0.0
        
        x_r, y_r, theta_r = waypoints[current_idx]
        qx, qy = 0.0, 0.0
        qz = np.sin(theta_r / 2.0)
        qw = np.cos(theta_r / 2.0)
        
        reference = np.array([tk, x_r, y_r, 0.0, qx, qy, qz, qw])
        return {'reference': reference}
    
    node = ROSNode(
        node_name='waypoint_generator',
        callback=waypoint_callback,
        subscribes_to=[],
        publishes_to=[('reference', PoseStamped, '/reference')],
        rate_hz=rate_hz
    )
    return node


# Create waypoint generator
square_waypoints = [
    (1.0, 0.0, 0.0),
    (1.0, 1.0, np.pi/2),
    (0.0, 1.0, np.pi),
    (0.0, 0.0, -np.pi/2)
]

waypoint_node = create_waypoint_generator_node(
    waypoints=square_waypoints,
    switch_time=15.0,
    rate_hz=10.0
)

print("✓ Waypoint generator node created (unchanged from software version)")

Node 2: Velocity Controller (Unchanged)

def create_velocity_controller_node(Kv=0.3, Komega=1.0, rate_hz=50.0):
    """Same as before - generates velocity commands."""
    def controller_callback(tk, reference, estimate):
        x_r, y_r = reference[1], reference[2]
        qz_r, qw_r = reference[6], reference[7]
        theta_r = np.arctan2(2.0 * qw_r * qz_r, 1.0 - 2.0 * qz_r**2)
        
        x, y = estimate[0], estimate[1]
        qz, qw = estimate[5], estimate[6]
        theta = np.arctan2(2.0 * qw * qz, 1.0 - 2.0 * qz**2)
        
        dx = x_r - x
        dy = y_r - y
        distance = np.sqrt(dx**2 + dy**2)
        
        heading_error = theta_r - theta
        heading_error = np.arctan2(np.sin(heading_error), np.cos(heading_error))
        
        v_cmd = Kv * distance
        omega_cmd = Komega * heading_error
        
        v_cmd = np.clip(v_cmd, -0.22, 0.22)
        omega_cmd = np.clip(omega_cmd, -2.84, 2.84)
        
        cmd_vel = np.array([v_cmd, 0.0, 0.0, 0.0, 0.0, omega_cmd])
        return {'cmd_vel': cmd_vel}
    
    node = ROSNode(
        node_name='velocity_controller',
        callback=controller_callback,
        subscribes_to=[
            ('/reference', PoseStamped, 'reference'),
            ('/estimate', Odometry, 'estimate'),
        ],
        publishes_to=[('cmd_vel', Twist, '/cmd_vel')],
        rate_hz=rate_hz,
        required_topics={'reference', 'estimate'}
    )
    return node


controller_node = create_velocity_controller_node(Kv=0.3, Komega=1.0, rate_hz=50.0)
print("✓ Velocity controller node created (unchanged from software version)")
print("  → Publishes to /cmd_vel (Gazebo will receive these commands!)")

Node 3: Kalman Filter (Uses Gazebo’s /odom)

The Kalman filter is unchanged algorithmically, but now subscribes to Gazebo’s /odom instead of our simulator’s /odom.

Key Point: The KF doesn’t know the difference! It just processes Odometry messages.

def create_kalman_filter_node(dt=0.1, rate_hz=10.0, Q=None, R=None):
    """Kalman filter using Gazebo's /odom measurements."""
    if Q is None:
        Q = Q_turtlebot
    if R is None:
        R = R_turtlebot
    
    xhat = np.array([[0.0], [0.0], [0.0], [0.0], [0.0]])
    P = np.diag([0.1, 0.1, 0.1, 1.0, 1.0])
    
    def filter_callback(tk, odom, cmd_vel):
        """
        NOTE: Now 'odom' comes from Gazebo, not our simulator!
        """
        nonlocal xhat, P
        
        # Extract measurement from Gazebo's Odometry message
        x_meas, y_meas = odom[0], odom[1]
        qz_meas, qw_meas = odom[5], odom[6]
        theta_meas = np.arctan2(2.0 * qw_meas * qz_meas, 1.0 - 2.0 * qz_meas**2)
        yk = np.array([[x_meas], [y_meas], [theta_meas]])
        
        uk = np.array([[cmd_vel[0]], [cmd_vel[5]]])
        
        Fk = compute_F_jacobian(xhat, dt)
        Hk = compute_H_jacobian()
        
        f_params = {'xk': xhat, 'uk': uk, 'dt': dt}
        h_params = {'xk': xhat}
        
        xhat_new, P_new = KF.f(
            x_P=(xhat, P),
            y=yk,
            u=uk,
            f=turtlebot_f,
            F=lambda **params: Fk,
            Q=lambda **params: Q,
            h=turtlebot_h,
            H=lambda **params: Hk,
            R=lambda **params: R,
            f_params=f_params,
            F_params={},
            Q_params={},
            h_params=h_params,
            H_params={},
            R_params={}
        )
        
        xhat = xhat_new
        P = P_new
        
        x_est, y_est, theta_est, v_est, omega_est = xhat.flatten()
        qx, qy = 0.0, 0.0
        qz_est = np.sin(theta_est / 2.0)
        qw_est = np.cos(theta_est / 2.0)
        
        estimate = np.array([
            x_est, y_est, 0.0,
            qx, qy, qz_est, qw_est,
            v_est * np.cos(theta_est), v_est * np.sin(theta_est), 0.0,
            0.0, 0.0, omega_est
        ])
        
        return {'estimate': estimate}
    
    node = ROSNode(
        node_name='kalman_filter',
        callback=filter_callback,
        subscribes_to=[
            ('/odom', Odometry, 'odom'),  # ← FROM GAZEBO!
            ('/cmd_vel', Twist, 'cmd_vel'),
        ],
        publishes_to=[('estimate', Odometry, '/estimate')],
        rate_hz=rate_hz,
        required_topics={'odom', 'cmd_vel'}
    )
    return node


kf_node = create_kalman_filter_node(dt=0.1, rate_hz=10.0)
print("✓ Kalman filter node created")
print("  → Subscribes to /odom (provided by Gazebo!)")
print("  → Subscribes to /cmd_vel (to predict state)")
print("  → Publishes /estimate")

Step 3: Run the Integrated System

Now we start our nodes and let them interact with Gazebo:

# Initialize ROS2
rclpy.init()

# Create nodes
print("Creating ROS2 nodes...")
waypoint_node.create_node()
controller_node.create_node()
kf_node.create_node()
print("All nodes created!")

# Start nodes
print("\nStarting nodes...")
waypoint_node.start()
print("  ✓ Waypoint generator running")
kf_node.start()
print("  ✓ Kalman filter running (using Gazebo's /odom!)")
controller_node.start()
print("  ✓ Velocity controller running (sending /cmd_vel to Gazebo!)")

print("\n🚀 TurtleBot Gazebo integration is live!")
print("\nSystem Architecture:")
print("  waypoint_generator → /reference")
print("           ↓")
print("  velocity_controller → /cmd_vel → GAZEBO (physics simulation)")
print("                                      ↓")
print("                                   /odom (with realistic noise)")
print("                                      ↓")
print("                             kalman_filter → /estimate")
print("                                      ↓ (feedback)")
print("                             velocity_controller")

print("\n💡 Tip: Open Gazebo GUI to watch the TurtleBot move!")
print("💡 Tip: Run 'rqt_graph' to see Gazebo nodes in the ROS graph!")

Data Logger (For Analysis)

# Create data logger
def create_data_logger_node():
    data_log = {
        'time': [],
        'reference': [],
        'cmd_vel': [],
        'odom': [],
        'estimate': []
    }
    
    def logger_callback(tk, reference, cmd_vel, odom, estimate):
        data_log['time'].append(tk)
        data_log['reference'].append(reference.copy())
        data_log['cmd_vel'].append(cmd_vel.copy())
        data_log['odom'].append(odom.copy())
        data_log['estimate'].append(estimate.copy())
        return {}
    
    node = ROSNode(
        node_name='data_logger',
        callback=logger_callback,
        subscribes_to=[
            ('/reference', PoseStamped, 'reference'),
            ('/cmd_vel', Twist, 'cmd_vel'),
            ('/odom', Odometry, 'odom'),
            ('/estimate', Odometry, 'estimate'),
        ],
        publishes_to=[],
        rate_hz=10.0
    )
    return node, data_log


logger_node, data_log = create_data_logger_node()
logger_node.create_node()
logger_node.start()
print("Data logger started!")

# Run for 60 seconds
T_sim = 60.0
print(f"\nRunning Gazebo simulation for {T_sim} seconds...")
print("Watch the TurtleBot in Gazebo GUI!\n")
pytime.sleep(T_sim)

print(f"\nSimulation complete! Collected {len(data_log['time'])} samples.")

Stop the System

# Stop ROS nodes
print("Stopping ROS nodes...")
logger_node.stop()
waypoint_node.stop()
controller_node.stop()
kf_node.stop()
print("All nodes stopped.")

# Shutdown ROS2
rclpy.shutdown()
print("ROS2 shutdown complete.")

# Stop Gazebo
print("\nStopping Gazebo...")
stop_gazebo(gz)
print("Gazebo stopped.")

Visualization: Gazebo vs Software Simulation

Show code cell source

Hide code cell source

# Convert to arrays
time_vec = np.array(data_log['time'])
odom_data = np.array(data_log['odom'])
estimates = np.array(data_log['estimate'])
commands = np.array(data_log['cmd_vel'])
references = np.array(data_log['reference'])

# Extract positions
odom_x = odom_data[:, 0]
odom_y = odom_data[:, 1]
est_x = estimates[:, 0]
est_y = estimates[:, 1]
ref_x = references[:, 1]
ref_y = references[:, 2]

# Plotting
fig, axs = plt.subplots(2, 2, figsize=(14, 10))

# Plot 1: 2D Trajectory
ax = axs[0, 0]
ax.plot(odom_x, odom_y, 'gray', linewidth=1, label='Gazebo Odometry (noisy)', alpha=0.5)
ax.plot(est_x, est_y, 'r--', linewidth=2, label='KF Estimate', alpha=0.8)
ax.scatter(ref_x[::50], ref_y[::50], c='green', s=100, marker='*', label='Waypoints', zorder=5)
ax.set_xlabel('X Position (m)', fontsize=11)
ax.set_ylabel('Y Position (m)', fontsize=11)
ax.set_title('Gazebo Integration: 2D Trajectory', fontsize=13, fontweight='bold')
ax.legend()
ax.grid(True, alpha=0.3)
ax.axis('equal')

# Plot 2: X Position
ax = axs[0, 1]
ax.plot(time_vec, odom_x, 'gray', label='Gazebo Odom', alpha=0.5)
ax.plot(time_vec, est_x, 'r--', label='KF Estimate', alpha=0.8)
ax.set_ylabel('X Position (m)', fontsize=11)
ax.set_title('X Position: Gazebo Odometry', fontsize=13, fontweight='bold')
ax.legend()
ax.grid(True, alpha=0.3)

# Plot 3: Y Position
ax = axs[1, 0]
ax.plot(time_vec, odom_y, 'gray', label='Gazebo Odom', alpha=0.5)
ax.plot(time_vec, est_y, 'r--', label='KF Estimate', alpha=0.8)
ax.set_xlabel('Time (s)', fontsize=11)
ax.set_ylabel('Y Position (m)', fontsize=11)
ax.set_title('Y Position: Gazebo Odometry', fontsize=13, fontweight='bold')
ax.legend()
ax.grid(True, alpha=0.3)

# Plot 4: Control Commands
ax = axs[1, 1]
ax.plot(time_vec, commands[:, 0], 'g-', label='Linear Velocity', linewidth=1.5)
ax_omega = ax.twinx()
ax_omega.plot(time_vec, commands[:, 5], 'purple', label='Angular Velocity', linewidth=1.5, alpha=0.7)
ax.set_xlabel('Time (s)', fontsize=11)
ax.set_ylabel('Linear Velocity (m/s)', fontsize=11, color='g')
ax_omega.set_ylabel('Angular Velocity (rad/s)', fontsize=11, color='purple')
ax.set_title('Commands to Gazebo', fontsize=13, fontweight='bold')
ax.tick_params(axis='y', labelcolor='g')
ax_omega.tick_params(axis='y', labelcolor='purple')
ax.grid(True, alpha=0.3)

plt.tight_layout()
plt.show()

print(f"\nGazebo Integration Performance:")
print(f"  Total samples: {len(time_vec)}")
print(f"  Simulation time: {time_vec[-1]:.1f} seconds")
print(f"\nNote: Gazebo provides realistic physics simulation!")
print(f"  - Wheel slip, friction, inertia")
print(f"  - Sensor noise from wheel encoders")
print(f"  - Same code works on real hardware!")

Summary: The Complete Pipeline

We’ve completed the full Theory → Software → ROS → Gazebo pipeline!

Step 1: Theory → Software (theory_to_python)

• Designed system as DynamicalSystem components

• Tested with NumPy arrays in Python

• Direct function calls, shared parameter dictionary

Step 2: Software → ROS (python_to_ros)

• Wrapped DynamicalSystem as ROSNode

• Created software simulator node

• Distributed ROS system with topics

Step 3: ROS → Gazebo (this notebook)

• Removed software simulator

• Connected to Gazebo’s physics simulation

• Same controller and observer code!

Step 4: Gazebo → Hardware (not shown, but trivial!)

• Remove Gazebo launch

• Connect to real TurtleBot’s /odom topic

• Same controller and observer code!

What Changed at Each Step:

Step	Changed	Unchanged
Theory → ROS	Wrapped as ROSNode	Dynamics, KF, controller
ROS → Gazebo	Removed simulator	Waypoint, controller, KF
Gazebo → Hardware	Launch command	Waypoint, controller, KF

ROS Graph Evolution:

Software:  our_nodes → turtlebot_simulator → /odom
Gazebo:    our_nodes → GAZEBO → /odom
Hardware:  our_nodes → ROBOT → /odom

The Beauty of ROS: The controller and observer are completely decoupled from the source of /odom. They work identically with:

• Software simulation (Python)

• Physics simulation (Gazebo)

• Real hardware (TurtleBot3)

This is the essence of modular robotics architecture!

Architecture 2: Teleoperation with Gazebo

In the previous section, we demonstrated autonomous navigation with Gazebo. Now we’ll demonstrate teleoperation mode with Gazebo, where a human operator directly controls the robot using keyboard input.

Key Architectural Difference:

• Autonomous (previous): Waypoint Gen → Controller → /cmd_vel → Gazebo → Observer

• Teleoperation (this section): Keyboard Teleop → /cmd_vel → Gazebo → Observer

The beauty of ROS: Gazebo doesn’t care whether /cmd_vel comes from an autonomous controller or a teleop node—it responds identically!

Nodes Used in Teleoperation with Gazebo:

1. 🔴 Keyboard Teleop - External tool (teleop_twist_keyboard)

2. 🔴 Gazebo - Physics simulation (responds to /cmd_vel)

3. ✅ Kalman Filter - Continues providing state estimates

4. ❌ Waypoint Generator - Not needed (bypassed)

5. ❌ Velocity Controller - Not needed (bypassed)

Note

This demonstrates the same teleoperation architecture from the ROS deployment notebook, but with Gazebo’s physics simulation instead of the Python simulator!

# Launch Gazebo (if not already running from previous section)
print("Launching Gazebo with TurtleBot3...")
print("This may take 10-15 seconds...\n")

gz_teleop = start_gazebo(
    robot='turtlebot3',
    world='empty_world',
    headless=False,  # Keep GUI for visual feedback during teleop
    x_pose=0.0,
    y_pose=0.0,
    z_pose=0.0,
    yaw=0.0
)

print("✓ Gazebo launched successfully!")
print("\nGazebo is publishing:")
print("  /odom (Odometry)")
print("\nGazebo is listening on:")
print("  /cmd_vel (Twist) ← waiting for teleop or controller")

# Give Gazebo time to initialize
import time as pytime
pytime.sleep(3)

# Re-initialize ROS2 if needed
if not rclpy.ok():
    rclpy.init()

# Create only Kalman filter node (NO waypoint gen, NO controller)
print("\nCreating Kalman filter for teleoperation mode...")
kf_gazebo_teleop = create_kalman_filter_node(dt=0.1, rate_hz=10.0, Q=Q_turtlebot, R=R_turtlebot)
kf_gazebo_teleop.create_node()
kf_gazebo_teleop.start()
print("✓ Kalman filter running")

print("\n🚀 System ready for teleoperation with Gazebo!")
print("\nActive Architecture:")
print("  (waiting for teleop) → /cmd_vel → GAZEBO → /odom → kalman_filter")
print("\nNOTE: Waypoint generator and controller are NOT running (bypassed!)")

# Create a logger to monitor the system during teleoperation
# (In real usage, you would manually control with keyboard)

def create_gazebo_teleop_logger():
    """Logger for Gazebo teleoperation monitoring."""
    gazebo_teleop_data = {
        "time": [],
        "odom": [],
        "estimate": [],
    }
    
    def logger_callback(tk, odom, estimate):
        gazebo_teleop_data["time"].append(tk)
        gazebo_teleop_data["odom"].append(odom.copy())
        gazebo_teleop_data["estimate"].append(estimate.copy())
        return {}
    
    node = ROSNode(
        node_name="gazebo_teleop_logger",
        callback=logger_callback,
        subscribes_to=[
            ("/odom", Odometry, "odom"),
            ("/estimate", Odometry, "estimate"),
        ],
        publishes_to=[],
        rate_hz=10.0)
    
    return node, gazebo_teleop_data


# Create and start logger
gazebo_teleop_logger, gazebo_teleop_data = create_gazebo_teleop_logger()
gazebo_teleop_logger.create_node()
gazebo_teleop_logger.start()

print("Logger started!")
print("\n" + "="*60)
print("TELEOPERATION MODE ACTIVE")
print("="*60)
print("\nTo control the robot manually:")
print("  1. Open a new terminal")
print("  2. Run: ros2 run teleop_twist_keyboard teleop_twist_keyboard")
print("  3. Use i/j/k/l keys to drive the TurtleBot in Gazebo")
print("\nThe Kalman filter is running and monitoring your commands!")
print("\nFor this demo, we'll collect 30 seconds of data...")
print("(In real usage, you would be driving the robot manually)")
print("="*60)

# Collect data for demonstration
T_gazebo_teleop = 30.0
print(f"\nCollecting {T_gazebo_teleop} seconds of teleoperation data...")
pytime.sleep(T_gazebo_teleop)

print(f"\nData collection complete! Collected {len(gazebo_teleop_data['time'])} samples.")

Summary: Dual Architectures with Gazebo

This notebook demonstrated two control architectures with Gazebo physics simulation:

Architecture 1: Autonomous Navigation (Waypoint Tracking)

Nodes Active:

• ✅ Waypoint Generator (pykal.ROSNode)

• ✅ Velocity Controller (pykal.ROSNode)

• 🔴 Gazebo (physics simulation)

• ✅ Kalman Filter (pykal.ROSNode)

Data Flow: Waypoint Gen → Controller → /cmd_vel → Gazebo → /odom → Observer

Use Case: Autonomous navigation in realistic physics environment

---

Architecture 2: Teleoperation (Manual Control)

Nodes Active:

• 🔴 Keyboard Teleop (external: teleop_twist_keyboard)

• 🔴 Gazebo (physics simulation)

• ✅ Kalman Filter (pykal.ROSNode)

Nodes Bypassed:

• ❌ Waypoint Generator

• ❌ Velocity Controller

Data Flow: Teleop → /cmd_vel → Gazebo → /odom → Observer

Use Case: Manual control with physics simulation for testing/intervention

---

Key Insights

1. Gazebo Agnostic: Gazebo doesn’t care whether /cmd_vel comes from autonomous or teleop

2. Same Interface: Both modes use /cmd_vel, demonstrating ROS modularity

3. Physics Preserved: Realistic dynamics, friction, inertia in both modes

4. Observer Monitoring: KF provides state estimates in both autonomous and manual modes

5. Red Nodes: Gazebo and teleop shown in RED (external tools, not pykal.ROSNode)

Architecture Consistency Across Pipeline:

• Theory (TurtleBot State Estimation): Pure Python DynamicalSystems

• ROS (ROS Deployment): pykal.ROSNode wrappers

• Gazebo (this notebook): Same ROS nodes + Gazebo physics

• Hardware (next step): Same code, real robot!

The same teleoperation pattern works across software simulation, Gazebo simulation, and hardware deployment—demonstrating true architecture portability!

# Stop nodes
print("Stopping teleoperation nodes...")
gazebo_teleop_logger.stop()
kf_gazebo_teleop.stop()

# Shutdown ROS2
rclpy.shutdown()
print("ROS2 shutdown complete.")

# Stop Gazebo
print("\nStopping Gazebo...")
stop_gazebo(gz_teleop)
print("Gazebo stopped.")

Stopping Teleoperation Mode

Running Keyboard Teleop with Gazebo

In a separate terminal, run the keyboard teleop tool:

ros2 run teleop_twist_keyboard teleop_twist_keyboard

Keyboard Controls:

• i - Move forward

• k - Stop

• , - Move backward

• j - Turn left

• l - Turn right

What Happens:

1. You press a key → teleop_twist_keyboard publishes to /cmd_vel

2. Gazebo receives /cmd_vel → Updates robot physics

3. Gazebo publishes /odom → Kalman filter estimates state

4. You see the TurtleBot move in the Gazebo GUI!

Tip

Try This:

1. Open Gazebo GUI (if using headless=False)

2. Run the teleop command in a terminal

3. Drive the robot around manually!

4. Watch the Kalman filter track your movements

For this notebook demo, we’ll create a programmatic logger to demonstrate the architecture (in real usage, you would manually control the robot):

Setting Up Teleoperation with Gazebo

For teleoperation mode, we only need:

1. Gazebo - Already provides the TurtleBot physics simulation

2. Kalman Filter - For state estimation and monitoring

3. Keyboard Teleop - External tool for manual control

We do NOT need the waypoint generator or velocity controller.

← ROS Nodes and Gazebo