Updating to PAL's latest changes

Signed-off-by: Jose Miguel <josemiguel.guerrero@urjc.es>
jmguerreroh · Mar 20, 2024 · acd0f77 · acd0f77
1 parent 71054c6
commit acd0f77
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 41 deletions.
diff --git a/config/params.yaml b/config/params.yaml
@@ -38,7 +38,7 @@
 # aws_racetrack - x: 2.75 - y: -15.0  - z: 0.2 - roll: 0.0 - pitch: 0.0 - yaw:  0.0
 
 # Options:
-# Tiago arm: no-arm - right-arm
+# Tiago arm: no-arm - tiago-arm
 
 tiago_simulator:
   world: aws_house

diff --git a/launch/navigation.launch.py b/launch/navigation.launch.py
@@ -17,24 +17,16 @@
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch_pal.include_utils import include_launch_py_description
-from launch_ros.actions import SetRemap
-from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 
 def generate_launch_description():
     sim_dir = get_package_share_directory('tiago_simulator')
-
+    tiago_2dnav_dir = get_package_share_directory('tiago_2dnav')
+    nav2_bringup_dir = get_package_share_directory("nav2_bringup")
     config = os.path.join(sim_dir, 'config', 'params.yaml')
 
-    sim_time_arg = DeclareLaunchArgument(
-      'use_sim_time', default_value='True',
-      description='Use simulation (Gazebo) clock if true')
-
-    rviz_arg = DeclareLaunchArgument(
-      'rviz', default_value='True',
-      description='Run RViz2 if true')
-
     with open(config, "r") as stream:
         try:
             conf = (yaml.safe_load(stream))
@@ -43,22 +35,34 @@ def generate_launch_description():
             print(exc)
 
     world_name = conf['tiago_simulator']['world']
-    nav2 = include_launch_py_description(
-        'tiago_2dnav', ['launch', 'tiago_nav_bringup.launch.py'],
+
+    nav2_bringup_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_bringup_dir, "launch", "bringup_launch.py")
+        ),
         launch_arguments={
-          'map': os.path.join(sim_dir, 'maps', world_name + '.yaml'),
-        }.items()
+            "params_file": os.path.join(
+                tiago_2dnav_dir, "params", "tiago_nav_public_sim.yaml"
+            ),
+            "map": os.path.join(sim_dir, 'maps', world_name + '.yaml'),
+            "use_sim_time": "True",
+        }.items(),
     )
 
-    # Create the launch description and populate
-    ld = LaunchDescription()
+    rviz_bringup_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_bringup_dir, "launch", "rviz_launch.py")
+        ),
+        launch_arguments={
+            "rviz_config": os.path.join(
+                tiago_2dnav_dir, "config", "rviz", "navigation.rviz"
+            ),
+        }.items(),
+    )
 
-    # Remappings
-    scan_remap = SetRemap(src='scan', dst='scan_raw')
-    ld.add_action(scan_remap)
+    ld = LaunchDescription()
 
-    ld.add_action(rviz_arg)
-    ld.add_action(sim_time_arg)
-    ld.add_action(nav2)
+    ld.add_action(nav2_bringup_launch)
+    ld.add_action(rviz_bringup_launch)
 
     return ld
diff --git a/launch/simulation.launch.py b/launch/simulation.launch.py
@@ -78,13 +78,8 @@ def generate_launch_description():
     )
 
     moveit_arg = DeclareLaunchArgument(
-        'moveit', default_value='false',
-        description='Specify if launching MoveIt2'
-    )
-
-    model_name = DeclareLaunchArgument(
-        'model_name', default_value='tiago',
-        description='Gazebo model name'
+        'moveit', default_value='True',
+        description='Specify if launching MoveIt 2'
     )
 
     world_name = conf['tiago_simulator']['world']
@@ -141,10 +136,6 @@ def generate_launch_description():
                     'launch'), '/view_bookstore.launch.py']),
             )
 
-    # robot_spawn = include_launch_py_description(
-    #     'tiago_simulator', ['launch', 'dependencies',
-    #                           'tiago_spawn.launch.py'])
-
     tiago_state_publisher = include_launch_py_description(
         'tiago_description',
         ['launch', 'robot_state_publisher.launch.py'])
@@ -163,8 +154,7 @@ def generate_launch_description():
 
     robot_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                         arguments=['-topic', 'robot_description',
-                                   '-entity',
-                                   LaunchConfiguration('model_name'),
+                                   '-entity', 'tiago',
                                    ' '.join(['-x', str(conf['tiago_simulator']
                                                            ['robot_position']
                                                            ['x'])]),
@@ -195,7 +185,8 @@ def generate_launch_description():
     # @TODO: pal_pcl_points_throttle_and_filter
 
     packages = ['tiago_description', 'pmb2_description',
-                'hey5_description', 'pal_gripper_description']
+                'hey5_description', 'pal_gripper_description', 
+                'pal_robotiq_description' ]
     model_path = get_model_paths(packages)
     resource_path = get_resource_paths(packages)
 
@@ -214,12 +205,13 @@ def generate_launch_description():
     #                                       tiago_resource_path))
     ld.add_action(sim_time_arg)
     ld.add_action(world_name_arg)
-    ld.add_action(model_name)
-    ld.add_action(gazebo)
     ld.add_action(arm_arg)
+
+    ld.add_action(gazebo)
     ld.add_action(tiago_state_publisher)
     ld.add_action(robot_entity)
     ld.add_action(tiago_bringup)
+
     ld.add_action(moveit_arg)
     ld.add_action(move_group)
     ld.add_action(tuck_arm)