Gazebo World Model Update
This commit is contained in:
@@ -1,110 +1,62 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Gazebo Launch File - Drone Landing Simulation
|
||||
Launches Gazebo with the world and spawns the drone.
|
||||
Supports both 'gz' (newer) and 'ign' (Fortress) commands.
|
||||
Works with Ignition Fortress (ROS 2 Humble)
|
||||
"""
|
||||
|
||||
import os
|
||||
import shutil
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
|
||||
from launch.conditions import IfCondition
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.actions import DeclareLaunchArgument, ExecuteProcess
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def get_gazebo_command():
|
||||
"""Determine whether to use 'gz' or 'ign' command."""
|
||||
if shutil.which('gz'):
|
||||
return 'gz'
|
||||
elif shutil.which('ign'):
|
||||
return 'ign'
|
||||
else:
|
||||
# Default to gz, will fail with helpful error
|
||||
return 'gz'
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""Generate the launch description."""
|
||||
|
||||
# Get paths
|
||||
script_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
gazebo_dir = os.path.dirname(script_dir)
|
||||
|
||||
world_file = os.path.join(gazebo_dir, 'worlds', 'drone_landing.sdf')
|
||||
model_path = os.path.join(gazebo_dir, 'models')
|
||||
drone_model = os.path.join(model_path, 'drone', 'model.sdf')
|
||||
|
||||
gz_resource_path = os.environ.get('GZ_SIM_RESOURCE_PATH', '')
|
||||
ign_resource_path = os.environ.get('IGN_GAZEBO_RESOURCE_PATH', '')
|
||||
combined_path = f"{model_path}:{gz_resource_path}:{ign_resource_path}"
|
||||
|
||||
os.environ['GZ_SIM_RESOURCE_PATH'] = combined_path
|
||||
os.environ['IGN_GAZEBO_RESOURCE_PATH'] = combined_path
|
||||
# Determine which gazebo command is available
|
||||
if shutil.which('ign'):
|
||||
gz_cmd = ['ign', 'gazebo', '-r', world_file]
|
||||
elif shutil.which('gz'):
|
||||
gz_cmd = ['gz', 'sim', '-r', world_file]
|
||||
else:
|
||||
gz_cmd = ['ign', 'gazebo', '-r', world_file]
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
|
||||
headless = LaunchConfiguration('headless', default='false')
|
||||
|
||||
# Detect which Gazebo command is available
|
||||
gz_cmd = get_gazebo_command()
|
||||
|
||||
# For ign command, use 'gazebo' subcommand; for gz, use 'sim'
|
||||
if gz_cmd == 'ign':
|
||||
sim_cmd = [gz_cmd, 'gazebo', '-r', world_file]
|
||||
spawn_cmd = [
|
||||
gz_cmd, 'service', '-s', '/world/drone_landing_world/create',
|
||||
'--reqtype', 'ignition.msgs.EntityFactory',
|
||||
'--reptype', 'ignition.msgs.Boolean',
|
||||
'--timeout', '5000',
|
||||
'--req', f'sdf_filename: "{drone_model}", name: "drone"'
|
||||
]
|
||||
else:
|
||||
sim_cmd = [gz_cmd, 'sim', '-r', world_file]
|
||||
spawn_cmd = [
|
||||
gz_cmd, 'service', '-s', '/world/drone_landing_world/create',
|
||||
'--reqtype', 'gz.msgs.EntityFactory',
|
||||
'--reptype', 'gz.msgs.Boolean',
|
||||
'--timeout', '5000',
|
||||
'--req', f'sdf_filename: "{drone_model}", name: "drone"'
|
||||
]
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='true',
|
||||
description='Use simulation (Gazebo) clock'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'headless',
|
||||
default_value='false',
|
||||
description='Run Gazebo in headless mode (no GUI)'
|
||||
description='Use simulation clock'
|
||||
),
|
||||
|
||||
# Start Gazebo
|
||||
ExecuteProcess(
|
||||
cmd=sim_cmd,
|
||||
output='screen',
|
||||
additional_env={
|
||||
'GZ_SIM_RESOURCE_PATH': combined_path,
|
||||
'IGN_GAZEBO_RESOURCE_PATH': combined_path
|
||||
}
|
||||
),
|
||||
|
||||
ExecuteProcess(
|
||||
cmd=spawn_cmd,
|
||||
cmd=gz_cmd,
|
||||
output='screen'
|
||||
),
|
||||
|
||||
# ROS-Gazebo Bridge
|
||||
Node(
|
||||
package='ros_gz_bridge',
|
||||
executable='parameter_bridge',
|
||||
name='gz_bridge',
|
||||
arguments=[
|
||||
'/drone/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist',
|
||||
'/model/drone/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry',
|
||||
'/drone/imu@sensor_msgs/msg/Imu@gz.msgs.IMU',
|
||||
'/clock@rosgraph_msgs/msg/Clock@gz.msgs.Clock',
|
||||
# Velocity commands (bidirectional)
|
||||
'/drone/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)
|
||||
'/drone/imu@sensor_msgs/msg/Imu[ignition.msgs.IMU',
|
||||
# Clock
|
||||
'/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
|
||||
],
|
||||
parameters=[{'use_sim_time': use_sim_time}],
|
||||
output='screen'
|
||||
@@ -119,12 +71,9 @@ if __name__ == '__main__':
|
||||
print(" ros2 launch gazebo/launch/drone_landing.launch.py")
|
||||
print()
|
||||
print("Or start Gazebo manually:")
|
||||
|
||||
gz_cmd = get_gazebo_command()
|
||||
if gz_cmd == 'ign':
|
||||
if shutil.which('ign'):
|
||||
print(" ign gazebo gazebo/worlds/drone_landing.sdf")
|
||||
else:
|
||||
print(" gz sim gazebo/worlds/drone_landing.sdf")
|
||||
|
||||
print()
|
||||
print("Then run: python run_gazebo.py")
|
||||
|
||||
Reference in New Issue
Block a user