diff --git a/config/kobuki_sim_nav_params.yaml b/config/kobuki_sim_nav_params.yaml index 945b211..d8d7286 100644 --- a/config/kobuki_sim_nav_params.yaml +++ b/config/kobuki_sim_nav_params.yaml @@ -1,17 +1,16 @@ amcl: ros__parameters: - use_sim_time: True + use_sim_time: true alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 alpha4: 0.2 - alpha5: 0.2 - base_frame_id: "base_footprint" + base_frame_id: base_footprint beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 do_beamskip: false - global_frame_id: "map" + global_frame_id: map lambda_short: 0.1 laser_likelihood_max_dist: 2.0 laser_max_range: 100.0 @@ -20,7 +19,7 @@ amcl: max_beams: 60 max_particles: 2000 min_particles: 500 - odom_frame_id: "odom" + odom_frame_id: odom pf_err: 0.05 pf_z: 0.99 recovery_alpha_fast: 0.0 @@ -37,99 +36,122 @@ amcl: z_max: 0.05 z_rand: 0.5 z_short: 0.05 - scan_topic: /scan - set_initial_pose: true + scan_topic: /scan_raw + set_initial_pose: false initial_pose: x: 0.0 y: 0.0 z: 0.0 yaw: 0.0 - -amcl_map_client: - ros__parameters: - use_sim_time: False - -amcl_rclcpp_node: +behavior_server: ros__parameters: - use_sim_time: False + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::BackUp" + drive_on_heading: + plugin: "nav2_behaviors::DriveOnHeading" + wait: + plugin: "nav2_behaviors::Wait" + assisted_teleop: + plugin: "nav2_behaviors::AssistedTeleop" + local_frame: odom + global_frame: odom + robot_base_frame: base_footprint + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + enable_stamped_cmd_vel: false bt_navigator: ros__parameters: - use_sim_time: True global_frame: map - robot_base_frame: base_link + robot_base_frame: base_footprint odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + + error_code_names: + - compute_path_error_code + - follow_path_error_code bt_navigator_navigate_through_poses_rclcpp_node: ros__parameters: - use_sim_time: True + use_sim_time: true bt_navigator_navigate_to_pose_rclcpp_node: ros__parameters: - use_sim_time: True + use_sim_time: true + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/base_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "/scan_raw" + min_height: 0.15 + max_height: 2.0 + enabled: True controller_server: ros__parameters: - use_sim_time: True + use_sim_time: true controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 + enable_stamped_cmd_vel: false progress_checker_plugin: "progress_checker" - goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + goal_checker_plugins: ["general_goal_checker"] controller_plugins: ["FollowPath"] + odom_topic: /odom # Progress checker parameters progress_checker: @@ -137,30 +159,23 @@ controller_server: required_movement_radius: 0.5 movement_time_allowance: 10.0 # Goal checker parameters - #precise_goal_checker: - # plugin: "nav2_controller::SimpleGoalChecker" - # xy_goal_tolerance: 0.25 - # yaw_goal_tolerance: 0.25 - # stateful: True general_goal_checker: - stateful: True plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 + stateful: True # 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.30 + max_vel_x: 1.00 max_vel_y: 0.0 - max_vel_theta: 1.0 + max_vel_theta: 5.00 min_speed_xy: 0.0 - max_speed_xy: 0.30 + max_speed_xy: 1.00 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 @@ -190,25 +205,101 @@ controller_server: RotateToGoal.slowing_factor: 5.0 RotateToGoal.lookahead_time: -1.0 +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_footprint" + 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 + # The following example illustrates configuring 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 + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_footprint + use_sim_time: true + robot_radius: 0.18 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan_raw + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + local_costmap: local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 global_frame: odom - robot_base_frame: base_link - use_sim_time: True + robot_base_frame: base_footprint + use_sim_time: true rolling_window: true width: 3 height: 3 resolution: 0.05 - robot_radius: 0.18 - plugins: ["voxel_layer", "keepout_filter", "inflation_layer"] - # filters: ["keepout_filter"] - keepout_filter: - plugin: "nav2_costmap_2d::KeepoutFilter" - enabled: True - filter_info_topic: "/costmap_filter_info" + robot_radius: 0.275 + plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 @@ -224,43 +315,7 @@ local_costmap: mark_threshold: 0 observation_sources: scan scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - always_send_full_costmap: True - -global_costmap: - global_costmap: - ros__parameters: - update_frequency: 1.0 - publish_frequency: 1.0 - global_frame: map - robot_base_frame: base_link - use_sim_time: True - robot_radius: 0.18 - resolution: 0.05 - track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "keepout_filter", "inflation_layer"] - # filters: ["keepout_filter"] - keepout_filter: - plugin: "nav2_costmap_2d::KeepoutFilter" - enabled: True - filter_info_topic: "/costmap_filter_info" - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan + topic: /scan_raw max_obstacle_height: 2.0 clearing: True marking: True @@ -272,22 +327,19 @@ global_costmap: static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 always_send_full_costmap: True -map_server: +loopback_simulator: ros__parameters: - use_sim_time: True - # Overridden in launch by the "map" launch configuration or provided default value. - # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. - yaml_filename: "" + base_frame_id: "base_footprint" + odom_frame_id: "odom" + map_frame_id: "map" + scan_frame_id: "laser_link" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 map_saver: ros__parameters: - use_sim_time: True + use_sim_time: true save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 @@ -296,73 +348,103 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - use_sim_time: True + use_sim_time: true planner_plugins: ["GridBased"] GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" + plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true -smoother_server: +slam_toolbox: ros__parameters: - use_sim_time: True - smoother_plugins: ["simple_smoother"] - simple_smoother: - plugin: "nav2_smoother::SimpleSmoother" - tolerance: 1.0e-10 - max_its: 1000 - do_refinement: True + use_sim_time: true + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None -behavior_server: + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan_raw + mode: mapping + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + # map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30.0 + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + +smoother_server: ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint - cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] - spin: - plugin: "nav2_behaviors/Spin" - backup: - plugin: "nav2_behaviors/BackUp" - drive_on_heading: - plugin: "nav2_behaviors/DriveOnHeading" - wait: - plugin: "nav2_behaviors/Wait" - global_frame: odom - robot_base_frame: base_link - transform_tolerance: 0.1 use_sim_time: true - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 -robot_state_publisher: +velocity_smoother: ros__parameters: - use_sim_time: True + use_sim_time: true + enable_stamped_cmd_vel: false waypoint_follower: ros__parameters: - use_sim_time: True - loop_rate: 20 + use_sim_time: true stop_on_failure: false waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True - waypoint_pause_duration: 200 - -velocity_smoother: - ros__parameters: - use_sim_time: True - smoothing_frequency: 20.0 - scale_velocities: False - feedback: "OPEN_LOOP" - max_velocity: [0.26, 0.0, 1.0] - min_velocity: [-0.26, 0.0, -1.0] - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" - odom_duration: 0.1 - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 \ No newline at end of file + waypoint_pause_duration: 0 \ No newline at end of file diff --git a/launch/navigation_sim.launch.py b/launch/navigation_sim.launch.py index 2698394..ba8b70b 100644 --- a/launch/navigation_sim.launch.py +++ b/launch/navigation_sim.launch.py @@ -59,9 +59,9 @@ def generate_launch_description(): ) # Actions - localization_cmd = IncludeLaunchDescription( + navigation_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(nav2_dir, 'launch', 'localization_launch.py') + os.path.join(nav2_dir, 'launch', 'bringup_launch.py') ), launch_arguments={ 'use_sim_time': use_sim_time, @@ -71,16 +71,6 @@ def generate_launch_description(): }.items() ) - navigation_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(nav2_dir, 'launch', 'navigation_launch.py') - ), - launch_arguments={ - 'use_sim_time': use_sim_time, - 'params_file': params_file - }.items() - ) - rviz_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(nav2_dir, 'launch', 'rviz_launch.py') @@ -100,7 +90,6 @@ def generate_launch_description(): ld.add_action(declare_nav_params_cmd) ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_map_cmd) - ld.add_action(localization_cmd) ld.add_action(navigation_cmd) ld.add_action(rviz_cmd) ld.add_action(cmd_vel_remap)