fix crach RL

Signed-off-by: stevedan <[email protected]>

nav2_system_tests/CMakeLists.txt

@@ -113,7 +113,7 @@ if(BUILD_TESTING)
   add_subdirectory(src/system_failure)
   add_subdirectory(src/updown)
   add_subdirectory(src/waypoint_follower)
-  # add_subdirectory(src/gps_navigation)
+  add_subdirectory(src/gps_navigation)
   add_subdirectory(src/behaviors/wait)
   add_subdirectory(src/behaviors/spin)
   add_subdirectory(src/behaviors/backup)

nav2_system_tests/src/gps_navigation/CMakeLists.txt

@@ -5,7 +5,5 @@ ament_add_test(test_gps_waypoint_follower
   TIMEOUT 180
   ENV
     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_gps.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
-    BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
 )
+

nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml

@@ -121,7 +121,7 @@ navsat_transform:
     magnetic_declination_radians: 0.0429351  # For lat/long 55.944831, -3.186998
     yaw_offset: 0.0
     zero_altitude: true
-    broadcast_utm_transform: true
+    broadcast_cartesian_transform: true
     publish_filtered_gps: true
     use_odometry_yaw: true
     wait_for_datum: false 

nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml

@@ -53,46 +53,93 @@ controller_server:
       yaw_goal_tolerance: 0.25
     # DWB parameters
     FollowPath:
