From 72f85c37a57c63f2e19b2e84f4ad044fb85e2e28 Mon Sep 17 00:00:00 2001
From: default <adithcool.5@gmail.com>
Date: Fri, 2 Jan 2026 07:22:44 +0000
Subject: [PATCH] Update Gazebo, add ROS bridging, and implement controller
 warmup.

---
 .gitignore                            |  6 +++-
 activate.sh                           | 27 -----------------
 drone_controller.py                   | 10 ++++++-
 gazebo/launch/drone_landing.launch.py |  4 ++-
 gazebo/worlds/drone_landing.sdf       | 42 +++++++++++++++++++++++----
 5 files changed, 54 insertions(+), 35 deletions(-)
 delete mode 100755 activate.sh

diff --git a/.gitignore b/.gitignore
index 0813a70..843cfaf 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,4 +2,8 @@
 
 venv/
 __pycache__/
-notes/
\ No newline at end of file
+notes/
+
+activate.sh
+activate.ps1
+activate.bat
\ No newline at end of file
diff --git a/activate.sh b/activate.sh
deleted file mode 100755
index 1216f17..0000000
--- a/activate.sh
+++ /dev/null
@@ -1,27 +0,0 @@
-#!/bin/bash
-# =============================================================================
-# Drone Competition - Environment Activation Script
-# =============================================================================
-# This script activates both ROS 2 and the Python virtual environment.
-#
-# Usage:
-#   source activate.sh
-# =============================================================================
-
-# Get the directory where this script is located
-SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
-
-# Source ROS 2
-source /opt/ros/jazzy/setup.bash
-echo "[OK] ROS 2 jazzy sourced"
-
-# Activate Python virtual environment
-source "$SCRIPT_DIR/venv/bin/activate"
-echo "[OK] Python venv activated"
-
-echo ""
-echo "Environment ready! You can now run:"
-echo "  python simulation_host.py    # PyBullet"
-echo "  python gazebo_bridge.py       # Gazebo"
-echo "  python ros_bridge.py"
-echo ""
diff --git a/drone_controller.py b/drone_controller.py
index 1cff220..c31001b 100644
--- a/drone_controller.py
+++ b/drone_controller.py
@@ -78,6 +78,7 @@ class DroneController(Node):
             self._latest_telemetry = json.loads(msg.data)
             if not self._telemetry_received:
                 self._telemetry_received = True
+                self._warmup_count = 0
                 self.get_logger().info('First telemetry received!')
         except json.JSONDecodeError as e:
             self.get_logger().warning(f'Failed to parse telemetry: {e}')
@@ -95,7 +96,14 @@ class DroneController(Node):
         if self._landing_complete:
             return
         
-        if self._check_landing_complete():
+        # Warmup period - wait for stable telemetry before checking landing
+        if not hasattr(self, '_warmup_count'):
+            self._warmup_count = 0
+        self._warmup_count += 1
+        if self._warmup_count < 100:  # Wait ~2 seconds at 50Hz
+            # Still fly with controller, just don't check for landing yet
+            pass
+        elif self._check_landing_complete():
             self._landing_complete = True
             self.get_logger().info('=' * 50)
             self.get_logger().info('LANDING COMPLETE!')
diff --git a/gazebo/launch/drone_landing.launch.py b/gazebo/launch/drone_landing.launch.py
index 516de09..5c05439 100644
--- a/gazebo/launch/drone_landing.launch.py
+++ b/gazebo/launch/drone_landing.launch.py
@@ -49,8 +49,10 @@ def generate_launch_description():
             executable='parameter_bridge',
             name='gz_bridge',
             arguments=[
-                # Velocity commands (bidirectional)
+                # Drone velocity commands (ROS to Gazebo)
                 '/drone/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist',
+                # Rover velocity commands (ROS to Gazebo)
+                '/rover/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist',
                 # Odometry (from Gazebo to ROS)
                 '/model/drone/odometry@nav_msgs/msg/Odometry[ignition.msgs.Odometry',
                 # IMU (from Gazebo to ROS)
diff --git a/gazebo/worlds/drone_landing.sdf b/gazebo/worlds/drone_landing.sdf
index 14e0886..132a1a9 100644
--- a/gazebo/worlds/drone_landing.sdf
+++ b/gazebo/worlds/drone_landing.sdf
@@ -1,7 +1,7 @@
 <?xml version="1.0" ?>
 <!--
   Drone Landing Simulation - Ignition Fortress World
-  Compatible with ROS 2 Humble
+  With velocity control for drone and rover
 -->
 <sdf version="1.8">
   <world name="drone_landing_world">
@@ -62,11 +62,20 @@
       </link>
     </model>
     
-    <!-- Landing Pad (Rover) -->
+    <!-- Landing Pad (Rover) - Now with velocity control -->
     <model name="landing_pad">
-      <static>true</static>
       <pose>0 0 0.15 0 0 0</pose>
+      
       <link name="base_link">
+        <inertial>
+          <mass>10.0</mass>
+          <inertia>
+            <ixx>1.0</ixx><ixy>0</ixy><ixz>0</ixz>
+            <iyy>1.0</iyy><iyz>0</iyz>
+            <izz>1.0</izz>
+          </inertia>
+        </inertial>
+        
         <visual name="visual">
           <geometry>
             <box><size>1.0 1.0 0.3</size></box>
@@ -81,6 +90,7 @@
             <box><size>1.0 1.0 0.3</size></box>
           </geometry>
         </collision>
+        
         <!-- H marker -->
         <visual name="h_center">
           <pose>0 0 0.151 0 0 0</pose>
@@ -107,11 +117,26 @@
           </material>
         </visual>
       </link>
+      
+      <!-- Rover velocity control - subscribes to /rover/cmd_vel -->
+      <plugin filename="libignition-gazebo-velocity-control-system.so"
+              name="ignition::gazebo::systems::VelocityControl">
+        <topic>/rover/cmd_vel</topic>
+        <initial_linear>0 0 0</initial_linear>
+      </plugin>
+      
+      <!-- Keep rover on ground -->
+      <plugin filename="libignition-gazebo-planar-move-system.so"
+              name="ignition::gazebo::systems::PlanarMove">
+        <cmd_topic>/rover/cmd_vel</cmd_topic>
+        <odom_topic>/rover/odom</odom_topic>
+      </plugin>
+      
     </model>
     
-    <!-- Drone -->
+    <!-- Drone with velocity control -->
     <model name="drone">
-      <pose>0 0 5 0 0 0</pose>
+      <pose>0 0 2 0 0 0</pose>
       
       <link name="base_link">
         <inertial>
@@ -187,6 +212,13 @@
         </sensor>
       </link>
       
+      <!-- Drone velocity control - subscribes to /drone/cmd_vel -->
+      <plugin filename="libignition-gazebo-velocity-control-system.so"
+              name="ignition::gazebo::systems::VelocityControl">
+        <topic>/drone/cmd_vel</topic>
+        <initial_linear>0 0 0</initial_linear>
+      </plugin>
+      
       <!-- Odometry Publisher -->
       <plugin filename="libignition-gazebo-odometry-publisher-system.so"
               name="ignition::gazebo::systems::OdometryPublisher">