diff --git a/.circleci/config.yml b/.circleci/config.yml index 99a7a13929..9cf5261e8d 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v26\ + - "<< parameters.key >>-v28\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v26\ + - "<< parameters.key >>-v28\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v26\ + key: "<< parameters.key >>-v28\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ 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 96dc593d82..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"] @@ -290,3 +339,48 @@ collision_monitor: min_height: 0.15 max_height: 2.0 enabled: True + +docking_server: + ros__parameters: + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + docks: ['home_dock'] # Input your docks here + home_dock: + type: 'simple_charging_dock' + frame: map + pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 \ No newline at end of file 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..3dd082c191 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,6 +22,7 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ( + AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable, @@ -33,100 +35,77 @@ 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_empty_world.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') 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', + 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', + ExecuteProcess( + cmd=['gz', 'sim', '-r', '-s', world_sdf_xacro], output='screen', - arguments=[ - '--x', '0', - '--y', '0', - '--z', '0', - '--roll', '0', - '--pitch', '0', - '--yaw', '0', - '--frame-id', 'base_link', - '--child-frame-id', 'base_scan' - ], ), - 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', + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_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' + 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={ + 'namespace': '', + 'use_namespace': 'False', 'use_sim_time': 'True', 'params_file': configured_params, + 'use_composition': 'False', 'autostart': 'True', }.items(), ), @@ -152,7 +131,8 @@ def main(argv=sys.argv[1:]): 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__': 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 diff --git a/tools/underlay.repos b/tools/underlay.repos index f28dc3a0cf..09a6da9a92 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -34,4 +34,4 @@ repositories: ros-navigation/nav2_minimal_turtlebot_simulation: type: git url: https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git - version: 1.0.1 + version: f0eeedbc95d9f7cc8a513f7d46a84b3d08a3d395