SirBlob/RDC_Simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ROS Bridge Update

Browse Source
This commit is contained in:
default
2026-01-02 06:50:31 +00:00
parent 1bbd9c8377
commit 924d803e71
4 changed files with 60 additions and 137 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
gazebo/worlds/drone_landing.sdf
View File

@@ -1,47 +1,29 @@
<?xml version="1.0" ?>
<!--
Drone Landing Simulation - Gazebo World
This world contains:
- Ground plane with grid texture
- Landing pad (rover) at origin - can be moved via pose commands
- Drone spawned at 5m altitude
- Sun for lighting
Drone Landing Simulation - Gazebo World (Ignition Fortress compatible)
-->
<sdf version="1.8">
<world name="drone_landing_world">
<!-- Physics configuration -->
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<!-- Required plugins -->
<plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics"/>
<plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands"/>
<plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster"/>
<plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<plugin filename="ignition-gazebo-imu-system" name="ignition::gazebo::systems::Imu"/>
<!-- Ignition Fortress plugins -->
<plugin filename="libignition-gazebo-physics-system.so" name="ignition::gazebo::systems::Physics"/>
<plugin filename="libignition-gazebo-user-commands-system.so" name="ignition::gazebo::systems::UserCommands"/>
<plugin filename="libignition-gazebo-scene-broadcaster-system.so" name="ignition::gazebo::systems::SceneBroadcaster"/>
<!-- Lighting -->
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<!-- Ground Plane -->
<!-- Ground -->
<model name="ground_plane">
<static>true</static>
<link name="link">
@@ -61,43 +43,18 @@
</plane>
</geometry>
<material>
<ambient>0.3 0.3 0.3 1</ambient>
<ambient>0.5 0.5 0.5 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
</material>
</visual>
</link>
</model>
<!-- Landing Pad (Rover) - Non-static so it can be moved -->
<!-- Landing Pad (static for now) -->
<model name="landing_pad">
<static>false</static>
<static>true</static>
<pose>0 0 0.15 0 0 0</pose>
<!-- Pose publisher to track position -->
<plugin filename="ignition-gazebo-pose-publisher-system" name="ignition::gazebo::systems::PosePublisher">
<publish_link_pose>false</publish_link_pose>
<publish_model_pose>true</publish_model_pose>
<publish_nested_model_pose>false</publish_nested_model_pose>
<update_frequency>50</update_frequency>
</plugin>
<link name="base_link">
<inertial>
<mass>100.0</mass>
<inertia>
<ixx>10.0</ixx>
<iyy>10.0</iyy>
<izz>10.0</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>1.0 1.0 0.3</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
@@ -105,49 +62,17 @@
</box>
</geometry>
<material>
<ambient>0.1 0.4 0.1 1</ambient>
<diffuse>0.2 0.6 0.2 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<ambient>0.1 0.5 0.1 1</ambient>
<diffuse>0.2 0.7 0.2 1</diffuse>
</material>
</visual>
<!-- Landing pad "H" marker -->
<visual name="h_marker">
<pose>0 0 0.151 0 0 0</pose>
<collision name="collision">
<geometry>
<box>
<size>0.6 0.1 0.001</size>
<size>1.0 1.0 0.3</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
</material>
</visual>
<visual name="h_left">
<pose>-0.25 0 0.151 0 0 0</pose>
<geometry>
<box>
<size>0.1 0.6 0.001</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
</material>
</visual>
<visual name="h_right">
<pose>0.25 0 0.151 0 0 0</pose>
<geometry>
<box>
<size>0.1 0.6 0.001</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
</material>
</visual>
</collision>
</link>
</model>

53
gazebo_bridge.py
View File

