Update row_coverage_demo_launch.py to Gazebo Ignition

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)