diff --git a/README.md b/README.md index b7e60e0..10ce82d 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ A GPS-denied drone landing simulation using relative sensors (IMU, altimeter, ca ## Quick Start -### Standalone Mode (Any Platform) +### Standalone Mode (Any Platform - No ROS 2 Required) ```bash source activate.sh # Linux/macOS @@ -13,7 +13,7 @@ source activate.sh # Linux/macOS python standalone_simulation.py --pattern circular --speed 0.3 ``` -### Two-Terminal Mode - PyBullet (with ROS 2) +### PyBullet + ROS 2 (Two Terminals) **Terminal 1 - Simulator:** ```bash @@ -25,20 +25,18 @@ python simulation_host.py python run_bridge.py --pattern circular --speed 0.3 ``` -### Two-Terminal Mode - Gazebo (Linux only) +### Gazebo + ROS 2 (Two Terminals - Linux/WSL2) -**Terminal 1 - Gazebo:** +**Terminal 1 - Launch Gazebo + Bridge:** ```bash -gz sim gazebo/worlds/drone_landing.sdf +ros2 launch gazebo/launch/drone_landing.launch.py ``` -**Terminal 2 - Bridge + Controllers:** +**Terminal 2 - Run Controllers:** ```bash python run_gazebo.py --pattern circular --speed 0.3 ``` -That's it! Only 2 terminals needed. - ## Installation | Platform | Command | @@ -53,8 +51,8 @@ That's it! Only 2 terminals needed. | Feature | Ubuntu | Arch | macOS | Windows | WSL2 | |---------|--------|------|-------|---------|------| | Standalone | ✅ | ✅ | ✅ | ✅ | ✅ | -| ROS 2 Mode | ✅ | ⚠️ | ❌ | ❌ | ✅ | -| Gazebo | ✅ | ⚠️ | ❌ | ❌ | ✅ | +| PyBullet + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ | +| Gazebo + ROS 2 | ✅ | ⚠️ | ❌ | ❌ | ✅ | ## Files @@ -62,21 +60,32 @@ That's it! Only 2 terminals needed. |------|-------------| | `standalone_simulation.py` | All-in-one (no ROS 2 required) | | `simulation_host.py` | PyBullet simulator server | -| `run_bridge.py` | **PyBullet bridge + Controllers** | -| `run_gazebo.py` | **Gazebo bridge + Controllers** | +| `run_bridge.py` | PyBullet bridge + Controllers | +| `run_gazebo.py` | Gazebo bridge + Controllers | | `config.py` | **Configuration file** (edit to customize) | -| `drone_controller.py` | Drone landing logic (edit this) | -| `rover_controller.py` | Moving landing pad | +| `drone_controller.py` | **Your landing algorithm** (edit this!) | +| `rover_controller.py` | Moving landing pad controller | -## Options +## Configuration + +Edit `config.py` to customize: +- Drone/rover starting positions +- Physical properties (mass, size) +- Controller gains (Kp, Kd) +- Landing detection thresholds + +## Command Line Options ```bash -# Standalone +# Standalone (no ROS 2) python standalone_simulation.py --pattern circular --speed 0.3 -# ROS 2 mode +# PyBullet + ROS 2 python run_bridge.py --pattern circular --speed 0.3 --host +# Gazebo + ROS 2 +python run_gazebo.py --pattern circular --speed 0.3 + Options: --pattern, -p stationary, linear, circular, square, random --speed, -s Speed in m/s (default: 0.5) @@ -88,18 +97,20 @@ Options: | Sensor | Data | |--------|------| -| **IMU** | Orientation, angular velocity | +| **IMU** | Orientation (roll, pitch, yaw), angular velocity | | **Altimeter** | Altitude, vertical velocity | -| **Velocity** | Estimated horizontal velocity | -| **Camera** | 320x240 downward-facing image | -| **Landing Pad** | Relative position when visible | +| **Velocity** | Estimated horizontal velocity (x, y, z) | +| **Camera** | 320x240 downward-facing JPEG image | +| **Landing Pad** | Relative position when visible in camera FOV | ## Documentation | Document | Description | |----------|-------------| | [Installation](docs/installation.md) | Platform setup guides + WSL2 | -| [Architecture](docs/architecture.md) | System components | +| [Architecture](docs/architecture.md) | System components diagram | +| [Gazebo Guide](docs/gazebo.md) | Gazebo-specific instructions | +| [PyBullet Guide](docs/pybullet.md) | PyBullet-specific instructions | | [Protocol](docs/protocol.md) | Sensor data formats | | [Drone Guide](docs/drone_guide.md) | Landing algorithm guide | diff --git a/docs/architecture.md b/docs/architecture.md index 939f000..57aa553 100644 --- a/docs/architecture.md +++ b/docs/architecture.md @@ -15,11 +15,12 @@ Single-process simulation - no ROS 2 or networking required: │ │ PyBullet Physics + Camera │ │ │ │ Built-in Landing Controller │ │ │ │ Rover Movement Patterns │ │ +│ │ Configuration from config.py │ │ │ └──────────────────────────────────┘ │ └────────────────────────────────────────┘ ``` -### 2. PyBullet + ROS 2 Mode (2 Terminals) +### 2. PyBullet + ROS 2 Mode (Two Terminals) ``` Terminal 1 Terminal 2 @@ -33,45 +34,65 @@ Terminal 1 Terminal 2 └──────────────────────────┘ ``` -### 3. Gazebo + ROS 2 Mode (2 Terminals, Linux Only) +Data flow: +- RoverController publishes position → Bridge sends to Simulator +- Simulator moves rover visually AND sends back telemetry +- DroneController receives telemetry, publishes commands +- Bridge forwards commands to Simulator + +### 3. Gazebo + ROS 2 Mode (Two Terminals, Linux/WSL2) ``` -Terminal 1 Terminal 2 -┌──────────────────┐ ┌──────────────────────────┐ -│ Gazebo │◄─ROS───►│ run_gazebo.py │ -│ (gz sim ...) │ │ ┌────────────────────┐ │ -│ │ │ │ GazeboBridge │ │ -│ │ │ │ DroneController │ │ -│ │ │ │ RoverController │ │ -└──────────────────┘ │ └────────────────────┘ │ - └──────────────────────────┘ +Terminal 1 Terminal 2 +┌───────────────────────────┐ ┌──────────────────────────┐ +│ ros2 launch ... .launch.py│ │ run_gazebo.py │ +│ ┌─────────────────────┐ │ │ ┌────────────────────┐ │ +│ │ Gazebo (ign gazebo) │ │ │ │ GazeboBridge │ │ +│ │ - Drone (vel ctrl) │ │◄────►│ │ DroneController │ │ +│ │ - Rover (vel ctrl) │ │ ROS │ │ RoverController │ │ +│ ├─────────────────────┤ │ │ └────────────────────┘ │ +│ │ ros_gz_bridge │ │ └──────────────────────────┘ +│ └─────────────────────┘ │ +└───────────────────────────┘ ``` +Data flow: +- RoverController publishes to `/rover/cmd_vel` → Gazebo moves rover +- Gazebo publishes odometry → GazeboBridge converts to telemetry +- DroneController receives telemetry, publishes to `/cmd_vel` +- GazeboBridge forwards to `/drone/cmd_vel` → Gazebo moves drone + ## Components | File | Description | |------|-------------| +| `config.py` | Central configuration (positions, physics, gains) | | `standalone_simulation.py` | All-in-one simulation | | `simulation_host.py` | PyBullet physics server (UDP) | | `run_bridge.py` | PyBullet bridge + controllers | | `run_gazebo.py` | Gazebo bridge + controllers | -| `drone_controller.py` | Landing algorithm | +| `drone_controller.py` | **Your landing algorithm** | | `rover_controller.py` | Moving landing pad | +| `ros_bridge.py` | ROS-UDP bridge (used by run_bridge.py) | +| `gazebo_bridge.py` | Gazebo-ROS bridge (used by run_gazebo.py) | | `gazebo/launch/drone_landing.launch.py` | ROS 2 launch file for Gazebo | +| `gazebo/worlds/drone_landing.sdf` | Gazebo world with drone + rover | -## ROS Topics +## ROS 2 Topics | Topic | Type | Description | |-------|------|-------------| -| `/cmd_vel` | `Twist` | Drone velocity commands | -| `/drone/telemetry` | `String` | GPS-denied sensor data | -| `/rover/telemetry` | `String` | Rover position (internal) | +| `/cmd_vel` | `Twist` | Drone commands from DroneController | +| `/drone/cmd_vel` | `Twist` | Drone commands to Gazebo | +| `/drone/telemetry` | `String` | GPS-denied sensor data (JSON) | +| `/rover/cmd_vel` | `Twist` | Rover velocity to simulator | +| `/rover/telemetry` | `String` | Rover position (JSON) | ## Network Configuration All components default to `0.0.0.0` for network accessibility. -### Remote Setup +### Remote Setup (PyBullet mode) **Machine 1 (with display):** ```bash @@ -87,8 +108,8 @@ python run_bridge.py --host 192.168.1.100 | Port | Direction | Content | |------|-----------|---------| -| 5555 | Bridge → Simulator | Commands | -| 5556 | Simulator → Bridge | Telemetry | +| 5555 | Bridge → Simulator | Commands (JSON) | +| 5556 | Simulator → Bridge | Telemetry (JSON) | ## GPS-Denied Sensors @@ -96,11 +117,23 @@ All modes provide the same sensor data: | Sensor | Data | |--------|------| -| IMU | Orientation, angular velocity | -| Altimeter | Altitude, vertical velocity | -| Velocity | Estimated from optical flow | -| Camera | 320x240 downward JPEG | -| Landing Pad | Relative position (when visible) | +| **IMU** | Orientation (roll, pitch, yaw), angular velocity | +| **Altimeter** | Altitude above ground, vertical velocity | +| **Velocity** | Estimated velocity (x, y, z) | +| **Camera** | 320x240 downward JPEG (base64) | +| **Landing Pad** | Relative position (x, y, distance) when visible | + +## Configuration (config.py) + +| Section | Parameters | +|---------|------------| +| `DRONE` | mass, size, color, start_position, thrust/torque scales | +| `ROVER` | size, color, start_position, default_pattern, default_speed | +| `CAMERA` | width, height, fov, jpeg_quality | +| `PHYSICS` | gravity, timestep, telemetry_rate | +| `CONTROLLER` | Kp_z, Kd_z, Kp_xy, Kd_xy, rate | +| `LANDING` | success_distance, success_velocity, height_threshold | +| `NETWORK` | host, command_port, telemetry_port | ## Platform Support diff --git a/docs/drone_guide.md b/docs/drone_guide.md index af8e9e0..a707528 100644 --- a/docs/drone_guide.md +++ b/docs/drone_guide.md @@ -7,7 +7,10 @@ Implement your landing algorithm in `drone_controller.py`. 1. Edit `drone_controller.py` 2. Find `calculate_landing_maneuver()` 3. Implement your algorithm -4. Test: `python controllers.py --pattern stationary` +4. Test with any mode: + - `python standalone_simulation.py --pattern stationary` (standalone) + - `python run_bridge.py --pattern stationary` (PyBullet + ROS 2) + - `python run_gazebo.py --pattern stationary` (Gazebo + ROS 2) ## GPS-Denied Challenge @@ -176,12 +179,29 @@ def process_camera(telemetry): ## Testing ```bash -# Easy -python controllers.py --pattern stationary +# Easy - stationary rover +python standalone_simulation.py --pattern stationary -# Medium -python controllers.py --pattern circular --speed 0.2 +# Medium - slow circular movement +python standalone_simulation.py --pattern circular --speed 0.2 -# Hard -python controllers.py --pattern random --speed 0.3 +# Hard - faster random movement +python standalone_simulation.py --pattern random --speed 0.3 + +# With ROS 2 (Gazebo) +ros2 launch gazebo/launch/drone_landing.launch.py # Terminal 1 +python run_gazebo.py --pattern circular # Terminal 2 +``` + +## Configuration + +Edit `config.py` to tune controller gains: + +```python +CONTROLLER = { + "Kp_z": 0.5, # Altitude proportional gain + "Kd_z": 0.3, # Altitude derivative gain + "Kp_xy": 0.3, # Horizontal proportional gain + "Kd_xy": 0.2, # Horizontal derivative gain +} ``` diff --git a/docs/gazebo.md b/docs/gazebo.md index b325ae9..2f60580 100644 --- a/docs/gazebo.md +++ b/docs/gazebo.md @@ -1,13 +1,13 @@ -# Gazebo Simulation +# Gazebo Simulation Guide -Running the GPS-denied drone simulation with Gazebo (Linux only). +Running the GPS-denied drone simulation with Gazebo Ignition Fortress on Linux/WSL2. -## Quick Start (2 Terminals) +## Quick Start (Two Terminals) -**Terminal 1 - Start Gazebo:** +**Terminal 1 - Launch Gazebo + Bridge:** ```bash source activate.sh -gz sim gazebo/worlds/drone_landing.sdf +ros2 launch gazebo/launch/drone_landing.launch.py ``` **Terminal 2 - Run Controllers:** @@ -16,7 +16,9 @@ source activate.sh python run_gazebo.py --pattern circular --speed 0.3 ``` -## Options +Both the drone AND rover will move! + +## Command Options ```bash python run_gazebo.py --help @@ -28,123 +30,130 @@ Options: --no-rover Disable rover controller ``` -## Spawning the Drone +## How It Works -If the drone isn't in the world, spawn it: +1. **Gazebo** runs the physics simulation with: + - Drone with `VelocityControl` plugin (responds to `/drone/cmd_vel`) + - Rover with `VelocityControl` plugin (responds to `/rover/cmd_vel`) -```bash -gz service -s /world/drone_landing_world/create \ - --reqtype gz.msgs.EntityFactory \ - --reptype gz.msgs.Boolean \ - --req 'sdf_filename: "gazebo/models/drone/model.sdf", name: "drone"' -``` +2. **ros_gz_bridge** connects Gazebo topics to ROS 2 + +3. **run_gazebo.py** starts: + - `GazeboBridge` - converts ROS topics to telemetry format + - `DroneController` - your landing algorithm + - `RoverController` - moves the landing pad ## GPS-Denied Sensors -The `run_gazebo.py` script provides the same sensor interface as PyBullet: +The `GazeboBridge` provides the same sensor interface as PyBullet: | Sensor | Source | |--------|--------| -| IMU | Gazebo odometry | +| IMU | Gazebo odometry orientation | | Altimeter | Gazebo Z position | | Velocity | Gazebo twist | -| Camera | Gazebo camera sensor | +| Camera | Gazebo camera sensor (if enabled) | | Landing Pad | Computed from relative position | -## Gazebo Topics +## Topics + +### ROS 2 Topics (your code uses these) + +| Topic | Type | Direction | +|-------|------|-----------| +| `/cmd_vel` | `Twist` | Input (from DroneController) | +| `/drone/telemetry` | `String` | Output (to DroneController) | +| `/rover/telemetry` | `String` | Output (rover position) | + +### Gazebo Topics (bridged automatically) | Topic | Type | Description | |-------|------|-------------| -| `/drone/cmd_vel` | `Twist` | Velocity commands | +| `/drone/cmd_vel` | `Twist` | Drone velocity commands | +| `/rover/cmd_vel` | `Twist` | Rover velocity commands | | `/model/drone/odometry` | `Odometry` | Drone state | -| `/drone/camera` | `Image` | Camera images | +| `/drone/imu` | `IMU` | IMU sensor data | +| `/clock` | `Clock` | Simulation time | -## Headless Mode +## Headless Mode (WSL2 / No GPU) -Run without GUI: +Run Gazebo without GUI: ```bash -gz sim -s gazebo/worlds/drone_landing.sdf +# Server mode only +ign gazebo -s gazebo/worlds/drone_landing.sdf ``` ---- - -## ROS 2 Launch File (Advanced) - -**Location:** `gazebo/launch/drone_landing.launch.py` - -This launch file automates Gazebo startup for ROS 2 integration: -1. Starts Gazebo with the world -2. Spawns the drone automatically -3. Starts `ros_gz_bridge` to connect topics - -### Option 1: Direct Launch (Recommended) - -Run the launch file directly with `ros2 launch`: - +Then run the bridge manually: ```bash -source activate.sh -ros2 launch gazebo/launch/drone_landing.launch.py +ros2 run ros_gz_bridge parameter_bridge \ + /drone/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist \ + /rover/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist \ + /model/drone/odometry@nav_msgs/msg/Odometry[ignition.msgs.Odometry ``` -Then in another terminal, run just the controllers: -```bash -source activate.sh -python controllers.py --pattern circular -``` +## World File Details -### Option 2: Include in Your ROS 2 Package +The world file `gazebo/worlds/drone_landing.sdf` includes: -If you have a ROS 2 package, copy the launch file and use: -```bash -ros2 launch my_drone_package drone_landing.launch.py -``` +- **Drone** at (0, 0, 2) with: + - `VelocityControl` plugin for movement + - `OdometryPublisher` plugin for telemetry + - IMU sensor -### Launch Arguments - -| Argument | Default | Description | -|----------|---------|-------------| -| `use_sim_time` | `true` | Use Gazebo clock | -| `headless` | `false` | Run without GUI | - -```bash -ros2 launch gazebo/launch/drone_landing.launch.py headless:=true -``` - -### Topics Bridged by Launch File - -| ROS 2 Topic | Description | -|-------------|-------------| -| `/drone/cmd_vel` | Velocity commands | -| `/model/drone/odometry` | Drone state | -| `/drone/imu` | IMU sensor | -| `/clock` | Simulation time | - ---- +- **Landing Pad (Rover)** at (0, 0, 0.15) with: + - `VelocityControl` plugin for movement + - Visual H marker ## Troubleshooting +### Drone falls immediately + +The drone should hover with the controller running. If it falls: +1. Check that `run_gazebo.py` is running +2. Verify the bridge shows "Passing message from ROS" +3. Check `/drone/cmd_vel` topic: `ros2 topic echo /drone/cmd_vel` + +### Rover doesn't move + +1. Check that `/rover/cmd_vel` is bridged +2. Verify RoverController is publishing: `ros2 topic echo /rover/cmd_vel` + ### Model not found Set the model path: ```bash export GZ_SIM_RESOURCE_PATH=$PWD/gazebo/models:$GZ_SIM_RESOURCE_PATH +export IGN_GAZEBO_RESOURCE_PATH=$PWD/gazebo/models:$IGN_GAZEBO_RESOURCE_PATH ``` -### Drone falls immediately +### "Cannot connect to display" (WSL2) -Enable the velocity controller: +Use headless mode: ```bash -gz topic -t /drone/enable -m gz.msgs.Boolean -p 'data: true' +ign gazebo -s gazebo/worlds/drone_landing.sdf ``` -### "Cannot connect to display" - -Use headless mode or WSLg: +Or ensure WSLg is working: ```bash -# Headless -gz sim -s gazebo/worlds/drone_landing.sdf - -# Or ensure DISPLAY is set export DISPLAY=:0 ``` + +### Plugin not found + +For Ignition Fortress, plugins use `libignition-gazebo-*-system.so` naming. + +Check available plugins: +```bash +ls /usr/lib/x86_64-linux-gnu/ign-gazebo-6/plugins/ +``` + +## Launch File Options + +```bash +ros2 launch gazebo/launch/drone_landing.launch.py use_sim_time:=true +``` + +| Argument | Default | Description | +|----------|---------|-------------| +| `use_sim_time` | `true` | Use Gazebo clock | diff --git a/docs/pybullet.md b/docs/pybullet.md index ab685d8..e677010 100644 --- a/docs/pybullet.md +++ b/docs/pybullet.md @@ -1,13 +1,15 @@ -# PyBullet Simulation +# PyBullet Simulation Guide -Running the GPS-denied drone simulation with PyBullet. +Running the GPS-denied drone simulation with PyBullet physics engine. -## Standalone Mode (Recommended) +## Standalone Mode (Single Terminal - Any Platform) -No ROS 2 required! Single terminal: +No ROS 2 required! Works on Windows, macOS, and Linux: ```bash -source activate.sh +source activate.sh # Linux/macOS +. .\activate.ps1 # Windows + python standalone_simulation.py --pattern circular --speed 0.3 ``` @@ -24,9 +26,9 @@ Options: --- -## ROS 2 Mode (2 Terminals) +## ROS 2 Mode (Two Terminals) -For distributed or remote simulation: +For distributed or remote simulation with ROS 2: **Terminal 1 - Simulator:** ```bash @@ -40,6 +42,16 @@ source activate.sh python run_bridge.py --pattern circular --speed 0.3 ``` +### How It Works + +1. `simulation_host.py` runs PyBullet physics and listens on UDP port 5555 +2. `run_bridge.py` starts: + - `ROS2SimulatorBridge` - connects ROS topics to UDP + - `DroneController` - your landing algorithm + - `RoverController` - moves the landing pad + +The rover position is sent to the simulator, so both drone AND rover move! + ### Remote Setup Run simulator on one machine, controllers on another: @@ -56,25 +68,32 @@ python run_bridge.py --host 192.168.1.100 --pattern circular --- -## Building Standalone Executable +## Configuration -Create a distributable executable: +All parameters are configurable in `config.py`: -```bash -source activate.sh +```python +DRONE = { + "mass": 1.0, + "start_position": (0.0, 0.0, 5.0), + "thrust_scale": 15.0, + ... +} -# Build standalone simulation (recommended) -python build_exe.py +ROVER = { + "start_position": (0.0, 0.0, 0.15), + "default_pattern": "circular", + "default_speed": 0.5, + ... +} -# Build simulation_host -python build_exe.py simulation_host - -# Build all -python build_exe.py all +CONTROLLER = { + "Kp_z": 0.5, + "Kd_z": 0.3, + ... +} ``` -Executables are created in `dist/`. - --- ## Simulation Parameters @@ -83,18 +102,19 @@ Executables are created in `dist/`. |-----------|-------| | Physics Rate | 240 Hz | | Telemetry Rate | 24 Hz | -| Drone Mass | 1.0 kg | +| Drone Mass | 1.0 kg (configurable) | +| Rover Mass | Static (kinematic) | | UDP Port | 5555 (commands), 5556 (telemetry) | ## GPS-Denied Sensors | Sensor | Description | |--------|-------------| -| IMU | Orientation, angular velocity | -| Altimeter | Barometric altitude, vertical velocity | -| Velocity | Optical flow estimate | -| Camera | 320x240 downward JPEG image | -| Landing Pad | Vision-based relative position | +| **IMU** | Orientation (roll, pitch, yaw), angular velocity | +| **Altimeter** | Altitude above ground, vertical velocity | +| **Velocity** | Estimated horizontal velocity (x, y, z) | +| **Camera** | 320x240 downward-facing JPEG image | +| **Landing Pad** | Vision-based relative position when in camera FOV | ## Troubleshooting @@ -111,15 +131,33 @@ ssh -X user@host ### Drone flies erratically -Reduce control gains in `drone_controller.py`: +Reduce control gains in `config.py`: ```python -Kp = 0.3 -Kd = 0.2 +CONTROLLER = { + "Kp_z": 0.3, + "Kd_z": 0.2, + "Kp_xy": 0.2, + "Kd_xy": 0.1, +} ``` ### Camera image not appearing Install Pillow: ```bash -pip install pillow +pip install pillow numpy +``` + +### Rover not moving (ROS 2 mode) + +Ensure `run_bridge.py` is used (not `ros_bridge.py` directly). +The rover controller must be running to send position updates. + +### WSL2 GUI issues + +Set display scaling: +```bash +export GDK_DPI_SCALE=1.0 +export QT_SCALE_FACTOR=1.0 +python standalone_simulation.py ``` diff --git a/drone_controller.py b/drone_controller.py index c31001b..d5553d1 100644 --- a/drone_controller.py +++ b/drone_controller.py @@ -174,36 +174,25 @@ class DroneController(Node): landing_pad = telemetry.get('landing_pad', None) - # Camera image is available in telemetry['camera']['image'] - # Decode with: base64.b64decode(telemetry['camera']['image']) + # Descent control + if altitude > 1.0: + target_descent_rate = -0.5 + else: + target_descent_rate = -0.2 - # Default target - target_x = 0.0 - target_y = 0.0 + descent_error = target_descent_rate - vertical_vel + thrust = self._Kp_z * descent_error - # Use landing pad detection for positioning + # Horizontal control if landing_pad is not None: target_x = landing_pad.get('relative_x', 0.0) target_y = landing_pad.get('relative_y', 0.0) - - # TODO: Implement your GPS-denied landing algorithm - # You can use the camera image for custom vision processing - - # Simple PD controller for altitude - target_altitude = 0.0 - altitude_error = target_altitude - altitude - Kp_z, Kd_z = 0.5, 0.3 - thrust = Kp_z * altitude_error - Kd_z * vertical_vel - - # Horizontal control based on landing pad detection - Kp_xy, Kd_xy = 0.3, 0.2 - - if landing_pad is not None: - pitch = Kp_xy * target_x - Kd_xy * vel_x - roll = Kp_xy * target_y - Kd_xy * vel_y + + pitch = self._Kp_xy * target_x - self._Kd_xy * vel_x + roll = self._Kp_xy * target_y - self._Kd_xy * vel_y else: - pitch = -Kd_xy * vel_x - roll = -Kd_xy * vel_y + pitch = -self._Kd_xy * vel_x + roll = -self._Kd_xy * vel_y yaw = 0.0 diff --git a/gazebo_bridge.py b/gazebo_bridge.py index 08b1321..a466d70 100644 --- a/gazebo_bridge.py +++ b/gazebo_bridge.py @@ -1,9 +1,4 @@ #!/usr/bin/env python3 -""" -Gazebo Bridge - GPS-denied interface with camera and rover control. -Provides sensor data: IMU, altimeter, camera image. -Controls rover movement via Gazebo pose commands. -""" import base64 import json @@ -21,7 +16,6 @@ from std_msgs.msg import String class GazeboBridge(Node): - """Bridges Gazebo topics to GPS-denied sensor interface with camera.""" CAMERA_FOV = 60.0 CAMERA_RANGE = 10.0 @@ -97,11 +91,6 @@ class GazeboBridge(Node): pos.get('y', 0.0), pos.get('z', 0.15) ] - # Note: In Gazebo mode, rover position is tracked but the visual - # model stays static. Moving the model requires the set_pose service - # which has networking issues in WSL2. For moving rover tests, - # use PyBullet (standalone_simulation.py or simulation_host.py). - except json.JSONDecodeError: pass diff --git a/rover_controller.py b/rover_controller.py index cde0947..be1984a 100644 --- a/rover_controller.py +++ b/rover_controller.py @@ -200,15 +200,7 @@ class RoverController(Node): def parse_args(): parser = argparse.ArgumentParser( description='Rover Controller - Moving Landing Platform', - formatter_class=argparse.RawDescriptionHelpFormatter, - epilog=""" -Movement Patterns: - stationary - Rover stays at origin (easiest) - linear - Back and forth along X axis - circular - Circular path around origin - random - Random movement within bounds - square - Square pattern around origin - """ + formatter_class=argparse.RawDescriptionHelpFormatter ) parser.add_argument( '--pattern', '-p', type=str,