{ "cells": [ { "cell_type": "markdown", "source": [ "[← ROS Nodes and Gazebo](../../../getting_started/ros_to_gazebo/ros_nodes_and_gazebo.rst)\n" ], "id": "cell-0", "metadata": {} }, { "cell_type": "markdown", "source": [ "# TurtleBot State Estimation: From Software to Gazebo Simulation\n" ], "id": "cell-1", "metadata": {} }, { "cell_type": "markdown", "source": [ "In the [TurtleBot ROS Deployment](../python_to_ros/turtlebot_ros_deployment.ipynb) 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.\n", "\n", "**Key Insight**: Gazebo is not a visualization tool—it's a ROS node ecosystem!\n", "\n", "When you launch Gazebo with a robot model, it automatically creates:\n", "- Physics simulation node\n", "- Sensor publisher nodes (`/odom`, `/scan`, `/imu`, etc.)\n", "- Robot state publisher\n", "- Transform (TF) broadcaster\n", "\n", "Our task: **Integrate our control nodes** with Gazebo's existing nodes.\n" ], "id": "cell-2", "metadata": {} }, { "cell_type": "markdown", "source": [ "## Architecture Evolution: Software → Gazebo\n", "\n", "**Software-Only Simulation** (previous notebook):\n", "\n", "```\n", "waypoint_generator → /reference\n", " ↓\n", "velocity_controller → /cmd_vel\n", " ↓\n", "turtlebot_simulator → /odom ← WE IMPLEMENTED THIS\n", " ↓\n", "kalman_filter → /estimate\n", "```\n", "\n", "**Gazebo Integration** (this notebook):\n", "\n", "```\n", "waypoint_generator → /reference\n", " ↓\n", "velocity_controller → /cmd_vel\n", " ↓\n", "GAZEBO (physics engine) → /odom ← GAZEBO PROVIDES THIS\n", " ↓\n", "kalman_filter → /estimate\n", "```\n", "\n", "**What Changed**:\n", "- ❌ Remove: `turtlebot_simulator` node\n", "- ✓ Add: Gazebo launch\n", "- ✓ Unchanged: waypoint generator, controller, Kalman filter\n", "\n", ":::{note}\n", "The controller and observer **don't know or care** whether `/odom` comes from our simulator, Gazebo, or a real robot!\n", ":::\n" ], "id": "cell-3", "metadata": {} }, { "cell_type": "markdown", "id": "cell-arch-diagram", "metadata": {}, "source": "## Conceptual Foundation: From Dynamical Systems to Gazebo\n\n### Theory: Composition of Dynamical Systems\n\nFrom the [theory notebook](../theory_to_python/turtlebot_state_estimation.ipynb), we modeled the system as a composition of dynamical systems:\n\n
\n \n
\n\nEach block is a `DynamicalSystem` with state evolution `f` and observation `h` functions. The observer (Extended Kalman Filter) corrects odometry drift.\n\n### ROS Deployment: Distributed Nodes \n\nFrom the [ROS deployment notebook](../python_to_ros/turtlebot_ros_deployment.ipynb), we wrapped these systems as ROS nodes:\n\n
\n \n
\n\nEach dynamical system became a ROS node communicating via topics. The TurtleBot simulator node provided `/odom` measurements.\n\n### Gazebo Integration: Same Architecture, Physics Engine\n\nNow we replace the software simulator with Gazebo's physics simulation:\n\n
\n \n
\n\n**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!\n\n**This is the power of ROS**: Interface-based modularity allows swapping implementations without changing dependent nodes.\n\n### Teleoperation with Gazebo\n\nJust as in the [ROS deployment notebook](../python_to_ros/turtlebot_ros_deployment.ipynb#adding-teleoperation), teleoperation provides **direct velocity control** by publishing commands straight to `/cmd_vel`:\n\n**Architecture Modes**:\n- **Autonomous Navigation** (this notebook): Waypoint Generator → Velocity Controller → `/cmd_vel` → Gazebo\n- **Teleoperation**: Keyboard Teleop → `/cmd_vel` → Gazebo\n\nWhen 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.\n\n:::{note}\nThis 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!\n:::" }, { "cell_type": "markdown", "source": [ "## What Gazebo Provides\n", "\n", "When we launch Gazebo with the TurtleBot3 Waffle model:\n", "\n", "**Input Topics** (Gazebo subscribes):\n", "- `/cmd_vel` (Twist) - Velocity commands for the robot\n", "\n", "**Output Topics** (Gazebo publishes):\n", "- `/odom` (Odometry) - Wheel odometry with realistic noise\n", "- `/scan` (LaserScan) - 360° LiDAR data\n", "- `/imu` (Imu) - Inertial measurement unit\n", "- `/camera/image_raw` (Image) - Camera feed\n", "- `/tf` (TFMessage) - Coordinate frame transforms\n", "\n", "For our state estimation, we only need `/cmd_vel` (input) and `/odom` (output).\n", "\n", "**Physics Simulation Features**:\n", "- Wheel slip and friction\n", "- Robot inertia and dynamics\n", "- Realistic sensor noise models\n", "- Collision detection\n", "- Same code works on real hardware!\n" ], "id": "cell-4", "metadata": {} }, { "cell_type": "markdown", "source": [ "## Setup: Imports\n" ], "id": "cell-5", "metadata": {} }, { "cell_type": "code", "source": [ "import numpy as np\n", "import matplotlib.pyplot as plt\n", "from pykal import DynamicalSystem\n", "from pykal.ros.ros_node import ROSNode\n", "from pykal.algorithm_library.estimators.kf import KF\n", "from pykal.gazebo import start_gazebo, stop_gazebo\n", "\n", "# ROS message types\n", "from geometry_msgs.msg import Twist, PoseStamped, Vector3\n", "from nav_msgs.msg import Odometry\n", "import rclpy\n", "import time\n", "\n", "print(\"Imports successful! Ready to integrate with Gazebo.\")" ], "execution_count": null, "outputs": [], "id": "cell-6", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "### Component Functions (Reuse from ROS Deployment)\n" ], "id": "cell-7", "metadata": {} }, { "cell_type": "code", "source": [ "# ============================================================================\n", "# TurtleBot Dynamics (for Kalman filter prediction)\n", "# ============================================================================\n", "\n", "def turtlebot_f(xk: np.ndarray, uk: np.ndarray, dt: float) -> np.ndarray:\n", " \"\"\"Unicycle kinematics: [x, y, theta, v, omega].\"\"\"\n", " x, y, theta, v, omega = xk.flatten()\n", " v_cmd, omega_cmd = uk.flatten()\n", " \n", " x_new = x + v * np.cos(theta) * dt\n", " y_new = y + v * np.sin(theta) * dt\n", " theta_new = np.arctan2(np.sin(theta + omega * dt), np.cos(theta + omega * dt))\n", " \n", " return np.array([[x_new], [y_new], [theta_new], [v_cmd], [omega_cmd]])\n", "\n", "\n", "def turtlebot_h(xk: np.ndarray) -> np.ndarray:\n", " \"\"\"Measurement: [x, y, theta] from odometry.\"\"\"\n", " return xk[:3, :]\n", "\n", "\n", "def compute_F_jacobian(xhat: np.ndarray, dt: float) -> np.ndarray:\n", " \"\"\"Jacobian of dynamics.\"\"\"\n", " _, _, theta, v, _ = xhat.flatten()\n", " return np.array([\n", " [1, 0, -v * np.sin(theta) * dt, np.cos(theta) * dt, 0],\n", " [0, 1, v * np.cos(theta) * dt, np.sin(theta) * dt, 0],\n", " [0, 0, 1, 0, dt],\n", " [0, 0, 0, 1, 0],\n", " [0, 0, 0, 0, 1]\n", " ])\n", "\n", "\n", "def compute_H_jacobian() -> np.ndarray:\n", " \"\"\"Jacobian of measurement.\"\"\"\n", " return np.array([\n", " [1, 0, 0, 0, 0],\n", " [0, 1, 0, 0, 0],\n", " [0, 0, 1, 0, 0]\n", " ])\n", "\n", "\n", "# Noise covariances\n", "Q_turtlebot = np.diag([0.01, 0.01, 0.02, 0.1, 0.1])\n", "R_turtlebot = np.diag([0.05, 0.05, 0.1])\n", "\n", "print(\"TurtleBot dynamics and Jacobians defined!\")" ], "execution_count": null, "outputs": [], "id": "cell-8", "metadata": {} }, { "cell_type": "markdown", "source": [ "## Step 1: Launch Gazebo with TurtleBot3\n", "\n", "We use `pykal.gazebo.start_gazebo()` to launch Gazebo with the TurtleBot3 Waffle model.\n", "\n", "**This replaces our software simulator node!**\n" ], "id": "cell-9", "metadata": {} }, { "cell_type": "code", "source": [ "# Launch Gazebo\n", "print(\"Launching Gazebo with TurtleBot3 Waffle...\")\n", "print(\"This may take 10-20 seconds...\\n\")\n", "\n", "gz = start_gazebo(\n", " robot='turtlebot3',\n", " world='empty_world', # Simple empty world for testing\n", " headless=False, # Set True to run without GUI (faster)\n", " x_pose=0.0,\n", " y_pose=0.0,\n", " z_pose=0.01,\n", " yaw=0.0\n", ")\n", "\n", "print(\"✓ Gazebo launched successfully!\")\n", "print(f\"\\nGazebo is now running and publishing:\")\n", "print(f\" - /odom (Odometry): Wheel odometry with noise\")\n", "print(f\" - /scan (LaserScan): LiDAR data\")\n", "print(f\" - /imu (Imu): IMU data\")\n", "print(f\" - /tf (TF): Coordinate transforms\")\n", "print(f\"\\nGazebo is listening on:\")\n", "print(f\" - /cmd_vel (Twist): Velocity commands\")\n", "\n", "# Give Gazebo time to initialize\n", "import time as pytime\n", "pytime.sleep(3)\n", "print(\"\\nGazebo initialization complete!\")" ], "execution_count": null, "outputs": [], "id": "cell-10", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "## Step 2: Node Architecture (Without Simulator)\n", "\n", "We create the same control nodes as before, **except the simulator**:\n", "\n", "**Changes from software simulation**:\n", "1. ❌ Remove `turtlebot_simulator` node (Gazebo replaces it)\n", "2. ✓ Keep `waypoint_generator` (unchanged)\n", "3. ✓ Keep `velocity_controller` (unchanged)\n", "4. ✓ Keep `kalman_filter` (unchanged, subscribes to Gazebo's `/odom`)\n", "\n", "**Power of ROS**: Same controller and observer code works with Gazebo!\n" ], "id": "cell-11", "metadata": {} }, { "cell_type": "markdown", "source": [ "### Node 1: Waypoint Generator (Unchanged)\n" ], "id": "cell-12", "metadata": {} }, { "cell_type": "code", "source": [ "def create_waypoint_generator_node(waypoints, switch_time=15.0, rate_hz=10.0):\n", " \"\"\"Same as before - generates reference poses.\"\"\"\n", " current_idx = 0\n", " time_at_waypoint = 0.0\n", " last_tk = 0.0\n", " \n", " def waypoint_callback(tk):\n", " nonlocal current_idx, time_at_waypoint, last_tk\n", " \n", " dt = tk - last_tk if last_tk > 0 else 0.0\n", " last_tk = tk\n", " time_at_waypoint += dt\n", " \n", " if time_at_waypoint >= switch_time:\n", " current_idx = (current_idx + 1) % len(waypoints)\n", " time_at_waypoint = 0.0\n", " \n", " x_r, y_r, theta_r = waypoints[current_idx]\n", " qx, qy = 0.0, 0.0\n", " qz = np.sin(theta_r / 2.0)\n", " qw = np.cos(theta_r / 2.0)\n", " \n", " reference = np.array([tk, x_r, y_r, 0.0, qx, qy, qz, qw])\n", " return {'reference': reference}\n", " \n", " node = ROSNode(\n", " node_name='waypoint_generator',\n", " callback=waypoint_callback,\n", " subscribes_to=[],\n", " publishes_to=[('reference', PoseStamped, '/reference')],\n", " rate_hz=rate_hz\n", " )\n", " return node\n", "\n", "\n", "# Create waypoint generator\n", "square_waypoints = [\n", " (1.0, 0.0, 0.0),\n", " (1.0, 1.0, np.pi/2),\n", " (0.0, 1.0, np.pi),\n", " (0.0, 0.0, -np.pi/2)\n", "]\n", "\n", "waypoint_node = create_waypoint_generator_node(\n", " waypoints=square_waypoints,\n", " switch_time=15.0,\n", " rate_hz=10.0\n", ")\n", "\n", "print(\"✓ Waypoint generator node created (unchanged from software version)\")" ], "execution_count": null, "outputs": [], "id": "cell-13", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "### Node 2: Velocity Controller (Unchanged)\n" ], "id": "cell-14", "metadata": {} }, { "cell_type": "code", "source": [ "def create_velocity_controller_node(Kv=0.3, Komega=1.0, rate_hz=50.0):\n", " \"\"\"Same as before - generates velocity commands.\"\"\"\n", " def controller_callback(tk, reference, estimate):\n", " x_r, y_r = reference[1], reference[2]\n", " qz_r, qw_r = reference[6], reference[7]\n", " theta_r = np.arctan2(2.0 * qw_r * qz_r, 1.0 - 2.0 * qz_r**2)\n", " \n", " x, y = estimate[0], estimate[1]\n", " qz, qw = estimate[5], estimate[6]\n", " theta = np.arctan2(2.0 * qw * qz, 1.0 - 2.0 * qz**2)\n", " \n", " dx = x_r - x\n", " dy = y_r - y\n", " distance = np.sqrt(dx**2 + dy**2)\n", " \n", " heading_error = theta_r - theta\n", " heading_error = np.arctan2(np.sin(heading_error), np.cos(heading_error))\n", " \n", " v_cmd = Kv * distance\n", " omega_cmd = Komega * heading_error\n", " \n", " v_cmd = np.clip(v_cmd, -0.22, 0.22)\n", " omega_cmd = np.clip(omega_cmd, -2.84, 2.84)\n", " \n", " cmd_vel = np.array([v_cmd, 0.0, 0.0, 0.0, 0.0, omega_cmd])\n", " return {'cmd_vel': cmd_vel}\n", " \n", " node = ROSNode(\n", " node_name='velocity_controller',\n", " callback=controller_callback,\n", " subscribes_to=[\n", " ('/reference', PoseStamped, 'reference'),\n", " ('/estimate', Odometry, 'estimate'),\n", " ],\n", " publishes_to=[('cmd_vel', Twist, '/cmd_vel')],\n", " rate_hz=rate_hz,\n", " required_topics={'reference', 'estimate'}\n", " )\n", " return node\n", "\n", "\n", "controller_node = create_velocity_controller_node(Kv=0.3, Komega=1.0, rate_hz=50.0)\n", "print(\"✓ Velocity controller node created (unchanged from software version)\")\n", "print(\" → Publishes to /cmd_vel (Gazebo will receive these commands!)\")" ], "execution_count": null, "outputs": [], "id": "cell-15", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "### Node 3: Kalman Filter (Uses Gazebo's /odom)\n", "\n", "The Kalman filter is **unchanged** algorithmically, but now subscribes to Gazebo's `/odom` instead of our simulator's `/odom`.\n", "\n", "**Key Point**: The KF **doesn't know** the difference! It just processes Odometry messages.\n" ], "id": "cell-16", "metadata": {} }, { "cell_type": "code", "source": [ "def create_kalman_filter_node(dt=0.1, rate_hz=10.0, Q=None, R=None):\n", " \"\"\"Kalman filter using Gazebo's /odom measurements.\"\"\"\n", " if Q is None:\n", " Q = Q_turtlebot\n", " if R is None:\n", " R = R_turtlebot\n", " \n", " xhat = np.array([[0.0], [0.0], [0.0], [0.0], [0.0]])\n", " P = np.diag([0.1, 0.1, 0.1, 1.0, 1.0])\n", " \n", " def filter_callback(tk, odom, cmd_vel):\n", " \"\"\"\n", " NOTE: Now 'odom' comes from Gazebo, not our simulator!\n", " \"\"\"\n", " nonlocal xhat, P\n", " \n", " # Extract measurement from Gazebo's Odometry message\n", " x_meas, y_meas = odom[0], odom[1]\n", " qz_meas, qw_meas = odom[5], odom[6]\n", " theta_meas = np.arctan2(2.0 * qw_meas * qz_meas, 1.0 - 2.0 * qz_meas**2)\n", " yk = np.array([[x_meas], [y_meas], [theta_meas]])\n", " \n", " uk = np.array([[cmd_vel[0]], [cmd_vel[5]]])\n", " \n", " Fk = compute_F_jacobian(xhat, dt)\n", " Hk = compute_H_jacobian()\n", " \n", " f_params = {'xk': xhat, 'uk': uk, 'dt': dt}\n", " h_params = {'xk': xhat}\n", " \n", " xhat_new, P_new = KF.f(\n", " x_P=(xhat, P),\n", " y=yk,\n", " u=uk,\n", " f=turtlebot_f,\n", " F=lambda **params: Fk,\n", " Q=lambda **params: Q,\n", " h=turtlebot_h,\n", " H=lambda **params: Hk,\n", " R=lambda **params: R,\n", " f_params=f_params,\n", " F_params={},\n", " Q_params={},\n", " h_params=h_params,\n", " H_params={},\n", " R_params={}\n", " )\n", " \n", " xhat = xhat_new\n", " P = P_new\n", " \n", " x_est, y_est, theta_est, v_est, omega_est = xhat.flatten()\n", " qx, qy = 0.0, 0.0\n", " qz_est = np.sin(theta_est / 2.0)\n", " qw_est = np.cos(theta_est / 2.0)\n", " \n", " estimate = np.array([\n", " x_est, y_est, 0.0,\n", " qx, qy, qz_est, qw_est,\n", " v_est * np.cos(theta_est), v_est * np.sin(theta_est), 0.0,\n", " 0.0, 0.0, omega_est\n", " ])\n", " \n", " return {'estimate': estimate}\n", " \n", " node = ROSNode(\n", " node_name='kalman_filter',\n", " callback=filter_callback,\n", " subscribes_to=[\n", " ('/odom', Odometry, 'odom'), # ← FROM GAZEBO!\n", " ('/cmd_vel', Twist, 'cmd_vel'),\n", " ],\n", " publishes_to=[('estimate', Odometry, '/estimate')],\n", " rate_hz=rate_hz,\n", " required_topics={'odom', 'cmd_vel'}\n", " )\n", " return node\n", "\n", "\n", "kf_node = create_kalman_filter_node(dt=0.1, rate_hz=10.0)\n", "print(\"✓ Kalman filter node created\")\n", "print(\" → Subscribes to /odom (provided by Gazebo!)\")\n", "print(\" → Subscribes to /cmd_vel (to predict state)\")\n", "print(\" → Publishes /estimate\")" ], "execution_count": null, "outputs": [], "id": "cell-17", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "## Step 3: Run the Integrated System\n", "\n", "Now we start our nodes and let them interact with Gazebo:\n" ], "id": "cell-18", "metadata": {} }, { "cell_type": "code", "source": [ "# Initialize ROS2\n", "rclpy.init()\n", "\n", "# Create nodes\n", "print(\"Creating ROS2 nodes...\")\n", "waypoint_node.create_node()\n", "controller_node.create_node()\n", "kf_node.create_node()\n", "print(\"All nodes created!\")\n", "\n", "# Start nodes\n", "print(\"\\nStarting nodes...\")\n", "waypoint_node.start()\n", "print(\" ✓ Waypoint generator running\")\n", "kf_node.start()\n", "print(\" ✓ Kalman filter running (using Gazebo's /odom!)\")\n", "controller_node.start()\n", "print(\" ✓ Velocity controller running (sending /cmd_vel to Gazebo!)\")\n", "\n", "print(\"\\n🚀 TurtleBot Gazebo integration is live!\")\n", "print(\"\\nSystem Architecture:\")\n", "print(\" waypoint_generator → /reference\")\n", "print(\" ↓\")\n", "print(\" velocity_controller → /cmd_vel → GAZEBO (physics simulation)\")\n", "print(\" ↓\")\n", "print(\" /odom (with realistic noise)\")\n", "print(\" ↓\")\n", "print(\" kalman_filter → /estimate\")\n", "print(\" ↓ (feedback)\")\n", "print(\" velocity_controller\")\n", "\n", "print(\"\\n💡 Tip: Open Gazebo GUI to watch the TurtleBot move!\")\n", "print(\"💡 Tip: Run 'rqt_graph' to see Gazebo nodes in the ROS graph!\")" ], "execution_count": null, "outputs": [], "id": "cell-19", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "### Data Logger (For Analysis)\n" ], "id": "cell-20", "metadata": {} }, { "cell_type": "code", "source": [ "# Create data logger\n", "def create_data_logger_node():\n", " data_log = {\n", " 'time': [],\n", " 'reference': [],\n", " 'cmd_vel': [],\n", " 'odom': [],\n", " 'estimate': []\n", " }\n", " \n", " def logger_callback(tk, reference, cmd_vel, odom, estimate):\n", " data_log['time'].append(tk)\n", " data_log['reference'].append(reference.copy())\n", " data_log['cmd_vel'].append(cmd_vel.copy())\n", " data_log['odom'].append(odom.copy())\n", " data_log['estimate'].append(estimate.copy())\n", " return {}\n", " \n", " node = ROSNode(\n", " node_name='data_logger',\n", " callback=logger_callback,\n", " subscribes_to=[\n", " ('/reference', PoseStamped, 'reference'),\n", " ('/cmd_vel', Twist, 'cmd_vel'),\n", " ('/odom', Odometry, 'odom'),\n", " ('/estimate', Odometry, 'estimate'),\n", " ],\n", " publishes_to=[],\n", " rate_hz=10.0\n", " )\n", " return node, data_log\n", "\n", "\n", "logger_node, data_log = create_data_logger_node()\n", "logger_node.create_node()\n", "logger_node.start()\n", "print(\"Data logger started!\")\n", "\n", "# Run for 60 seconds\n", "T_sim = 60.0\n", "print(f\"\\nRunning Gazebo simulation for {T_sim} seconds...\")\n", "print(\"Watch the TurtleBot in Gazebo GUI!\\n\")\n", "pytime.sleep(T_sim)\n", "\n", "print(f\"\\nSimulation complete! Collected {len(data_log['time'])} samples.\")" ], "execution_count": null, "outputs": [], "id": "cell-21", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "### Stop the System\n" ], "id": "cell-22", "metadata": {} }, { "cell_type": "code", "source": [ "# Stop ROS nodes\n", "print(\"Stopping ROS nodes...\")\n", "logger_node.stop()\n", "waypoint_node.stop()\n", "controller_node.stop()\n", "kf_node.stop()\n", "print(\"All nodes stopped.\")\n", "\n", "# Shutdown ROS2\n", "rclpy.shutdown()\n", "print(\"ROS2 shutdown complete.\")\n", "\n", "# Stop Gazebo\n", "print(\"\\nStopping Gazebo...\")\n", "stop_gazebo(gz)\n", "print(\"Gazebo stopped.\")" ], "execution_count": null, "outputs": [], "id": "cell-23", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "## Visualization: Gazebo vs Software Simulation\n" ], "id": "cell-24", "metadata": {} }, { "cell_type": "code", "source": [ "# Convert to arrays\n", "time_vec = np.array(data_log['time'])\n", "odom_data = np.array(data_log['odom'])\n", "estimates = np.array(data_log['estimate'])\n", "commands = np.array(data_log['cmd_vel'])\n", "references = np.array(data_log['reference'])\n", "\n", "# Extract positions\n", "odom_x = odom_data[:, 0]\n", "odom_y = odom_data[:, 1]\n", "est_x = estimates[:, 0]\n", "est_y = estimates[:, 1]\n", "ref_x = references[:, 1]\n", "ref_y = references[:, 2]\n", "\n", "# Plotting\n", "fig, axs = plt.subplots(2, 2, figsize=(14, 10))\n", "\n", "# Plot 1: 2D Trajectory\n", "ax = axs[0, 0]\n", "ax.plot(odom_x, odom_y, 'gray', linewidth=1, label='Gazebo Odometry (noisy)', alpha=0.5)\n", "ax.plot(est_x, est_y, 'r--', linewidth=2, label='KF Estimate', alpha=0.8)\n", "ax.scatter(ref_x[::50], ref_y[::50], c='green', s=100, marker='*', label='Waypoints', zorder=5)\n", "ax.set_xlabel('X Position (m)', fontsize=11)\n", "ax.set_ylabel('Y Position (m)', fontsize=11)\n", "ax.set_title('Gazebo Integration: 2D Trajectory', fontsize=13, fontweight='bold')\n", "ax.legend()\n", "ax.grid(True, alpha=0.3)\n", "ax.axis('equal')\n", "\n", "# Plot 2: X Position\n", "ax = axs[0, 1]\n", "ax.plot(time_vec, odom_x, 'gray', label='Gazebo Odom', alpha=0.5)\n", "ax.plot(time_vec, est_x, 'r--', label='KF Estimate', alpha=0.8)\n", "ax.set_ylabel('X Position (m)', fontsize=11)\n", "ax.set_title('X Position: Gazebo Odometry', fontsize=13, fontweight='bold')\n", "ax.legend()\n", "ax.grid(True, alpha=0.3)\n", "\n", "# Plot 3: Y Position\n", "ax = axs[1, 0]\n", "ax.plot(time_vec, odom_y, 'gray', label='Gazebo Odom', alpha=0.5)\n", "ax.plot(time_vec, est_y, 'r--', label='KF Estimate', alpha=0.8)\n", "ax.set_xlabel('Time (s)', fontsize=11)\n", "ax.set_ylabel('Y Position (m)', fontsize=11)\n", "ax.set_title('Y Position: Gazebo Odometry', fontsize=13, fontweight='bold')\n", "ax.legend()\n", "ax.grid(True, alpha=0.3)\n", "\n", "# Plot 4: Control Commands\n", "ax = axs[1, 1]\n", "ax.plot(time_vec, commands[:, 0], 'g-', label='Linear Velocity', linewidth=1.5)\n", "ax_omega = ax.twinx()\n", "ax_omega.plot(time_vec, commands[:, 5], 'purple', label='Angular Velocity', linewidth=1.5, alpha=0.7)\n", "ax.set_xlabel('Time (s)', fontsize=11)\n", "ax.set_ylabel('Linear Velocity (m/s)', fontsize=11, color='g')\n", "ax_omega.set_ylabel('Angular Velocity (rad/s)', fontsize=11, color='purple')\n", "ax.set_title('Commands to Gazebo', fontsize=13, fontweight='bold')\n", "ax.tick_params(axis='y', labelcolor='g')\n", "ax_omega.tick_params(axis='y', labelcolor='purple')\n", "ax.grid(True, alpha=0.3)\n", "\n", "plt.tight_layout()\n", "plt.show()\n", "\n", "print(f\"\\nGazebo Integration Performance:\")\n", "print(f\" Total samples: {len(time_vec)}\")\n", "print(f\" Simulation time: {time_vec[-1]:.1f} seconds\")\n", "print(f\"\\nNote: Gazebo provides realistic physics simulation!\")\n", "print(f\" - Wheel slip, friction, inertia\")\n", "print(f\" - Sensor noise from wheel encoders\")\n", "print(f\" - Same code works on real hardware!\")" ], "execution_count": null, "outputs": [], "id": "cell-25", "metadata": { "tags": [ "hide-input" ] } }, { "cell_type": "markdown", "source": [ "## Summary: The Complete Pipeline\n", "\n", "We've completed the full Theory → Software → ROS → Gazebo pipeline!\n", "\n", "**Step 1: Theory → Software** ([theory_to_python](../theory_to_python/turtlebot_state_estimation.ipynb))\n", "- Designed system as `DynamicalSystem` components\n", "- Tested with NumPy arrays in Python\n", "- Direct function calls, shared parameter dictionary\n", "\n", "**Step 2: Software → ROS** ([python_to_ros](../python_to_ros/turtlebot_ros_deployment.ipynb))\n", "- Wrapped `DynamicalSystem` as `ROSNode`\n", "- Created software simulator node\n", "- Distributed ROS system with topics\n", "\n", "**Step 3: ROS → Gazebo** (this notebook)\n", "- Removed software simulator\n", "- Connected to Gazebo's physics simulation\n", "- **Same controller and observer code!**\n", "\n", "**Step 4: Gazebo → Hardware** (not shown, but trivial!)\n", "- Remove Gazebo launch\n", "- Connect to real TurtleBot's `/odom` topic\n", "- **Same controller and observer code!**\n", "\n", "**What Changed at Each Step**:\n", "\n", "| Step | Changed | Unchanged |\n", "|------|---------|----------|\n", "| Theory → ROS | Wrapped as ROSNode | Dynamics, KF, controller |\n", "| ROS → Gazebo | Removed simulator | Waypoint, controller, KF |\n", "| Gazebo → Hardware | Launch command | Waypoint, controller, KF |\n", "\n", "**ROS Graph Evolution**:\n", "\n", "```\n", "Software: our_nodes → turtlebot_simulator → /odom\n", "Gazebo: our_nodes → GAZEBO → /odom\n", "Hardware: our_nodes → ROBOT → /odom\n", "```\n", "\n", "**The Beauty of ROS**: The controller and observer are **completely decoupled** from the source of `/odom`. They work identically with:\n", "- Software simulation (Python)\n", "- Physics simulation (Gazebo)\n", "- Real hardware (TurtleBot3)\n", "\n", "This is the essence of **modular robotics architecture**!\n" ], "id": "cell-26", "metadata": {} }, { "cell_type": "markdown", "id": "1t7z4zlf1yb", "source": "# Architecture 2: Teleoperation with Gazebo\n\nIn 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.\n\n**Key Architectural Difference**:\n- **Autonomous** (previous): Waypoint Gen → Controller → `/cmd_vel` → **Gazebo** → Observer\n- **Teleoperation** (this section): **Keyboard Teleop** → `/cmd_vel` → **Gazebo** → Observer\n\nThe beauty of ROS: Gazebo doesn't care whether `/cmd_vel` comes from an autonomous controller or a teleop node—it responds identically!\n\n
\n \n
\n\n**Nodes Used in Teleoperation with Gazebo**:\n1. 🔴 **Keyboard Teleop** - External tool (`teleop_twist_keyboard`)\n2. 🔴 **Gazebo** - Physics simulation (responds to `/cmd_vel`)\n3. ✅ **Kalman Filter** - Continues providing state estimates\n4. ❌ **Waypoint Generator** - Not needed (bypassed)\n5. ❌ **Velocity Controller** - Not needed (bypassed)\n\n:::{note}\nThis demonstrates the **same teleoperation architecture** from the [ROS deployment notebook](../python_to_ros/turtlebot_ros_deployment.ipynb#architecture-2-teleoperation-mode), but with Gazebo's physics simulation instead of the Python simulator!\n:::", "metadata": {} }, { "cell_type": "code", "id": "ptgdm0c2jrl", "source": "# Launch Gazebo (if not already running from previous section)\nprint(\"Launching Gazebo with TurtleBot3...\")\nprint(\"This may take 10-15 seconds...\\n\")\n\ngz_teleop = start_gazebo(\n robot='turtlebot3',\n world='empty_world',\n headless=False, # Keep GUI for visual feedback during teleop\n x_pose=0.0,\n y_pose=0.0,\n z_pose=0.0,\n yaw=0.0\n)\n\nprint(\"✓ Gazebo launched successfully!\")\nprint(\"\\nGazebo is publishing:\")\nprint(\" /odom (Odometry)\")\nprint(\"\\nGazebo is listening on:\")\nprint(\" /cmd_vel (Twist) ← waiting for teleop or controller\")\n\n# Give Gazebo time to initialize\nimport time as pytime\npytime.sleep(3)\n\n# Re-initialize ROS2 if needed\nif not rclpy.ok():\n rclpy.init()\n\n# Create only Kalman filter node (NO waypoint gen, NO controller)\nprint(\"\\nCreating Kalman filter for teleoperation mode...\")\nkf_gazebo_teleop = create_kalman_filter_node(dt=0.1, rate_hz=10.0, Q=Q_turtlebot, R=R_turtlebot)\nkf_gazebo_teleop.create_node()\nkf_gazebo_teleop.start()\nprint(\"✓ Kalman filter running\")\n\nprint(\"\\n🚀 System ready for teleoperation with Gazebo!\")\nprint(\"\\nActive Architecture:\")\nprint(\" (waiting for teleop) → /cmd_vel → GAZEBO → /odom → kalman_filter\")\nprint(\"\\nNOTE: Waypoint generator and controller are NOT running (bypassed!)\")", "metadata": {}, "execution_count": null, "outputs": [] }, { "cell_type": "code", "id": "i5k5elnh98q", "source": "# Create a logger to monitor the system during teleoperation\n# (In real usage, you would manually control with keyboard)\n\ndef create_gazebo_teleop_logger():\n \"\"\"Logger for Gazebo teleoperation monitoring.\"\"\"\n gazebo_teleop_data = {\n \"time\": [],\n \"odom\": [],\n \"estimate\": [],\n }\n \n def logger_callback(tk, odom, estimate):\n gazebo_teleop_data[\"time\"].append(tk)\n gazebo_teleop_data[\"odom\"].append(odom.copy())\n gazebo_teleop_data[\"estimate\"].append(estimate.copy())\n return {}\n \n node = ROSNode(\n node_name=\"gazebo_teleop_logger\",\n callback=logger_callback,\n subscribes_to=[\n (\"/odom\", Odometry, \"odom\"),\n (\"/estimate\", Odometry, \"estimate\"),\n ],\n publishes_to=[],\n rate_hz=10.0)\n \n return node, gazebo_teleop_data\n\n\n# Create and start logger\ngazebo_teleop_logger, gazebo_teleop_data = create_gazebo_teleop_logger()\ngazebo_teleop_logger.create_node()\ngazebo_teleop_logger.start()\n\nprint(\"Logger started!\")\nprint(\"\\n\" + \"=\"*60)\nprint(\"TELEOPERATION MODE ACTIVE\")\nprint(\"=\"*60)\nprint(\"\\nTo control the robot manually:\")\nprint(\" 1. Open a new terminal\")\nprint(\" 2. Run: ros2 run teleop_twist_keyboard teleop_twist_keyboard\")\nprint(\" 3. Use i/j/k/l keys to drive the TurtleBot in Gazebo\")\nprint(\"\\nThe Kalman filter is running and monitoring your commands!\")\nprint(\"\\nFor this demo, we'll collect 30 seconds of data...\")\nprint(\"(In real usage, you would be driving the robot manually)\")\nprint(\"=\"*60)\n\n# Collect data for demonstration\nT_gazebo_teleop = 30.0\nprint(f\"\\nCollecting {T_gazebo_teleop} seconds of teleoperation data...\")\npytime.sleep(T_gazebo_teleop)\n\nprint(f\"\\nData collection complete! Collected {len(gazebo_teleop_data['time'])} samples.\")", "metadata": {}, "execution_count": null, "outputs": [] }, { "cell_type": "markdown", "id": "pxlhwze07yn", "source": "## Summary: Dual Architectures with Gazebo\n\nThis notebook demonstrated **two control architectures** with Gazebo physics simulation:\n\n### Architecture 1: Autonomous Navigation (Waypoint Tracking)\n\n**Nodes Active**:\n- ✅ Waypoint Generator (`pykal.ROSNode`)\n- ✅ Velocity Controller (`pykal.ROSNode`)\n- 🔴 **Gazebo** (physics simulation)\n- ✅ Kalman Filter (`pykal.ROSNode`)\n\n**Data Flow**: Waypoint Gen → Controller → `/cmd_vel` → **Gazebo** → `/odom` → Observer\n\n**Use Case**: Autonomous navigation in realistic physics environment\n\n---\n\n### Architecture 2: Teleoperation (Manual Control)\n\n**Nodes Active**:\n- 🔴 **Keyboard Teleop** (external: `teleop_twist_keyboard`)\n- 🔴 **Gazebo** (physics simulation)\n- ✅ Kalman Filter (`pykal.ROSNode`)\n\n**Nodes Bypassed**:\n- ❌ Waypoint Generator\n- ❌ Velocity Controller\n\n**Data Flow**: Teleop → `/cmd_vel` → **Gazebo** → `/odom` → Observer\n\n**Use Case**: Manual control with physics simulation for testing/intervention\n\n---\n\n### Key Insights\n\n1. **Gazebo Agnostic**: Gazebo doesn't care whether `/cmd_vel` comes from autonomous or teleop\n2. **Same Interface**: Both modes use `/cmd_vel`, demonstrating ROS modularity\n3. **Physics Preserved**: Realistic dynamics, friction, inertia in both modes\n4. **Observer Monitoring**: KF provides state estimates in both autonomous and manual modes\n5. **Red Nodes**: Gazebo and teleop shown in RED (external tools, not `pykal.ROSNode`)\n\n**Architecture Consistency Across Pipeline**:\n- **Theory** ([TurtleBot State Estimation](../theory_to_python/turtlebot_state_estimation.ipynb)): Pure Python DynamicalSystems\n- **ROS** ([ROS Deployment](../python_to_ros/turtlebot_ros_deployment.ipynb)): pykal.ROSNode wrappers\n- **Gazebo** (this notebook): Same ROS nodes + Gazebo physics\n- **Hardware** (next step): Same code, real robot!\n\nThe **same teleoperation pattern** works across software simulation, Gazebo simulation, and hardware deployment—demonstrating true architecture portability!", "metadata": {} }, { "cell_type": "code", "id": "t3b8x5vonzd", "source": "# Stop nodes\nprint(\"Stopping teleoperation nodes...\")\ngazebo_teleop_logger.stop()\nkf_gazebo_teleop.stop()\n\n# Shutdown ROS2\nrclpy.shutdown()\nprint(\"ROS2 shutdown complete.\")\n\n# Stop Gazebo\nprint(\"\\nStopping Gazebo...\")\nstop_gazebo(gz_teleop)\nprint(\"Gazebo stopped.\")", "metadata": {}, "execution_count": null, "outputs": [] }, { "cell_type": "markdown", "id": "ciw3d47yx07", "source": "### Stopping Teleoperation Mode", "metadata": {} }, { "cell_type": "markdown", "id": "jfnv7mwfnun", "source": "## Running Keyboard Teleop with Gazebo\n\n**In a separate terminal**, run the keyboard teleop tool:\n\n```bash\nros2 run teleop_twist_keyboard teleop_twist_keyboard\n```\n\n**Keyboard Controls**:\n- `i` - Move forward\n- `k` - Stop\n- `,` - Move backward\n- `j` - Turn left\n- `l` - Turn right\n\n**What Happens**:\n1. You press a key → `teleop_twist_keyboard` publishes to `/cmd_vel`\n2. Gazebo receives `/cmd_vel` → Updates robot physics\n3. Gazebo publishes `/odom` → Kalman filter estimates state\n4. You see the TurtleBot move in the Gazebo GUI!\n\n:::{tip}\n**Try This**: \n1. Open Gazebo GUI (if using `headless=False`)\n2. Run the teleop command in a terminal\n3. Drive the robot around manually!\n4. Watch the Kalman filter track your movements\n:::\n\n**For this notebook demo**, we'll create a programmatic logger to demonstrate the architecture (in real usage, you would manually control the robot):", "metadata": {} }, { "cell_type": "markdown", "id": "f1xrqrd0ijl", "source": "## Setting Up Teleoperation with Gazebo\n\nFor teleoperation mode, we only need:\n1. **Gazebo** - Already provides the TurtleBot physics simulation\n2. **Kalman Filter** - For state estimation and monitoring\n3. **Keyboard Teleop** - External tool for manual control\n\nWe do NOT need the waypoint generator or velocity controller.", "metadata": {} }, { "cell_type": "markdown", "source": [ "[← ROS Nodes and Gazebo](../../../getting_started/ros_to_gazebo/ros_nodes_and_gazebo.rst)\n" ], "id": "cell-27", "metadata": {} } ], "metadata": { "kernelspec": { "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { "name": "python", "version": "3.12.0" } }, "nbformat": 4, "nbformat_minor": 5 }