From ad7110ec91abd4b204339fa429834d1267e01b20 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 25 Nov 2024 16:25:54 +0100 Subject: [PATCH] Update row_coverage_demo_launch.py to Gazebo Ignition --- .../launch/row_coverage_demo_launch.py | 69 ++++++++++++------- 1 file changed, 43 insertions(+), 26 deletions(-) diff --git a/opennav_coverage_demo/launch/row_coverage_demo_launch.py b/opennav_coverage_demo/launch/row_coverage_demo_launch.py index 342fd84..1fb6f05 100644 --- a/opennav_coverage_demo/launch/row_coverage_demo_launch.py +++ b/opennav_coverage_demo/launch/row_coverage_demo_launch.py @@ -13,11 +13,12 @@ # limitations under the License. import os +import tempfile from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler +from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -26,22 +27,21 @@ def generate_launch_description(): nav2_bringup_dir = get_package_share_directory('nav2_bringup') coverage_demo_dir = get_package_share_directory('opennav_coverage_demo') rviz_config_file = os.path.join(coverage_demo_dir, 'rviz_config.rviz') + sim_dir = get_package_share_directory("nav2_minimal_tb3_sim") world = os.path.join(coverage_demo_dir, 'blank.world') param_file_path = os.path.join(coverage_demo_dir, 'demo_params.yaml') - sdf = os.path.join(nav2_bringup_dir, 'worlds', 'waffle.model') + robot_sdf = os.path.join(sim_dir, "urdf", "gz_waffle.sdf.xacro") # start the simulation - start_gazebo_server_cmd = ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', - '-s', 'libgazebo_ros_factory.so', world], - cwd=[coverage_demo_dir], output='screen') - - # start_gazebo_client_cmd = ExecuteProcess( - # cmd=['gzclient'], - # cwd=[coverage_demo_dir], output='screen') - - urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + world_sdf = tempfile.mktemp(prefix="nav2_", suffix=".sdf") + world_sdf_xacro = ExecuteProcess(cmd=["xacro", "-o", world_sdf, "headless:=false", world]) + gazebo_server = ExecuteProcess( + cmd=["gz", "sim", "-r", "-s", world_sdf], + output="screen", + ) + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') with open(urdf, 'r') as infp: robot_description = infp.read() @@ -53,15 +53,29 @@ def generate_launch_description(): parameters=[{'use_sim_time': True, 'robot_description': robot_description}]) - start_gazebo_spawner_cmd = Node( - package='gazebo_ros', - executable='spawn_entity.py', - output='screen', - arguments=[ - '-entity', 'tb3', - '-file', sdf, - '-x', '6.23', '-y', '15.0', '-z', '0.10', - '-R', '0.0', '-P', '0.0', '-Y', '-1.5708']) + remove_temp_sdf_file = RegisterEventHandler( + event_handler=OnShutdown(on_shutdown=[OpaqueFunction(function=lambda _: os.remove(world_sdf))]) + ) + + gazebo_client = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory("ros_gz_sim"), "launch", "gz_sim.launch.py") + ), + launch_arguments={"gz_args": ["-v4 -g "]}.items(), + ) + + start_gazebo_spawner_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')), + launch_arguments={'namespace': '', + 'robot_name': 'turtlebot3_waffle', + 'robot_sdf': robot_sdf, + 'x_pose': str(6.23), + 'y_pose': str(15.0), + 'z_pose': str(0.1), + 'roll': str(0.0), + 'pitch': str(0.0), + 'yaw': str(-1.5708)}.items()) # start the visualization rviz_cmd = IncludeLaunchDescription( @@ -80,7 +94,7 @@ def generate_launch_description(): package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']) + arguments=['6.23', '15', '0', '0', '0', '0', 'map', 'odom']) fake_gps_cmd = Node( package='tf2_ros', executable='static_transform_publisher', @@ -95,10 +109,13 @@ def generate_launch_description(): output='screen') ld = LaunchDescription() - ld.add_action(start_gazebo_server_cmd) - # ld.add_action(start_gazebo_client_cmd) - ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(world_sdf_xacro) + ld.add_action(remove_temp_sdf_file) + ld.add_action(gazebo_server) + ld.add_action(gazebo_client) ld.add_action(start_gazebo_spawner_cmd) + + ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) ld.add_action(fake_localization_cmd)