diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 851a1e2db4..e3152c0bde 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/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) diff --git a/nav2_system_tests/src/gps_navigation/CMakeLists.txt b/nav2_system_tests/src/gps_navigation/CMakeLists.txt index 3342b6cd58..742d004f11 100644 --- a/nav2_system_tests/src/gps_navigation/CMakeLists.txt +++ b/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 ) + diff --git a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml index 6783a7b1db..e46ce5d1a8 100644 --- a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml +++ b/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 diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index ed36779a21..0911902aee 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/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"] diff --git a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py index 69f19dd6ac..b198d90361 100755 --- a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py +++ b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py @@ -14,6 +14,7 @@ # limitations under the License. import os +from pathlib import Path import sys from ament_index_python.packages import get_package_share_directory @@ -21,10 +22,12 @@ 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()) diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index b9fb4725ef..56064aa9ac 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/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