-      plugin: "dwb_core::DWBLocalPlanner"
-      debug_trajectory_details: True
-      min_vel_x: 0.0
-      min_vel_y: 0.0
-      max_vel_x: 0.26
-      max_vel_y: 0.0
-      max_vel_theta: 1.0
-      min_speed_xy: 0.0
-      max_speed_xy: 0.26
-      min_speed_theta: 0.0
-      # Add high threshold velocity for turtlebot 3 issue.
-      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
-      acc_lim_x: 2.5
-      acc_lim_y: 0.0
-      acc_lim_theta: 3.2
-      decel_lim_x: -2.5
-      decel_lim_y: 0.0
-      decel_lim_theta: -3.2
-      vx_samples: 20
-      vy_samples: 5
-      vtheta_samples: 20
-      sim_time: 1.7
-      linear_granularity: 0.05
-      angular_granularity: 0.025
-      transform_tolerance: 0.2
-      xy_goal_tolerance: 0.25
-      trans_stopped_velocity: 0.25
-      short_circuit_trajectory_evaluation: True
-      stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
-      BaseObstacle.scale: 0.02
-      PathAlign.scale: 32.0
-      PathAlign.forward_point_distance: 0.1
-      GoalAlign.scale: 24.0
-      GoalAlign.forward_point_distance: 0.1
-      PathDist.scale: 32.0
-      GoalDist.scale: 24.0
-      RotateToGoal.scale: 32.0
-      RotateToGoal.slowing_factor: 5.0
-      RotateToGoal.lookahead_time: -1.0
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 56
+      model_dt: 0.05
+      batch_size: 2000
+      ax_max: 3.0
+      ax_min: -3.0
+      ay_max: 3.0
+      az_max: 3.5
+      vx_std: 0.2
+      vy_std: 0.2
+      wz_std: 0.4
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.5
+      wz_max: 1.9
+      iteration_count: 1
+      prune_distance: 1.7
+      transform_tolerance: 0.1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "DiffDrive"
+      visualize: true
+      regenerate_noises: true
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.2
+      critics: [
+        "ConstraintCritic", "CostCritic", "GoalCritic",
+        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
+        "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        mode: 0
+      # TwirlingCritic:
+      #   enabled: true
+      #   twirling_cost_power: 1
+      #   twirling_cost_weight: 10.0
 
 local_costmap:
   local_costmap:
@@ -151,6 +198,8 @@ global_costmap:
       rolling_window: True
       width: 50
       height: 50
+      # origin_x: 0.0
+      # origin_y: 0.0
       track_unknown_space: true
       # no static map
       plugins: ["obstacle_layer", "inflation_layer"]

nav2_system_tests/src/gps_navigation/test_case_py.launch.py

@@ -14,17 +14,20 @@
 # limitations under the License.
 
 import os
+from pathlib import Path
 import sys
 
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
 from launch import LaunchService
 from launch.actions import (
+    AppendEnvironmentVariable,
     ExecuteProcess,
     IncludeLaunchDescription,
     SetEnvironmentVariable,
 )
+from launch.launch_context import LaunchContext
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
@@ -33,106 +36,83 @@
 
 
 def generate_launch_description():
-    world = os.getenv('TEST_WORLD')
+    sim_dir = get_package_share_directory("nav2_minimal_tb3_sim")
+    nav2_bringup_dir = get_package_share_directory("nav2_bringup")
 
+    world_sdf_xacro = os.path.join(sim_dir, "worlds", "tb3_gps.sdf.xacro")
+    robot_sdf = os.path.join(sim_dir, "urdf", "gz_waffle_gps.sdf.xacro")
+
+    urdf = os.path.join(sim_dir, "urdf", "turtlebot3_waffle_gps.urdf")
+
+    with open(urdf, "r") as infp:
+        robot_description = infp.read()
+
+    # use local param file
     launch_dir = os.path.dirname(os.path.realpath(__file__))
-    params_file = os.path.join(launch_dir, 'nav2_no_map_params.yaml')
-    bringup_dir = get_package_share_directory('nav2_bringup')
+    params_file = os.path.join(launch_dir, "nav2_no_map_params.yaml")
 
     configured_params = RewrittenYaml(
-        source_file=params_file, root_key='', param_rewrites='', convert_types=True
+        source_file=params_file,
+        root_key="",
+        param_rewrites="",
+        convert_types=True,
     )
 
     return LaunchDescription(
         [
-            SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
-            SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
-            # Launch gazebo server for simulation
-            ExecuteProcess(
-                cmd=[
-                    'gzserver',
-                    '-s',
-                    'libgazebo_ros_init.so',
-                    '--minimal_comms',
-                    world,
-                ],
-                output='screen',
+            SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"),
+            SetEnvironmentVariable("RCUTILS_LOGGING_USE_STDOUT", "1"),
+            AppendEnvironmentVariable(
+                "GZ_SIM_RESOURCE_PATH", os.path.join(sim_dir, "models")
             ),
-            # TODO(orduno) Launch the robot state publisher instead
-            #              using a local copy of TB3 urdf file
-            Node(
-                package='tf2_ros',
-                executable='static_transform_publisher',
-                output='screen',
-                arguments=[
-                    '--x', '0',
-                    '--y', '0',
-                    '--z', '0',
-                    '--roll', '0',
-                    '--pitch', '0',
-                    '--yaw', '0',
-                    '--frame-id', 'base_footprint',
-                    '--child-frame-id', 'base_link'
-                ],
+            AppendEnvironmentVariable(
+                "GZ_SIM_RESOURCE_PATH",
+                str(Path(os.path.join(sim_dir)).parent.resolve()),
             ),
-            Node(
-                package='tf2_ros',
-                executable='static_transform_publisher',
-                output='screen',
-                arguments=[
-                    '--x', '0',
-                    '--y', '0',
-                    '--z', '0',
-                    '--roll', '0',
-                    '--pitch', '0',
-                    '--yaw', '0',
-                    '--frame-id', 'base_link',
-                    '--child-frame-id', 'base_scan'
-                ],
+            ExecuteProcess(
+                cmd=["gz", "sim", "-r", "-s", world_sdf_xacro],
+                output="screen",
             ),
-            Node(
-                package='tf2_ros',
-                executable='static_transform_publisher',
-                output='screen',
-                arguments=[
-                    '--x', '-0.32',
-                    '--y', '0',
-                    '--z', '0.068',
-                    '--roll', '0',
-                    '--pitch', '0',
-                    '--yaw', '0',
-                    '--frame-id', 'base_link',
-                    '--child-frame-id', 'imu_link'
-                ],
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(sim_dir, "launch", "spawn_tb3_gps.launch.py")
+                ),
+                launch_arguments={
+                    "use_sim_time": "True",
+                    "robot_sdf": robot_sdf,
+                    "x_pose": "0.0",
+                    "y_pose": "0.0",
+                    "z_pose": "0.0",
+                    "roll": "0.0",
+                    "pitch": "0.0",
+                    "yaw": "0.0",
+                }.items(),
             ),
             Node(
-                package='tf2_ros',
-                executable='static_transform_publisher',
-                output='screen',
-                arguments=[
-                    '--x', '0',
-                    '--y', '0',
-                    '--z', '0',
-                    '--roll', '0',
-                    '--pitch', '0',
-                    '--yaw', '0',
-                    '--frame-id', 'base_link',
-                    '--child-frame-id', 'gps_link'
+                package="robot_state_publisher",
+                executable="robot_state_publisher",
+                name="robot_state_publisher",
+                output="screen",
+                parameters=[
+                    {"use_sim_time": True, "robot_description": robot_description}
                 ],
             ),
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(
-                    os.path.join(bringup_dir, 'launch', 'navigation_launch.py')
+                    os.path.join(nav2_bringup_dir, "launch", "navigation_launch.py")
                 ),
                 launch_arguments={
-                    'use_sim_time': 'True',
-                    'params_file': configured_params,
-                    'autostart': 'True',
+                    "namespace": "",
+                    "use_namespace": "False",
+                    "use_sim_time": "True",
+                    "params_file": configured_params,
+                    "use_composition": "False",
+                    "autostart": "True",
                 }.items(),
             ),
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(
-                    os.path.join(launch_dir, 'dual_ekf_navsat.launch.py')
+                    os.path.join(launch_dir, "dual_ekf_navsat.launch.py")
                 ),
             ),
         ]
@@ -143,17 +123,18 @@ def main(argv=sys.argv[1:]):
     ld = generate_launch_description()
 
     test1_action = ExecuteProcess(
-        cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester.py')],
-        name='tester_node',
-        output='screen',
+        cmd=[os.path.join(os.getenv("TEST_DIR"), "tester.py")],
+        name="tester_node",
+        output="screen",
     )
 
     lts = LaunchTestService()
     lts.add_test_action(ld, test1_action)
     ls = LaunchService(argv=argv)
     ls.include_launch_description(ld)
-    return lts.run(ls)
+    return_code = lts.run(ls)
+    return return_code
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     sys.exit(main())

nav2_system_tests/src/gps_navigation/tester.py

@@ -180,9 +180,9 @@ def main(argv=sys.argv[1:]):
     time.sleep(2)
     test.cancel_goal()
 
-    # set waypoint outside of map
+    # set waypoint outside of map but not outide the utm zone
     time.sleep(2)
-    test.setWaypoints([[35.0, -118.0]])
+    test.setWaypoints([[55.929834, -3.184343]])
     result = test.run(True, False)
     assert not result
     result = not result
@@ -193,7 +193,7 @@ def main(argv=sys.argv[1:]):
 
     # stop on failure test with bogous waypoint
     test.setStopFailureParam(True)
-    bwps = [[55.944831, -3.186998], [35.0, -118.0], [55.944782, -3.187060]]
+    bwps = [[55.944831, -3.186998], [55.929834, -3.184343], [55.944782, -3.187060]]
     test.setWaypoints(bwps)
     result = test.run(True, False)
     assert not result