diff --git a/config/nav2_params.yaml b/config/nav2_params.yaml index 2e1e520..f9564b4 100644 --- a/config/nav2_params.yaml +++ b/config/nav2_params.yaml @@ -108,91 +108,179 @@ bt_navigator_navigate_to_pose_rclcpp_node: ros__parameters: use_sim_time: False +# controller_server: +# ros__parameters: +# use_sim_time: False +# controller_frequency: 30.0 +# min_x_velocity_threshold: 0.001 +# min_y_velocity_threshold: 0.001 +# min_theta_velocity_threshold: 0.001 +# failure_tolerance: 0.3 +# progress_checker_plugin: "progress_checker" +# goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" +# controller_plugins: ["FollowPath"] + +# # Progress checker parameters +# progress_checker: +# plugin: "nav2_controller::SimpleProgressChecker" +# 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 +# # DWB parameters +# FollowPath: +# # plugin: "dwb_core::DWBLocalPlanner" + +# # trying roation_shim_controller +# #plugin: "nav2_rotation_shim_controller::RotationShimController" +# #plugin: "dwb_core::DWBLocalPlanner" +# #primary_controller: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" +# #primary_controller: "dwb_core::DWBLocalPlanner" +# # angular_dist_threshold: 0.785 +# # forward_sampling_distance: 0.5 +# # rotate_to_heading_angular_vel: 0.3 +# # max_angular_accel: 0.2 +# # simulate_ahead_time: 1.0 + +# # # primary controller params (dwb) +# # debug_trajectory_details: True +# # min_vel_x: 0.0 +# # min_vel_y: 0.0 +# # max_vel_x: 0.26 +# # max_vel_y: 0.26 +# # max_vel_theta: 1.0 +# # min_speed_xy: 0.001 +# # 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: 0.02 # was 2.5 +# # acc_lim_y: 0.02 +# # acc_lim_theta: 1.0 # was 3.2 +# # decel_lim_x: -0.02 # was -2.5 +# # decel_lim_y: -0.02 +# # decel_lim_theta: -0.1 # was -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 + +# # trying mppi_controller +# plugin: "nav2_mppi_controller::MPPIController" +# time_steps: 56 +# model_dt: 0.05 +# batch_size: 2000 +# 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: false +# reset_period: 1.0 # (only in Humble) +# TrajectoryVisualizer: +# trajectory_step: 5 +# time_step: 3 +# AckermannConstrains: +# min_turning_r: 0.2 +# critics: ["ConstraintCritic", "ObstaclesCritic", "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 +# ObstaclesCritic: +# enabled: true +# cost_power: 1 +# repulsion_weight: 1.5 +# critical_weight: 20.0 +# consider_footprint: false +# collision_cost: 10000.0 +# collision_margin_distance: 0.1 +# near_goal_distance: 0.5 +# inflation_radius: 0.55 # (only in Humble) +# cost_scaling_factor: 10.0 # (only in Humble) +# PathAlignCritic: +# enabled: true +# cost_power: 1 +# cost_weight: 14.0 +# max_path_occupancy_ratio: 0.05 +# trajectory_point_step: 3 +# 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 + +#try mppi controller controller_server: ros__parameters: - use_sim_time: False - controller_frequency: 50.0 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.001 - min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.3 - progress_checker_plugin: "progress_checker" - goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" - controller_plugins: ["FollowPath"] - - # Progress checker parameters - progress_checker: - plugin: "nav2_controller::SimpleProgressChecker" - 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 - # DWB parameters + controller_frequency: 30.0 FollowPath: - # plugin: "dwb_core::DWBLocalPlanner" - - # trying roation_shim_controller - #plugin: "nav2_rotation_shim_controller::RotationShimController" - #plugin: "dwb_core::DWBLocalPlanner" - #primary_controller: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - #primary_controller: "dwb_core::DWBLocalPlanner" - # angular_dist_threshold: 0.785 - # forward_sampling_distance: 0.5 - # rotate_to_heading_angular_vel: 0.3 - # max_angular_accel: 0.2 - # simulate_ahead_time: 1.0 - - # # primary controller params (dwb) - # debug_trajectory_details: True - # min_vel_x: 0.0 - # min_vel_y: 0.0 - # max_vel_x: 0.26 - # max_vel_y: 0.26 - # max_vel_theta: 1.0 - # min_speed_xy: 0.001 - # 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: 0.02 # was 2.5 - # acc_lim_y: 0.02 - # acc_lim_theta: 1.0 # was 3.2 - # decel_lim_x: -0.02 # was -2.5 - # decel_lim_y: -0.02 - # decel_lim_theta: -0.1 # was -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 - - # trying mppi_controller plugin: "nav2_mppi_controller::MPPIController" time_steps: 56 model_dt: 0.05 @@ -275,7 +363,7 @@ controller_server: # enabled: true # twirling_cost_power: 1 # twirling_cost_weight: 10.0 - + local_costmap: local_costmap: ros__parameters: