{ "cells": [ { "cell_type": "markdown", "source": [ "[← Dynamical Systems as ROS Nodes](../../../getting_started/python_to_ros/dynamical_systems_as_ros_nodes.rst)\n" ], "id": "cell-0", "metadata": {} }, { "cell_type": "markdown", "source": [ "# Crazyflie Multi-Sensor Fusion: From Python to ROS2\n" ], "id": "cell-1", "metadata": {} }, { "cell_type": "markdown", "source": [ "In the [Crazyflie Sensor Fusion](../theory_to_python/crazyflie_sensor_fusion.ipynb) tutorial, we designed a multi-sensor Kalman filter system in Python. Now we'll deploy this system as ROS2 nodes, demonstrating a more complex architecture with **three separate sensor nodes**.\n", "\n", "**Key Insight**: Multi-sensor fusion maps naturally to ROS!\n", "\n", "Unlike single-sensor systems (like TurtleBot), the Crazyflie has:\n", "- **Three sensor nodes**: Motion capture, barometer, IMU\n", "- **One fusion node**: Kalman filter subscribing to all three\n", "\n", "This demonstrates how ROS handles multi-rate, heterogeneous sensor systems.\n" ], "id": "cell-2", "metadata": {} }, { "cell_type": "markdown", "id": "cell-conceptual-foundation", "metadata": {}, "source": "# Conceptual Foundation: From Dynamical Systems to ROS Nodes\n\nRecall from the [Crazyflie Sensor Fusion](../theory_to_python/crazyflie_sensor_fusion.ipynb) tutorial that we represented our multi-sensor fusion 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 sensor fusion node combines data from three separate sensor nodes (mocap, barometer, IMU).\n\n## The ROS Mapping\n\nThere is a direct correspondence between this dynamical system representation and a ROS node architecture:\n\n
\n \n
\n\n**The correspondence**:\n- Each **dynamical system block** becomes a **ROS node**\n- Each **data connection** becomes a **ROS topic**\n- The **parameter dictionary** is replaced by **message passing**\n- The **multi-sensor fusion** becomes **multi-topic subscription**\n\n**Note**: In the Python version, we had three separate sensor nodes publishing to different topics. In ROS, this multi-rate sensor fusion is handled naturally through the `ROSNode` staleness policies.\n\n## Adding Teleoperation\n\nFor aerial vehicles like the Crazyflie, we use joystick teleoperation instead of keyboard input. Teleoperation provides **direct velocity control**, bypassing the high-level navigation pipeline entirely:\n\n
\n \n
\n\n**Key Architectural Difference**:\n- **Autonomous Navigation**: Setpoint Generator → Position Controller → `/cmd_vel` → Plant\n- **Teleoperation**: Joystick Teleop → `/cmd_vel` → Plant\n\nThe **Joystick Teleop** node publishes velocity commands directly to `/cmd_vel`, completely bypassing both the setpoint generator and position controller. The multi-sensor Kalman filter continues running to provide state estimates for monitoring and visualization.\n\nThis design pattern is common in aerial robotics: teleoperation provides low-level velocity control for manual flight, while autonomous mode uses the full planning and control pipeline for waypoint following.\n\n:::{note}\nThis tutorial implements the basic ROS node architecture (without teleop). The teleop version will be demonstrated in a later tutorial.\n:::\n\n## Individual Node Blocks\n\nEach ROS node wraps a dynamical system block. Here are the individual components:\n\n### Setpoint Generator\n
\n \n
\n\n### Position Controller\n
\n \n
\n\n### Crazyflie Plant\n
\n \n
\n\n### Kalman Filter Observer\n
\n \n
\n\nIn the following sections, we'll implement each of these blocks as `ROSNode` instances, demonstrating how multi-sensor fusion from the theory notebook translates directly to ROS's multi-topic subscription pattern." }, { "cell_type": "markdown", "source": [ "## Architecture Comparison: Python vs ROS2 Multi-Sensor System\n", "\n", "**Python Simulation** (theory notebook):\n", "\n", "```python\n", "# Concatenate measurements from 3 sensors\n", "y_mocap = mocap_sensor(xk) + noise_mocap\n", "y_baro = baro_sensor(xk) + noise_baro\n", "y_imu = imu_sensor(xk) + noise_imu\n", "yk_combined = np.vstack([y_mocap, y_baro, y_imu])\n", "\n", "# Single KF update with stacked measurements\n", "xhat_P = observer.step(y=yk_combined, ...)\n", "```\n", "\n", "**ROS2 Nodes** (this notebook):\n", "\n", "```\n", "setpoint_node → /setpoint (Vector3)\n", " ↓\n", "controller_node → /cmd_vel (Twist)\n", " ↓\n", "crazyflie_simulator_node → /mocap (Vector3)\n", " → /baro (Float64)\n", " → /imu (Vector3)\n", " → /true_state (Odometry)\n", " ↓ ↓ ↓\n", " └──────────────────┴──────────────────┘\n", " ↓\n", " kalman_filter_node → /estimate (Odometry)\n", "```\n", "\n", ":::{note}\n", "The **three sensor topics converge** at the Kalman filter node. This is the hallmark of sensor fusion in ROS!\n", ":::\n" ], "id": "cell-3", "metadata": {} }, { "cell_type": "markdown", "source": [ "## ROS2 Multi-Sensor System Architecture\n", "\n", "Our system consists of **five ROS nodes** (vs four for TurtleBot):\n", "\n", "![Crazyflie Multi-Sensor System](../../../../_static/tutorial/theory_to_python/crazyflie_multisensor_system.svg)\n", "\n", "**Node 1: Setpoint Generator**\n", "- **Publishes**: `/setpoint` (Vector3)\n", "- **Function**: Generates 3D position references\n", "\n", "**Node 2: Position Controller**\n", "- **Subscribes**: `/setpoint`, `/estimate`\n", "- **Publishes**: `/cmd_vel` (Twist)\n", "- **Function**: 3D proportional position control\n", "\n", "**Node 3: Crazyflie Simulator**\n", "- **Subscribes**: `/cmd_vel`\n", "- **Publishes**: `/mocap`, `/baro`, `/imu`, `/true_state`\n", "- **Function**: Simulates dynamics + three sensors\n", "\n", "**Node 4: Multi-Sensor Kalman Filter**\n", "- **Subscribes**: `/mocap`, `/baro`, `/imu`, `/cmd_vel`\n", "- **Publishes**: `/estimate` (Odometry)\n", "- **Function**: Fuses 3 heterogeneous sensors\n", "" ], "id": "cell-4", "metadata": {} }, { "cell_type": "markdown", "source": [ "## Setup: Imports and Core Functions\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.data_change import corrupt\n", "\n", "# ROS message types\n", "from geometry_msgs.msg import Twist, Vector3\n", "from nav_msgs.msg import Odometry\n", "from std_msgs.msg import Float64\n", "import rclpy\n", "import time\n", "\n", "print(\"Imports successful! Ready to deploy Crazyflie to ROS.\")" ], "execution_count": null, "outputs": [], "id": "cell-6", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "### Core Crazyflie Functions (from Theory Notebook)\n" ], "id": "cell-7", "metadata": {} }, { "cell_type": "code", "source": [ "# ============================================================================\n", "# Crazyflie Dynamics\n", "# ============================================================================\n", "\n", "\n", "def crazyflie_f(xk: np.ndarray, uk: np.ndarray, dt: float) -> np.ndarray:\n", " \"\"\"3D constant-velocity dynamics: [x, y, z, vx, vy, vz].\"\"\"\n", " pos = xk[:3]\n", " vel = xk[3:]\n", " pos_new = pos + vel * dt\n", " vel_new = uk # Assume instantaneous velocity response\n", " return np.vstack([pos_new, vel_new])\n", "\n", "\n", "def crazyflie_h_mocap(xk: np.ndarray) -> np.ndarray:\n", " \"\"\"Motion capture: measures [x, y, z].\"\"\"\n", " return xk[:3]\n", "\n", "\n", "def crazyflie_h_baro(xk: np.ndarray) -> np.ndarray:\n", " \"\"\"Barometer: measures z only.\"\"\"\n", " return xk[2:3]\n", "\n", "\n", "def crazyflie_h_imu(xk: np.ndarray) -> np.ndarray:\n", " \"\"\"IMU: measures velocity [vx, vy, vz].\"\"\"\n", " return xk[3:]\n", "\n", "\n", "def h_multisensor(xk: np.ndarray) -> np.ndarray:\n", " \"\"\"Combined sensor model: [mocap(3), baro(1), imu(3)] = 7D.\"\"\"\n", " return np.vstack([crazyflie_h_mocap(xk), crazyflie_h_baro(xk), crazyflie_h_imu(xk)])\n", "\n", "\n", "# ============================================================================\n", "# KF Jacobians\n", "# ============================================================================\n", "\n", "\n", "def compute_F_crazyflie(dt: float) -> np.ndarray:\n", " \"\"\"State transition Jacobian (constant for linear system).\"\"\"\n", " I3 = np.eye(3)\n", " return np.block([[I3, I3 * dt], [np.zeros((3, 3)), I3]])\n", "\n", "\n", "def compute_H_multisensor() -> np.ndarray:\n", " \"\"\"Measurement Jacobian for multi-sensor fusion (7x6).\"\"\"\n", " return np.array(\n", " [\n", " [1, 0, 0, 0, 0, 0], # mocap x\n", " [0, 1, 0, 0, 0, 0], # mocap y\n", " [0, 0, 1, 0, 0, 0], # mocap z\n", " [0, 0, 1, 0, 0, 0], # baro z\n", " [0, 0, 0, 1, 0, 0], # imu vx\n", " [0, 0, 0, 0, 1, 0], # imu vy\n", " [0, 0, 0, 0, 0, 1], # imu vz\n", " ]\n", " )\n", "\n", "\n", "# ============================================================================\n", "# Noise Covariances\n", "# ============================================================================\n", "\n", "Q_crazyflie = np.diag([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]) # Process noise\n", "\n", "# Individual sensor noise\n", "R_mocap = np.diag([0.005, 0.005, 0.005]) # 5mm std\n", "R_baro = np.array([[0.1]]) # 10cm std\n", "R_imu = np.diag([0.1, 0.1, 0.1]) # 0.1 m/s std\n", "\n", "# Combined (block-diagonal)\n", "R_combined = np.block(\n", " [\n", " [R_mocap, np.zeros((3, 1)), np.zeros((3, 3))],\n", " [np.zeros((1, 3)), R_baro, np.zeros((1, 3))],\n", " [np.zeros((3, 3)), np.zeros((3, 1)), R_imu],\n", " ]\n", ")\n", "\n", "print(\"Core Crazyflie functions defined!\")\n", "print(f\" State dimension: 6 (position + velocity)\")\n", "print(f\" Measurement dimension: 7 (mocap:3 + baro:1 + imu:3)\")" ], "execution_count": null, "outputs": [], "id": "cell-8", "metadata": {} }, { "cell_type": "markdown", "source": [ "## Node 1: Setpoint Generator\n", "\n", "**ROS Wrapper Pattern**: Publishes 3D position setpoints.\n", "\n", "**Message Format**: `Vector3` = `(3,)` array `[x, y, z]`\n", "\n", "**Key Difference from TurtleBot**: No orientation (just position for quadrotor)\n" ], "id": "cell-9", "metadata": {} }, { "cell_type": "code", "source": [ "def create_setpoint_node(initial_position, transitions=None, rate_hz=10.0):\n", " \"\"\"\n", " Create ROS node that publishes position setpoints.\n", "\n", " Parameters\n", " ----------\n", " initial_position : np.ndarray\n", " Starting position [x, y, z]\n", " transitions : list of tuples\n", " Each tuple is (time, new_position)\n", " rate_hz : float\n", " Publishing rate\n", " \"\"\"\n", " if transitions is None:\n", " transitions = []\n", "\n", " current_setpoint = np.array(initial_position).reshape(-1, 1)\n", " transition_idx = 0\n", "\n", " def setpoint_callback(tk):\n", " nonlocal current_setpoint, transition_idx\n", "\n", " # Check for transitions\n", " if transition_idx < len(transitions):\n", " t_switch, new_pos = transitions[transition_idx]\n", " if tk >= t_switch:\n", " current_setpoint = np.array(new_pos).reshape(-1, 1)\n", " transition_idx += 1\n", "\n", " # Return as Vector3\n", " return {\"setpoint\": current_setpoint.flatten()}\n", "\n", " node = ROSNode(\n", " node_name=\"setpoint_generator\",\n", " callback=setpoint_callback,\n", " subscribes_to=[],\n", " publishes_to=[\n", " (\"setpoint\", Vector3, \"/setpoint\"),\n", " ],\n", " rate_hz=rate_hz)\n", "\n", " return node\n", "\n", "\n", "# Create setpoint node\n", "setpoint_node = create_setpoint_node(\n", " initial_position=[0.0, 0.0, 1.0],\n", " transitions=[(20.0, [1.0, 1.0, 1.5]), (40.0, [0.0, 0.0, 1.0])],\n", " rate_hz=10.0)\n", "\n", "print(\"Setpoint generator node created!\")\n", "print(f\" Publishes: /setpoint (Vector3)\")\n", "print(f\" Initial: [0, 0, 1.0] m\")\n", "print(f\" Transitions: t=20s → [1, 1, 1.5], t=40s → [0, 0, 1.0]\")" ], "execution_count": null, "outputs": [], "id": "cell-10", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "## Node 2: Position Controller\n", "\n", "**ROS Wrapper Pattern**: 3D proportional control.\n", "\n", "**Controller Law**:\n", "\n", "$$\n", "\\vec{v}_{cmd} = K_p (\\vec{r} - \\hat{\\vec{p}})\n", "$$\n", "\n", "where $\\vec{r} \\in \\mathbb{R}^3$ is the reference position and $\\hat{\\vec{p}} \\in \\mathbb{R}^3$ is the estimated position.\n" ], "id": "cell-11", "metadata": {} }, { "cell_type": "code", "source": [ "def create_position_controller_node(Kp=0.8, max_vel=0.5, rate_hz=50.0):\n", " \"\"\"\n", " Create ROS node for proportional position controller.\n", " \"\"\"\n", "\n", " def controller_callback(tk, setpoint, estimate):\n", " \"\"\"\n", " Args:\n", " tk (float): Current time\n", " setpoint (np.ndarray): (3) [x_r, y_r, z_r]\n", " estimate (np.ndarray): (13) Odometry\n", "\n", " Returns:\n", " dict: {'cmd_vel': (6) Twist}\n", " \"\"\"\n", " # Extract position from setpoint\n", " r = setpoint\n", "\n", " # Extract position from estimate\n", " phat = estimate[:3]\n", "\n", " # Proportional control\n", " error = r - phat\n", " v_cmd = Kp * error\n", " v_cmd = np.clip(v_cmd, -max_vel, max_vel)\n", "\n", " # Pack as Twist: [vx, vy, vz, wx, wy, wz]\n", " cmd_vel = np.concatenate([v_cmd, np.zeros(3)])\n", "\n", " return {\"cmd_vel\": cmd_vel}\n", "\n", " node = ROSNode(\n", " node_name=\"position_controller\",\n", " callback=controller_callback,\n", " subscribes_to=[\n", " (\"/setpoint\", Vector3, \"setpoint\"),\n", " (\"/estimate\", Odometry, \"estimate\"),\n", " ],\n", " publishes_to=[\n", " (\"cmd_vel\", Twist, \"/cmd_vel\"),\n", " ],\n", " rate_hz=rate_hz,\n", " required_topics={\"setpoint\", \"estimate\"})\n", "\n", " return node\n", "\n", "\n", "controller_node = create_position_controller_node(Kp=0.8, max_vel=0.5, rate_hz=50.0)\n", "\n", "print(\"Position controller node created!\")\n", "print(f\" Subscribes: /setpoint, /estimate\")\n", "print(f\" Publishes: /cmd_vel (Twist)\")\n", "print(f\" Gain: Kp={0.8}\")" ], "execution_count": null, "outputs": [], "id": "cell-12", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "## Node 3: Crazyflie Simulator with Multi-Sensor Output\n", "\n", "**ROS Wrapper Pattern**: Publishes **three separate sensor topics**.\n", "\n", "**Dynamics**: Linear 3D motion\n", "\n", "$$\n", "\\vec{p}_{k+1} = \\vec{p}_k + \\vec{v}_k \\Delta t\n", "$$\n", "\n", "$$\n", "\\vec{v}_{k+1} = \\vec{v}_{cmd}\n", "$$\n", "\n", "**Three Sensor Models**:\n", "\n", "1. **Motion Capture**: $y_{mocap} = \\vec{p} + \\mathcal{N}(0, R_{mocap})$ (high precision)\n", "2. **Barometer**: $y_{baro} = p_z + \\text{bias} + \\mathcal{N}(0, R_{baro})$ (drift + noise)\n", "3. **IMU**: $y_{imu} = \\vec{v} + \\mathcal{N}(0, R_{imu}) + \\text{spikes}$ (noise + vibration)\n", "\n", ":::{note}\n", "Unlike TurtleBot's single `/odom` topic, we publish **three separate sensor topics** to simulate realistic multi-sensor hardware.\n", ":::\n" ], "id": "cell-13", "metadata": {} }, { "cell_type": "code", "source": [ "def create_crazyflie_simulator_node(dt=0.01, rate_hz=100.0):\n", " \"\"\"\n", " Create ROS node that simulates Crazyflie with multi-sensor output.\n", " \"\"\"\n", " # Internal state: [x, y, z, vx, vy, vz]\n", " xk = np.array([[0.0], [0.0], [1.0], [0.0], [0.0], [0.0]])\n", "\n", " def simulator_callback(tk, cmd_vel):\n", " \"\"\"\n", " Simulate dynamics and generate noisy sensor measurements.\n", "\n", " Args:\n", " tk (float): Current time\n", " cmd_vel (np.ndarray): (6) Twist\n", "\n", " Returns:\n", " dict: {'mocap', 'baro', 'imu', 'true_state'}\n", " \"\"\"\n", " nonlocal xk\n", "\n", " # Extract velocity commands\n", " uk = cmd_vel[:3].reshape(-1, 1)\n", "\n", " # Update state\n", " xk = crazyflie_f(xk, uk, dt)\n", "\n", " # Extract state components\n", " pos = xk[:3].flatten()\n", " vel = xk[3:].flatten()\n", "\n", " # Generate true sensor measurements\n", " mocap_clean = pos\n", " baro_clean = pos[2]\n", " imu_clean = vel\n", "\n", " # Add realistic noise\n", " # Mocap: Gaussian noise (high precision)\n", " mocap_noisy = mocap_clean + np.random.multivariate_normal(np.zeros(3), R_mocap)\n", "\n", " # Barometer: Bias + drift + Gaussian noise\n", " baro_noisy = corrupt.with_bias(baro_clean, bias=0.02) # 2cm offset\n", " baro_noisy = baro_noisy + np.random.normal(0, np.sqrt(R_baro[0, 0]))\n", "\n", " # IMU: Gaussian noise + occasional spikes (vibration)\n", " imu_noisy = imu_clean + np.random.multivariate_normal(np.zeros(3), R_imu)\n", " if np.random.rand() < 0.03: # 3% spike rate\n", " spike_idx = np.random.randint(0, 3)\n", " imu_noisy[spike_idx] += np.random.choice([-1, 1]) * 0.2\n", "\n", " # True state as Odometry (for visualization)\n", " true_state = np.concatenate(\n", " [\n", " pos, # position\n", " [0.0, 0.0, 0.0, 1.0], # quaternion (identity)\n", " vel, # linear velocity\n", " [0.0, 0.0, 0.0], # angular velocity\n", " ]\n", " )\n", "\n", " return {\n", " \"mocap\": mocap_noisy,\n", " \"baro\": np.array([baro_noisy]),\n", " \"imu\": imu_noisy,\n", " \"true_state\": true_state,\n", " }\n", "\n", " node = ROSNode(\n", " node_name=\"crazyflie_simulator\",\n", " callback=simulator_callback,\n", " subscribes_to=[\n", " (\"/cmd_vel\", Twist, \"cmd_vel\"),\n", " ],\n", " publishes_to=[\n", " (\"mocap\", Vector3, \"/mocap\"),\n", " (\"baro\", Float64, \"/baro\"),\n", " (\"imu\", Vector3, \"/imu\"),\n", " (\"true_state\", Odometry, \"/true_state\"),\n", " ],\n", " rate_hz=rate_hz)\n", "\n", " return node\n", "\n", "\n", "simulator_node = create_crazyflie_simulator_node(dt=0.01, rate_hz=100.0)\n", "\n", "print(\"Crazyflie simulator node created!\")\n", "print(f\" Subscribes: /cmd_vel (Twist)\")\n", "print(f\" Publishes:\")\n", "print(f\" - /mocap (Vector3): 3D position, ~5mm noise\")\n", "print(f\" - /baro (Float64): Altitude, ~10cm noise + drift\")\n", "print(f\" - /imu (Vector3): Velocity, ~0.1 m/s noise + spikes\")\n", "print(f\" - /true_state (Odometry): Ground truth\")\n", "print(f\" Rate: 100 Hz (conceptually represents fastest sensor)\")" ], "execution_count": null, "outputs": [], "id": "cell-14", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "## Node 4: Multi-Sensor Kalman Filter\n", "\n", "**ROS Wrapper Pattern**: Subscribes to **three sensor topics** + `/cmd_vel`.\n", "\n", "**Sensor Fusion**: Stacks measurements into 7D vector\n", "\n", "$$\n", "y_k = \\begin{bmatrix} y_{mocap} \\\\ y_{baro} \\\\ y_{imu} \\end{bmatrix} \\in \\mathbb{R}^7\n", "$$\n", "\n", "**Measurement Jacobian**:\n", "\n", "$$\n", "H = \\begin{bmatrix}\n", "I_3 & 0 \\\\\n", "\\begin{bmatrix}0 & 0 & 1\\end{bmatrix} & 0 \\\\\n", "0 & I_3\n", "\\end{bmatrix}\n", "$$\n", "\n", "**Block-Diagonal Noise**: $R = \\text{diag}(R_{mocap}, R_{baro}, R_{imu})$\n", "\n", ":::{note}\n", "This is where sensor fusion happens! The KF callback **receives all three sensor messages** and combines them optimally based on their noise characteristics.\n", ":::\n" ], "id": "cell-15", "metadata": {} }, { "cell_type": "code", "source": [ "def create_kalman_filter_node(dt=0.01, rate_hz=100.0, Q=None, R=None):\n", " \"\"\"\n", " Create ROS node for multi-sensor Kalman filter.\n", " \"\"\"\n", " if Q is None:\n", " Q = Q_crazyflie\n", " if R is None:\n", " R = R_combined\n", "\n", " # Initial estimate\n", " xhat = np.array([[0.0], [0.0], [1.0], [0.0], [0.0], [0.0]])\n", " P = np.diag([0.1, 0.1, 0.1, 1.0, 1.0, 1.0])\n", "\n", " def filter_callback(tk, mocap, baro, imu, cmd_vel):\n", " \"\"\"\n", " Multi-sensor KF update.\n", "\n", " Args:\n", " tk (float): Current time\n", " mocap (np.ndarray): (3) [x, y, z]\n", " baro (np.ndarray): (1) [z]\n", " imu (np.ndarray): (3) [vx, vy, vz]\n", " cmd_vel (np.ndarray): (6) Twist\n", "\n", " Returns:\n", " dict: {'estimate': (13) Odometry}\n", " \"\"\"\n", " nonlocal xhat, P\n", "\n", " # Concatenate measurements: [mocap(3), baro(1), imu(3)] = 7D\n", " yk = np.vstack([mocap.reshape(-1, 1), baro.reshape(-1, 1), imu.reshape(-1, 1)])\n", "\n", " # Extract control input\n", " uk = cmd_vel[:3].reshape(-1, 1)\n", "\n", " # Compute Jacobians\n", " Fk = compute_F_crazyflie(dt)\n", " Hk = compute_H_multisensor()\n", "\n", " # Run KF update\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=crazyflie_f,\n", " F=lambda **params: Fk,\n", " Q=lambda **params: Q,\n", " h=h_multisensor,\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", " # Update state\n", " xhat = xhat_new\n", " P = P_new\n", "\n", " # Extract estimate\n", " pos_est = xhat[:3].flatten()\n", " vel_est = xhat[3:].flatten()\n", "\n", " # Convert to Odometry\n", " estimate = np.concatenate(\n", " [\n", " pos_est,\n", " [0.0, 0.0, 0.0, 1.0], # quaternion\n", " vel_est,\n", " [0.0, 0.0, 0.0], # angular velocity\n", " ]\n", " )\n", "\n", " return {\"estimate\": estimate}\n", "\n", " node = ROSNode(\n", " node_name=\"kalman_filter\",\n", " callback=filter_callback,\n", " subscribes_to=[\n", " (\"/mocap\", Vector3, \"mocap\"),\n", " (\"/baro\", Float64, \"baro\"),\n", " (\"/imu\", Vector3, \"imu\"),\n", " (\"/cmd_vel\", Twist, \"cmd_vel\"),\n", " ],\n", " publishes_to=[\n", " (\"estimate\", Odometry, \"/estimate\"),\n", " ],\n", " rate_hz=rate_hz,\n", " required_topics={\"mocap\", \"baro\", \"imu\", \"cmd_vel\"},\n", " stale_config={\n", " \"mocap\": {\"after\": 0.2, \"policy\": \"hold\"}, # 10 Hz sensor\n", " \"baro\": {\"after\": 0.1, \"policy\": \"hold\"}, # 20 Hz sensor\n", " \"imu\": {\"after\": 0.02, \"policy\": \"hold\"}, # 100 Hz sensor\n", " })\n", "\n", " return node\n", "\n", "\n", "kf_node = create_kalman_filter_node(dt=0.01, rate_hz=100.0)\n", "\n", "print(\"Multi-sensor Kalman filter node created!\")\n", "print(f\" Subscribes:\")\n", "print(f\" - /mocap (Vector3)\")\n", "print(f\" - /baro (Float64)\")\n", "print(f\" - /imu (Vector3)\")\n", "print(f\" - /cmd_vel (Twist)\")\n", "print(f\" Publishes: /estimate (Odometry)\")\n", "print(f\" Fusion: Combines 3 sensors with optimal weighting\")\n", "print(f\" Staleness policies: mocap(200ms), baro(100ms), imu(20ms)\")" ], "execution_count": null, "outputs": [], "id": "cell-16", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "## Running the Multi-Sensor ROS2 System\n" ], "id": "cell-17", "metadata": {} }, { "cell_type": "code", "source": [ "# Initialize ROS2\n", "rclpy.init()\n", "\n", "# Create all nodes\n", "print(\"Creating ROS2 nodes...\")\n", "setpoint_node.create_node()\n", "controller_node.create_node()\n", "simulator_node.create_node()\n", "kf_node.create_node()\n", "print(\"All nodes created!\")\n", "\n", "# Start all nodes\n", "print(\"\\nStarting nodes...\")\n", "setpoint_node.start()\n", "print(\" ✓ Setpoint generator running\")\n", "simulator_node.start()\n", "print(\" ✓ Crazyflie simulator running (publishing 3 sensor topics!)\")\n", "kf_node.start()\n", "print(\" ✓ Multi-sensor Kalman filter running\")\n", "controller_node.start()\n", "print(\" ✓ Position controller running\")\n", "\n", "print(\"\\n🚀 Crazyflie multi-sensor ROS system is live!\")\n", "print(\"\\nROS2 Topic Graph:\")\n", "print(\" /setpoint ← setpoint_generator\")\n", "print(\" ↓\")\n", "print(\" position_controller → /cmd_vel\")\n", "print(\" ↓\")\n", "print(\" crazyflie_simulator → /mocap, /baro, /imu, /true_state\")\n", "print(\" ↓ ↓ ↓\")\n", "print(\" └─────────┴────────┘\")\n", "print(\" ↓\")\n", "print(\" kalman_filter (FUSION!) → /estimate\")\n", "print(\" ↓ (feedback)\")\n", "print(\" position_controller\")\n", "\n", "print(\"\\n💡 Tip: Run 'rqt_graph' to see the multi-sensor fusion architecture!\")\n", "print(\" You'll see THREE sensor topics converging at kalman_filter.\")" ], "execution_count": null, "outputs": [], "id": "cell-18", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "### Data Collection\n" ], "id": "cell-19", "metadata": {} }, { "cell_type": "code", "source": [ "# Data logger node\n", "def create_data_logger_node():\n", " data_log = {\n", " \"time\": [],\n", " \"setpoint\": [],\n", " \"cmd_vel\": [],\n", " \"true_state\": [],\n", " \"mocap\": [],\n", " \"baro\": [],\n", " \"imu\": [],\n", " \"estimate\": [],\n", " }\n", "\n", " def logger_callback(tk, setpoint, cmd_vel, true_state, mocap, baro, imu, estimate):\n", " data_log[\"time\"].append(tk)\n", " data_log[\"setpoint\"].append(setpoint.copy())\n", " data_log[\"cmd_vel\"].append(cmd_vel.copy())\n", " data_log[\"true_state\"].append(true_state.copy())\n", " data_log[\"mocap\"].append(mocap.copy())\n", " data_log[\"baro\"].append(baro.copy())\n", " data_log[\"imu\"].append(imu.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", " (\"/setpoint\", Vector3, \"setpoint\"),\n", " (\"/cmd_vel\", Twist, \"cmd_vel\"),\n", " (\"/true_state\", Odometry, \"true_state\"),\n", " (\"/mocap\", Vector3, \"mocap\"),\n", " (\"/baro\", Float64, \"baro\"),\n", " (\"/imu\", Vector3, \"imu\"),\n", " (\"/estimate\", Odometry, \"estimate\"),\n", " ],\n", " publishes_to=[],\n", " rate_hz=100.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 simulation\n", "import time as pytime\n", "\n", "T_sim = 60.0\n", "print(f\"\\nRunning multi-sensor simulation for {T_sim} seconds...\")\n", "pytime.sleep(T_sim)\n", "\n", "print(f\"\\nSimulation complete! Collected {len(data_log['time'])} samples.\")" ], "execution_count": null, "outputs": [], "id": "cell-20", "metadata": { "tags": [ "skip-execution" ] } }, { "cell_type": "markdown", "source": [ "### Stopping the System\n" ], "id": "cell-21", "metadata": {} }, { "cell_type": "code", "source": [ "print(\"Stopping nodes...\")\n", "logger_node.stop()\n", "setpoint_node.stop()\n", "controller_node.stop()\n", "simulator_node.stop()\n", "kf_node.stop()\n", "\n", "rclpy.shutdown()\n", "print(\"All nodes stopped. ROS2 shutdown complete.\")" ], "execution_count": null, "outputs": [], "id": "cell-22", "metadata": {} }, { "cell_type": "markdown", "source": [ "## Visualization: Multi-Sensor Fusion Analysis\n" ], "id": "cell-23", "metadata": {} }, { "cell_type": "code", "source": [ "# Convert to arrays\n", "time_vec = np.array(data_log[\"time\"])\n", "true_states = np.array(data_log[\"true_state\"])\n", "estimates = np.array(data_log[\"estimate\"])\n", "mocap_meas = np.array(data_log[\"mocap\"])\n", "baro_meas = np.array(data_log[\"baro\"])\n", "imu_meas = np.array(data_log[\"imu\"])\n", "setpoints = np.array(data_log[\"setpoint\"])\n", "\n", "# Extract positions\n", "true_pos = true_states[:, :3]\n", "est_pos = estimates[:, :3]\n", "\n", "# Plotting\n", "from mpl_toolkits.mplot3d import Axes3D\n", "\n", "fig = plt.figure(figsize=(15, 10))\n", "\n", "# Plot 1: 3D Trajectory\n", "ax = fig.add_subplot(2, 3, 1, projection=\"3d\")\n", "ax.plot(\n", " true_pos[:, 0],\n", " true_pos[:, 1],\n", " true_pos[:, 2],\n", " \"b-\",\n", " label=\"True\",\n", " linewidth=2,\n", " alpha=0.7)\n", "ax.plot(\n", " est_pos[:, 0],\n", " est_pos[:, 1],\n", " est_pos[:, 2],\n", " \"r--\",\n", " label=\"Estimate\",\n", " linewidth=2,\n", " alpha=0.7)\n", "ax.scatter(\n", " setpoints[:, 0],\n", " setpoints[:, 1],\n", " setpoints[:, 2],\n", " c=\"green\",\n", " s=100,\n", " marker=\"*\",\n", " label=\"Setpoints\")\n", "ax.set_xlabel(\"X (m)\")\n", "ax.set_ylabel(\"Y (m)\")\n", "ax.set_zlabel(\"Z (m)\")\n", "ax.set_title(\"3D Trajectory\", fontweight=\"bold\")\n", "ax.legend()\n", "\n", "# Plot 2: XY Trajectory\n", "ax = fig.add_subplot(2, 3, 2)\n", "ax.plot(true_pos[:, 0], true_pos[:, 1], \"b-\", label=\"True\", linewidth=2, alpha=0.7)\n", "ax.plot(est_pos[:, 0], est_pos[:, 1], \"r--\", label=\"Estimate\", linewidth=2, alpha=0.7)\n", "ax.scatter(setpoints[::500, 0], setpoints[::500, 1], c=\"green\", s=100, marker=\"*\")\n", "ax.set_xlabel(\"X (m)\")\n", "ax.set_ylabel(\"Y (m)\")\n", "ax.set_title(\"XY Trajectory (Top View)\", fontweight=\"bold\")\n", "ax.legend()\n", "ax.grid(True, alpha=0.3)\n", "ax.axis(\"equal\")\n", "\n", "# Plot 3: Z Position with ALL Sensors\n", "ax = fig.add_subplot(2, 3, 3)\n", "ax.plot(time_vec, true_pos[:, 2], \"k-\", label=\"Truth\", linewidth=2, alpha=0.8)\n", "ax.plot(time_vec, est_pos[:, 2], \"r--\", label=\"Fused Estimate\", linewidth=2, alpha=0.7)\n", "ax.scatter(\n", " time_vec[::100], mocap_meas[::100, 2], c=\"green\", s=5, alpha=0.3, label=\"Mocap\"\n", ")\n", "ax.scatter(\n", " time_vec[::50], baro_meas[::50].flatten(), c=\"purple\", s=5, alpha=0.3, label=\"Baro\"\n", ")\n", "ax.set_xlabel(\"Time (s)\")\n", "ax.set_ylabel(\"Z (m)\")\n", "ax.set_title(\"Altitude: Multi-Sensor Fusion\", fontweight=\"bold\")\n", "ax.legend(fontsize=9)\n", "ax.grid(True, alpha=0.3)\n", "\n", "# Plot 4: X Velocity with IMU\n", "ax = fig.add_subplot(2, 3, 4)\n", "true_vel = true_states[:, 7:10]\n", "est_vel = estimates[:, 7:10]\n", "ax.plot(time_vec, true_vel[:, 0], \"b-\", label=\"True Vx\", alpha=0.7)\n", "ax.plot(time_vec, est_vel[:, 0], \"r--\", label=\"Est. Vx\", alpha=0.7)\n", "ax.scatter(\n", " time_vec[::100], imu_meas[::100, 0], c=\"gray\", s=5, alpha=0.3, label=\"IMU Vx\"\n", ")\n", "ax.set_xlabel(\"Time (s)\")\n", "ax.set_ylabel(\"Vx (m/s)\")\n", "ax.set_title(\"X Velocity: IMU Fusion\", fontweight=\"bold\")\n", "ax.legend()\n", "ax.grid(True, alpha=0.3)\n", "\n", "# Plot 5: Position Error\n", "ax = fig.add_subplot(2, 3, 5)\n", "pos_error = np.linalg.norm(true_pos - est_pos, axis=1)\n", "ax.plot(time_vec, pos_error * 1000, \"m-\", linewidth=1.5)\n", "ax.set_xlabel(\"Time (s)\")\n", "ax.set_ylabel(\"Error (mm)\")\n", "ax.set_title(\"3D Position Error\", fontweight=\"bold\")\n", "ax.grid(True, alpha=0.3)\n", "\n", "# Plot 6: Sensor Comparison for Z\n", "ax = fig.add_subplot(2, 3, 6)\n", "mocap_z_error = np.abs(mocap_meas[:, 2] - true_pos[:, 2])\n", "baro_z_error = np.abs(baro_meas.flatten() - true_pos[:, 2])\n", "fused_z_error = np.abs(est_pos[:, 2] - true_pos[:, 2])\n", "ax.plot(time_vec, mocap_z_error * 1000, \"g-\", label=\"Mocap Error\", alpha=0.7)\n", "ax.plot(time_vec, baro_z_error * 1000, \"purple\", label=\"Baro Error\", alpha=0.7)\n", "ax.plot(\n", " time_vec, fused_z_error * 1000, \"r--\", label=\"Fused Error\", linewidth=2, alpha=0.8\n", ")\n", "ax.set_xlabel(\"Time (s)\")\n", "ax.set_ylabel(\"Z Error (mm)\")\n", "ax.set_title(\"Fusion Improves Z Estimate\", fontweight=\"bold\")\n", "ax.legend()\n", "ax.grid(True, alpha=0.3)\n", "\n", "plt.tight_layout()\n", "plt.show()\n", "\n", "print(f\"\\nPerformance Metrics (ROS Multi-Sensor Deployment):\")\n", "print(f\" Mean position error: {np.mean(pos_error)*1000:.2f} mm\")\n", "print(f\" Max position error: {np.max(pos_error)*1000:.2f} mm\")\n", "print(f\" RMS position error: {np.sqrt(np.mean(pos_error**2))*1000:.2f} mm\")" ], "execution_count": null, "outputs": [], "id": "cell-24", "metadata": { "tags": [ "hide-input" ] } }, { "cell_type": "markdown", "source": [ "## Summary: Multi-Sensor Python → ROS2 Deployment\n", "\n", "We've successfully deployed the Crazyflie multi-sensor fusion system to ROS2!\n", "\n", "**Key Difference from TurtleBot**:\n", "\n", "| Aspect | TurtleBot (Single-Sensor) | Crazyflie (Multi-Sensor) |\n", "|--------|---------------------------|-------------------------|\n", "| Sensors | 1 topic (`/odom`) | 3 topics (`/mocap`, `/baro`, `/imu`) |\n", "| KF Subscribes | 2 topics | 4 topics |\n", "| Measurement dim | 3D | 7D (stacked) |\n", "| Noise matrix | 3×3 | 7×7 (block-diagonal) |\n", "| ROS pattern | Simple | **Sensor fusion convergence** |\n", "\n", "**Multi-Sensor ROS Pattern**:\n", "\n", "```\n", "Python: Concatenate arrays in code\n", " y = np.vstack([y1, y2, y3])\n", "\n", "ROS: Topics converge at subscriber\n", " /sensor1 ──┐\n", " /sensor2 ──┼──→ kalman_filter (fusion!)\n", " /sensor3 ──┘\n", "```\n", "\n", "**Architecture Preservation**:\n", "- Block diagram → ROS graph (with 3 sensor branches)\n", "- Sensor fusion visible in `rqt_graph`\n", "- Same KF algorithm as theory notebook\n", "\n", "**Sensor Characteristics Preserved**:\n", "- Mocap: High precision (5mm)\n", "- Barometer: Drift + bias (10cm)\n", "- IMU: Noise + spikes (0.1 m/s)\n", "\n", "**Next Step**: In the Gazebo tutorial, we'll adapt this architecture to work with Gazebo's actual sensor suite!\n" ], "id": "cell-25", "metadata": {} }, { "cell_type": "markdown", "id": "yk7fdvb67l8", "source": "# Architecture 2: Teleoperation Mode\n\nIn the previous section, we demonstrated **autonomous navigation** with setpoint tracking. Now we'll demonstrate **teleoperation mode**, where a human operator directly controls the Crazyflie's velocity using a joystick.\n\n**Key Architectural Difference**:\n- **Autonomous** (previous): Setpoint Gen → Position Controller → `/cmd_vel` → Plant → Multi-Sensor Observer\n- **Teleoperation** (this section): **Joystick Teleop** → `/cmd_vel` → Plant → Multi-Sensor Observer\n\nThe teleoperation node (**shown in RED** in diagrams) is an **external ROS2 tool**, not wrapped in `pykal.ROSNode`. For aerial vehicles like the Crazyflie, we use joystick control instead of keyboard for smoother 3D maneuvering.\n\n
\n \n
\n\n**Nodes Used in Teleoperation Mode**:\n1. ✅ **Crazyflie Simulator** - Receives `/cmd_vel` from teleop\n2. ✅ **Multi-Sensor Kalman Filter** - Continues fusing mocap, barometer, IMU data\n3. ❌ **Setpoint Generator** - Not needed (bypassed)\n4. ❌ **Position Controller** - Not needed (bypassed)\n5. 🔴 **Joystick Teleop** - External tool (`joy_node` + `teleop_twist_joy`)", "metadata": {} }, { "cell_type": "code", "id": "12l5hpkv0ffk", "source": "# Re-initialize ROS2 (if needed after previous shutdown)\nif not rclpy.ok():\n rclpy.init()\n\n# Create fresh simulator and KF nodes for teleoperation\nprint(\"Creating nodes for teleoperation mode...\")\nsimulator_teleop_cf = create_crazyflie_simulator_node(dt=0.01, rate_hz=50.0)\nkf_teleop_cf = create_kalman_filter_node(dt=0.01, rate_hz=50.0, Q=Q_crazyflie, R=R_multisensor)\n\nsimulator_teleop_cf.create_node()\nkf_teleop_cf.create_node()\nprint(\"✓ Simulator and multi-sensor Kalman filter created\")\n\n# Start nodes\nprint(\"\\nStarting nodes...\")\nsimulator_teleop_cf.start()\nprint(\" ✓ Crazyflie simulator running\")\nkf_teleop_cf.start()\nprint(\" ✓ Multi-sensor Kalman filter running\")\n\nprint(\"\\n🚀 System ready for teleoperation!\")\nprint(\"\\nActive ROS2 Topics:\")\nprint(\" /cmd_vel ← (waiting for joystick teleop)\")\nprint(\" /mocap, /baro, /imu ← crazyflie_simulator (3 sensor topics!)\")\nprint(\" /estimate ← kalman_filter\")\nprint(\"\\nNOTE: Setpoint generator and position controller are NOT running (bypassed!)\")", "metadata": {}, "execution_count": null, "outputs": [] }, { "cell_type": "code", "id": "7pbyq1ljjw7", "source": "# Create a simulated joystick teleop node\ndef create_simulated_joystick_teleop():\n \"\"\"\n Simulate joystick teleop with programmed 3D maneuvers.\n \n This mimics what teleop_twist_joy would publish,\n demonstrating the teleoperation architecture for aerial vehicles.\n \"\"\"\n import time\n \n # Programmed 3D maneuver sequence (aerial flight)\n maneuvers = [\n (0, 5, (0.3, 0.0, 0.0)), # 0-5s: Forward\n (5, 10, (0.2, 0.2, 0.1)), # 5-10s: Forward-right + climb\n (10, 15, (0.0, 0.0, 0.0)), # 10-15s: Hover\n (15, 20, (-0.2, 0.0, 0.0)), # 15-20s: Backward\n (20, 25, (0.0, 0.3, 0.0)), # 20-25s: Strafe right\n (25, 30, (0.0, 0.0, -0.1)), # 25-30s: Descend\n (30, 35, (0.2, 0.0, 0.0)), # 30-35s: Forward\n ]\n \n start_time = None\n \n def teleop_callback(tk):\n nonlocal start_time\n \n if start_time is None:\n start_time = tk\n \n elapsed = tk - start_time\n \n # Find current maneuver\n vx, vy, vz = 0.0, 0.0, 0.0\n for t_start, t_end, (vx_cmd, vy_cmd, vz_cmd) in maneuvers:\n if t_start <= elapsed < t_end:\n vx, vy, vz = vx_cmd, vy_cmd, vz_cmd\n break\n \n # Pack as Twist: [vx, vy, vz, wx, wy, wz]\n cmd_vel = np.array([vx, vy, vz, 0.0, 0.0, 0.0])\n \n return {\"cmd_vel\": cmd_vel}\n \n node = ROSNode(\n node_name=\"simulated_joystick_teleop\",\n callback=teleop_callback,\n subscribes_to=[],\n publishes_to=[\n (\"cmd_vel\", Twist, \"/cmd_vel\"),\n ],\n rate_hz=50.0)\n \n return node\n\n\n# Create and start simulated joystick teleop\nprint(\"Creating simulated joystick teleop...\")\njoystick_teleop = create_simulated_joystick_teleop()\njoystick_teleop.create_node()\njoystick_teleop.start()\nprint(\"✓ Simulated joystick teleop running (mimics joy_node + teleop_twist_joy)\")\nprint(\"\\nThis simulates a human operator flying the Crazyflie with a joystick!\")\nprint(\"In real usage, you would run: ros2 run joy joy_node && ros2 run teleop_twist_joy teleop_node\")", "metadata": {}, "execution_count": null, "outputs": [] }, { "cell_type": "code", "id": "23rjudj8ehnh", "source": "# Stop all teleoperation nodes\nprint(\"Stopping teleoperation nodes...\")\nteleop_logger_cf.stop()\njoystick_teleop.stop()\nsimulator_teleop_cf.stop()\nkf_teleop_cf.stop()\n\n# Shutdown ROS2\nrclpy.shutdown()\nprint(\"All nodes stopped. ROS2 shutdown complete.\")", "metadata": {}, "execution_count": null, "outputs": [] }, { "cell_type": "code", "id": "w8f7uwr6ane", "source": "# Convert teleoperation data to arrays\ntime_teleop_cf = np.array(teleop_data_cf[\"time\"])\ncmd_teleop_cf = np.array(teleop_data_cf[\"cmd_vel\"])\nmocap_teleop = np.array(teleop_data_cf[\"mocap\"])\nbaro_teleop = np.array(teleop_data_cf[\"baro\"])\nimu_teleop = np.array(teleop_data_cf[\"imu\"])\nest_teleop_cf = np.array(teleop_data_cf[\"estimate\"])\n\n# Extract 3D positions (ground truth from mocap)\nmocap_x = mocap_teleop[:, 0]\nmocap_y = mocap_teleop[:, 1]\nmocap_z = mocap_teleop[:, 2]\n\n# Extract estimates\nest_x_cf = est_teleop_cf[:, 0]\nest_y_cf = est_teleop_cf[:, 1]\nest_z_cf = est_teleop_cf[:, 2]\n\n# Extract velocity commands\nvx_cmd_cf = cmd_teleop_cf[:, 0]\nvy_cmd_cf = cmd_teleop_cf[:, 1]\nvz_cmd_cf = cmd_teleop_cf[:, 2]\n\n# Plotting\nfrom mpl_toolkits.mplot3d import Axes3D\n\nfig = plt.figure(figsize=(15, 10))\n\n# Plot 1: 3D Trajectory (teleoperation)\nax = fig.add_subplot(2, 3, 1, projection='3d')\nax.plot(mocap_x, mocap_y, mocap_z, 'b-', linewidth=2, label='Mocap (Ground Truth)', alpha=0.7)\nax.plot(est_x_cf, est_y_cf, est_z_cf, 'r--', linewidth=2, label='KF Estimate', alpha=0.7)\nax.scatter(mocap_x[0], mocap_y[0], mocap_z[0], c='green', s=150, marker='o', label='Start')\nax.scatter(mocap_x[-1], mocap_y[-1], mocap_z[-1], c='red', s=150, marker='X', label='End')\nax.set_xlabel('X (m)', fontsize=10)\nax.set_ylabel('Y (m)', fontsize=10)\nax.set_zlabel('Z (m)', fontsize=10)\nax.set_title('Teleoperation: 3D Flight Path', fontsize=13, fontweight='bold')\nax.legend()\n\n# Plot 2: XY Trajectory (top view)\nax = fig.add_subplot(2, 3, 2)\nax.plot(mocap_x, mocap_y, 'b-', linewidth=2, label='Mocap', alpha=0.7)\nax.plot(est_x_cf, est_y_cf, 'r--', linewidth=2, label='KF Estimate', alpha=0.7)\nax.scatter(mocap_x[0], mocap_y[0], c='green', s=150, marker='o')\nax.scatter(mocap_x[-1], mocap_y[-1], c='red', s=150, marker='X')\nax.set_xlabel('X (m)', fontsize=10)\nax.set_ylabel('Y (m)', fontsize=10)\nax.set_title('XY Trajectory (Top View)', fontsize=13, fontweight='bold')\nax.legend()\nax.grid(True, alpha=0.3)\nax.axis('equal')\n\n# Plot 3: Altitude (Z)\nax = fig.add_subplot(2, 3, 3)\nax.plot(time_teleop_cf, mocap_z, 'b-', label='Mocap Z', alpha=0.7)\nax.plot(time_teleop_cf, est_z_cf, 'r--', linewidth=2, label='KF Est. Z', alpha=0.7)\nax.plot(time_teleop_cf, baro_teleop, 'g:', alpha=0.5, label='Barometer')\nax.set_xlabel('Time (s)', fontsize=10)\nax.set_ylabel('Altitude (m)', fontsize=10)\nax.set_title('Altitude: Multi-Sensor Fusion', fontsize=13, fontweight='bold')\nax.legend()\nax.grid(True, alpha=0.3)\n\n# Plot 4: Velocity Commands\nax = fig.add_subplot(2, 3, 4)\nax.plot(time_teleop_cf, vx_cmd_cf, label='vx', alpha=0.7)\nax.plot(time_teleop_cf, vy_cmd_cf, label='vy', alpha=0.7)\nax.plot(time_teleop_cf, vz_cmd_cf, label='vz', alpha=0.7)\nax.set_xlabel('Time (s)', fontsize=10)\nax.set_ylabel('Command (m/s)', fontsize=10)\nax.set_title('Joystick Commands to /cmd_vel', fontsize=13, fontweight='bold')\nax.legend()\nax.grid(True, alpha=0.3)\n\n# Plot 5: Position Error\nax = fig.add_subplot(2, 3, 5)\npos_error_cf = np.sqrt((mocap_x - est_x_cf)**2 + (mocap_y - est_y_cf)**2 + (mocap_z - est_z_cf)**2)\nax.plot(time_teleop_cf, pos_error_cf * 1000, 'm-', linewidth=1.5)\nax.set_xlabel('Time (s)', fontsize=10)\nax.set_ylabel('Error (mm)', fontsize=10)\nax.set_title('Multi-Sensor KF Error', fontsize=13, fontweight='bold')\nax.grid(True, alpha=0.3)\n\n# Plot 6: Sensor Data (IMU velocity)\nax = fig.add_subplot(2, 3, 6)\nax.plot(time_teleop_cf, imu_teleop[:, 0], label='IMU vx', alpha=0.6)\nax.plot(time_teleop_cf, imu_teleop[:, 1], label='IMU vy', alpha=0.6)\nax.plot(time_teleop_cf, imu_teleop[:, 2], label='IMU vz', alpha=0.6)\nax.set_xlabel('Time (s)', fontsize=10)\nax.set_ylabel('Velocity (m/s)', fontsize=10)\nax.set_title('IMU Sensor Data', fontsize=13, fontweight='bold')\nax.legend()\nax.grid(True, alpha=0.3)\n\nplt.tight_layout()\nplt.show()\n\nprint(f\"\\nTeleoperation Performance (3D Flight):\")\nprint(f\" Mean position error: {np.mean(pos_error_cf)*1000:.2f} mm\")\nprint(f\" Max position error: {np.max(pos_error_cf)*1000:.2f} mm\")\nprint(f\" Total 3D distance: {np.sum(np.sqrt(np.diff(mocap_x)**2 + np.diff(mocap_y)**2 + np.diff(mocap_z)**2)):.2f} m\")\nprint(f\" Altitude range: [{np.min(mocap_z):.2f}, {np.max(mocap_z):.2f}] m\")", "metadata": {}, "execution_count": null, "outputs": [] }, { "cell_type": "markdown", "id": "n1bcaku1xwc", "source": "## Summary: Two ROS Architectures with Multi-Sensor Fusion\n\nThis notebook demonstrated **two distinct control architectures** for the Crazyflie ROS system, both utilizing **multi-sensor fusion**:\n\n### Architecture 1: Autonomous Navigation (Setpoint Tracking)\n\n**Nodes Active**:\n- ✅ Setpoint Generator (`pykal.ROSNode`)\n- ✅ Position Controller (`pykal.ROSNode`)\n- ✅ Crazyflie Simulator (`pykal.ROSNode`) - publishes 3 sensor topics\n- ✅ Multi-Sensor Kalman Filter (`pykal.ROSNode`) - fuses mocap + baro + IMU\n\n**Data Flow**: Setpoint Gen → Controller → `/cmd_vel` → Plant → [mocap, baro, IMU] → Observer → (feedback)\n\n**Use Case**: Autonomous waypoint following with high-level planning\n\n---\n\n### Architecture 2: Teleoperation (Manual Joystick Control)\n\n**Nodes Active**:\n- 🔴 Joystick Teleop (external tools: `joy_node` + `teleop_twist_joy`)\n- ✅ Crazyflie Simulator (`pykal.ROSNode`) - publishes 3 sensor topics\n- ✅ Multi-Sensor Kalman Filter (`pykal.ROSNode`) - fuses mocap + baro + IMU\n\n**Nodes Bypassed**:\n- ❌ Setpoint Generator (not needed)\n- ❌ Position Controller (not needed)\n\n**Data Flow**: Joystick Teleop → `/cmd_vel` → Plant → [mocap, baro, IMU] → Observer\n\n**Use Case**: Direct manual 3D flight control for intervention or testing\n\n---\n\n### Key Insights\n\n1. **Multi-Sensor Fusion Preserved**: The 7D measurement fusion (mocap + baro + IMU) works in both modes\n2. **Modularity**: Easy mode switching by activating/deactivating nodes\n3. **Red Nodes**: External teleop tools shown in RED to distinguish from `pykal.ROSNode` wrappers\n4. **Observer Persistence**: Kalman filter provides state estimation in both autonomous and teleop modes\n5. **3D Control**: Aerial vehicles benefit from joystick's analog input for smooth 3D maneuvering\n6. **Common Pattern**: Dual-mode architecture (autonomous + teleop) is standard for drones\n\n**Architectural Consistency**:\n- **Theory** ([sensor fusion notebook](../theory_to_python/crazyflie_sensor_fusion.ipynb)): Multi-sensor fusion as DynamicalSystem\n- **ROS** (this notebook): Multi-sensor fusion as ROSNode with 3 topic subscriptions\n- **Gazebo** ([next tutorial](../ros_to_gazebo/crazyflie_gazebo_integration.ipynb)): Same architecture with physics simulation\n\nThe multi-sensor architecture remains consistent across the entire pipeline: Theory → Software → ROS → Gazebo → Hardware!", "metadata": {} }, { "cell_type": "markdown", "id": "amgdtqjx66d", "source": "## Visualization: Teleoperation Results", "metadata": {} }, { "cell_type": "markdown", "id": "292mi47gxse", "source": "### Stopping Teleoperation Nodes", "metadata": {} }, { "cell_type": "code", "id": "zvnrpcacge", "source": "# Create logger for teleoperation data\ndef create_teleop_logger_cf():\n \"\"\"Logger for teleoperation mode (no setpoint reference).\"\"\"\n teleop_data_cf = {\n \"time\": [],\n \"cmd_vel\": [],\n \"mocap\": [],\n \"baro\": [],\n \"imu\": [],\n \"estimate\": [],\n }\n \n def logger_callback(tk, cmd_vel, mocap, baro, imu, estimate):\n teleop_data_cf[\"time\"].append(tk)\n teleop_data_cf[\"cmd_vel\"].append(cmd_vel.copy())\n teleop_data_cf[\"mocap\"].append(mocap.copy())\n teleop_data_cf[\"baro\"].append(baro.copy())\n teleop_data_cf[\"imu\"].append(imu.copy())\n teleop_data_cf[\"estimate\"].append(estimate.copy())\n return {}\n \n node = ROSNode(\n node_name=\"teleop_logger_cf\",\n callback=logger_callback,\n subscribes_to=[\n (\"/cmd_vel\", Twist, \"cmd_vel\"),\n (\"/mocap\", Vector3, \"mocap\"),\n (\"/baro\", Float64, \"baro\"),\n (\"/imu\", Vector3, \"imu\"),\n (\"/estimate\", Odometry, \"estimate\"),\n ],\n publishes_to=[],\n rate_hz=50.0)\n \n return node, teleop_data_cf\n\n\n# Create and start logger\nfrom std_msgs.msg import Float64\nfrom geometry_msgs.msg import Vector3\n\nteleop_logger_cf, teleop_data_cf = create_teleop_logger_cf()\nteleop_logger_cf.create_node()\nteleop_logger_cf.start()\nprint(\"Data logger started!\")\n\n# Run teleoperation for 35 seconds\nimport time as pytime\n\nT_teleop_cf = 35.0\nprint(f\"\\nRunning teleoperation for {T_teleop_cf} seconds...\")\nprint(\"Crazyflie is being controlled via simulated joystick input!\")\nprint(\"Multi-sensor fusion (mocap + baro + IMU) continues in background...\")\npytime.sleep(T_teleop_cf)\n\nprint(f\"\\nTeleoperation complete! Collected {len(teleop_data_cf['time'])} samples.\")", "metadata": {}, "execution_count": null, "outputs": [] }, { "cell_type": "markdown", "id": "hg3g29n80ns", "source": "### Data Collection During Teleoperation", "metadata": {} }, { "cell_type": "markdown", "id": "y3ue82zo3v", "source": "## Running the Joystick Teleop Tool\n\nTo control the Crazyflie manually with a joystick, we use the standard ROS2 `joy` and `teleop_twist_joy` packages.\n\n**In separate terminals**, run:\n\n```bash\n# Terminal 1: Start joystick node\nros2 run joy joy_node\n\n# Terminal 2: Start teleop twist joy node\nros2 run teleop_twist_joy teleop_node\n```\n\n**Joystick Controls** (typical configuration):\n- **Left stick Y-axis** - Forward/backward velocity\n- **Left stick X-axis** - Left/right velocity\n- **Right stick X-axis** - Yaw rotation\n- **Buttons** - Enable/disable, speed scaling\n\n:::{note}\nFor aerial vehicles, joystick control provides smoother 3D velocity commands compared to keyboard discrete inputs. The teleop publishes 6-DOF `Twist` messages: `[vx, vy, vz, wx, wy, wz]`.\n:::\n\n**For this notebook demo**, we'll simulate joystick commands programmatically to demonstrate the architecture:", "metadata": {} }, { "cell_type": "markdown", "id": "q74mw06h33", "source": "## Setting Up Teleoperation Mode\n\nWe only need to run the **simulator** and **multi-sensor observer** nodes. The setpoint generator and position controller are not used in teleoperation mode.", "metadata": {} }, { "cell_type": "markdown", "source": [ "[← Dynamical Systems as ROS Nodes](../../../getting_started/python_to_ros/dynamical_systems_as_ros_nodes.rst)\n" ], "id": "cell-26", "metadata": {} } ], "metadata": { "kernelspec": { "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { "name": "python", "version": "3.12.0" } }, "nbformat": 4, "nbformat_minor": 5 }