@@ -8,8 +8,6 @@ Controls rover movement via Gazebo pose commands.
import base64
import json
import math
import subprocess
import threading
from typing import Optional, Dict, Any
import numpy as np
@@ -44,11 +42,6 @@ class GazeboBridge(Node):
self._drone_pos = [0.0, 0.0, 5.0]
self._drone_orn = [0.0, 0.0, 0.0, 1.0]
# Detect gz vs ign command
import shutil
self._gz_cmd = 'gz' if shutil.which('gz') else 'ign'
self.get_logger().info(f' Using Gazebo command: {self._gz_cmd}')
sensor_qos = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
depth=10
@@ -95,57 +88,23 @@ class GazeboBridge(Node):
self._gz_cmd_vel_pub.publish(gz_msg)
def _rover_callback(self, msg: String) -> None:
"""Receive rover position and update in Gazebo."""
"""Receive rover position (for telemetry only - rover is static in Gazebo)."""
try:
data = json.loads(msg.data)
pos = data.get('position', {})
new_pos = [
self._rover_pos = [
pos.get('x', 0.0),
pos.get('y', 0.0),
pos.get('z', 0.15)
]
# Only update if position changed significantly
if (abs(new_pos[0] - self._rover_pos[0]) > 0.01 or
abs(new_pos[1] - self._rover_pos[1]) > 0.01):
self._rover_pos = new_pos
# Move rover in Gazebo (async to avoid blocking)
threading.Thread(target=self._move_rover_in_gazebo, daemon=True).start()
# 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
def _move_rover_in_gazebo(self) -> None:
"""Send pose command to Gazebo to move the landing pad."""
try:
x, y, z = self._rover_pos
# Use Gazebo service to set model pose
if self._gz_cmd == 'ign':
# Ignition Gazebo (Fortress)
cmd = [
'ign', 'service', '-s', '/world/drone_landing_world/set_pose',
'--reqtype', 'ignition.msgs.Pose',
'--reptype', 'ignition.msgs.Boolean',
'--timeout', '100',
'--req', f'name: "landing_pad", position: {{x: {x}, y: {y}, z: {z}}}'
]
else:
# Newer Gazebo
cmd = [
'gz', 'service', '-s', '/world/drone_landing_world/set_pose',
'--reqtype', 'gz.msgs.Pose',
'--reptype', 'gz.msgs.Boolean',
'--timeout', '100',
'--req', f'name: "landing_pad", position: {{x: {x}, y: {y}, z: {z}}}'
]
subprocess.run(cmd, capture_output=True, timeout=1.0)
except subprocess.TimeoutExpired:
pass
except Exception as e:
pass # Silently ignore errors to avoid spam
def _camera_callback(self, msg: Image) -> None:
"""Process camera images and encode as base64."""

25
ros_bridge.py
View File

@@ -34,6 +34,9 @@ class ROS2SimulatorBridge(Node):
self._simulator_port = simulator_port or self.DEFAULT_SIMULATOR_PORT
self._telemetry_port = self._simulator_port + 1
# Rover position (updated from /rover/telemetry)
self._rover_pos = {'x': 0.0, 'y': 0.0, 'z': 0.15}
self.get_logger().info('=' * 60)
self.get_logger().info('ROS 2 Simulator Bridge Starting...')
self.get_logger().info(f' Simulator Host: {self._simulator_host}')
@@ -50,6 +53,12 @@ class ROS2SimulatorBridge(Node):
)
self.get_logger().info(f' Subscribed to: {self.CMD_VEL_TOPIC}')
# Subscribe to rover telemetry
self._rover_sub = self.create_subscription(
String, '/rover/telemetry', self._rover_callback, 10
)
self.get_logger().info(' Subscribed to: /rover/telemetry')
self._telemetry_pub = self.create_publisher(
String, self.TELEMETRY_TOPIC, 10
)
@@ -83,10 +92,24 @@ class ROS2SimulatorBridge(Node):
'thrust': msg.linear.z,
'yaw': msg.angular.z,
'pitch': msg.linear.x,
'roll': msg.linear.y
'roll': msg.linear.y,
'rover': self._rover_pos # Include current rover position
}
self._send_command_to_simulator(command)
def _rover_callback(self, msg: String) -> None:
"""Update rover position from RoverController telemetry."""
try:
data = json.loads(msg.data)
pos = data.get('position', {})
self._rover_pos = {
'x': pos.get('x', 0.0),
'y': pos.get('y', 0.0),
'z': pos.get('z', 0.15)
}
except json.JSONDecodeError:
pass
def _send_command_to_simulator(self, command: dict) -> bool:
try:
json_data = json.dumps(command).encode('utf-8')

16
simulation_host.py
View File

@@ -208,12 +208,28 @@ class DroneSimulator:
try:
command = json.loads(data.decode('utf-8'))
# Drone control commands
self._last_command = {
'thrust': float(command.get('thrust', 0.0)),
'pitch': float(command.get('pitch', 0.0)),
'roll': float(command.get('roll', 0.0)),
'yaw': float(command.get('yaw', 0.0))
}
# Rover position update (if provided)
if 'rover' in command:
rover_data = command['rover']
new_x = float(rover_data.get('x', self._rover_pos[0]))
new_y = float(rover_data.get('y', self._rover_pos[1]))
new_z = float(rover_data.get('z', self._rover_pos[2]))
self._rover_pos = [new_x, new_y, new_z]
p.resetBasePositionAndOrientation(
self._rover_id,
self._rover_pos,
[0, 0, 0, 1]
)
except (json.JSONDecodeError, ValueError) as e:
print(f"Invalid command received: {e}")