From 1bb714ae76ee86916eaba87dbf64d6f3e5a3ed53 Mon Sep 17 00:00:00 2001 From: Pradheep Date: Wed, 17 Jul 2024 13:00:43 +0200 Subject: [PATCH] controller params update --- configs/mp_400/navigation.yaml | 82 ++++++++++++++++++++++++--------- configs/mp_500/navigation.yaml | 74 ++++++++++++++++------------- configs/mpo_500/navigation.yaml | 2 +- configs/mpo_700/navigation.yaml | 4 +- 4 files changed, 106 insertions(+), 56 deletions(-) diff --git a/configs/mp_400/navigation.yaml b/configs/mp_400/navigation.yaml index 25292e5..db5dc02 100644 --- a/configs/mp_400/navigation.yaml +++ b/configs/mp_400/navigation.yaml @@ -198,28 +198,66 @@ controller_server: stateful: True # RegulatedPurePursuitController Parameters FollowPath: - plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - desired_linear_vel: 0.5 - max_linear_accel: 0.2 - max_linear_decel: 0.2 - lookahead_dist: 0.6 - min_lookahead_dist: 0.3 #0.3 - max_lookahead_dist: 0.9 #0.9 - lookahead_time: 1.5 #1.5 - rotate_to_heading_angular_vel: 0.6 - transform_tolerance: 0.1 - use_velocity_scaled_lookahead_dist: false - min_approach_linear_velocity: 0.01 - use_approach_linear_velocity_scaling: true - max_allowed_time_to_collision: 1.0 - use_regulated_linear_velocity_scaling: true - use_cost_regulated_linear_velocity_scaling: false - regulated_linear_scaling_min_radius: 0.9 - regulated_linear_scaling_min_speed: 0.25 - use_rotate_to_heading: true - rotate_to_heading_min_angle: 0.785 - max_angular_accel: 0.6 - allow_reversing: false + plugin: "neo_local_planner2::NeoLocalPlanner" + # The x acceleration limit of the robot in meters/sec^2 + # The x acceleration limit of the robot in meters/sec^2 + acc_lim_x : 0.25 + # The rotational acceleration limit of the robot in radians/sec^2 + acc_lim_theta : 0.8 + # The maximum x velocity for the robot in m/s. + max_vel_x : 0.8 + # The minimum x velocity for the robot in m/s, negative for backwards motion. + min_vel_x : -0.1 + # The absolute value of the maximum rotational velocity for the robot in rad/s + max_rot_vel : 0.8 + # The absolute value of the minimum rotational velocity for the robot in rad/s + min_rot_vel : 0.1 + # The absolute value of the maximum translational velocity for the robot in m/s + max_trans_vel : 0.8 + # The absolute value of the minimum translational velocity for the robot in m/s + min_trans_vel : 0.1 + # The tolerance in radians for the controller in yaw/rotation when achieving its goal + yaw_goal_tolerance : 0.05 + # The tolerance in meters for the controller in the x & y distance when achieving a goal + xy_goal_tolerance : 0.1 + # How long to fine tune for goal position after reaching tolerance limits [s] + goal_tune_time : 3.0 + # How far to predict control pose into the future based on latest odometry [s] + lookahead_time : 0.3 + # How far to look ahead when computing path orientation [m] + lookahead_dist : 0.5 + # Threshold yaw error below which we consider to start moving [rad] + start_yaw_error : 0.2 + # Gain when adjusting final x position for goal [1/s] + pos_x_gain : 1.0 + # Gain for lane keeping based on y error (differential only) [rad/s^2] + pos_y_yaw_gain : 1.0 + # Gain for lane keeping based on yaw error (differential only) [1/s] + yaw_gain : 2.0 + # Gain for adjusting yaw when not translating, or in case of holonomic drive [1/s] + static_yaw_gain : 3.0 + # Gain for y cost avoidance (differential only) + cost_y_yaw_gain : 0.3 + # How far ahead to compute y cost gradient (constant offset) [m] + cost_y_lookahead_dist : 0.3 + # How far ahead to compute y cost gradient (dynamic offset) [s] + cost_y_lookahead_time : 1.5 + # Gain for yaw cost avoidance + cost_yaw_gain : 2.0 + # Gain for final control low pass filter + low_pass_gain : 0.2 + # Max cost to allow, above we slow down to min_trans_vel or even stop + max_cost : 0.95 + # Max velocity based on curvature [rad/s] + max_curve_vel : 0.3 + # Max distance to goal when looking for it [m] + max_goal_dist : 0.3 + # Max distance allowable for backing up (zero = unlimited) [m] + max_backup_dist : 0.1 + # Minimal distance for stopping [m] + min_stop_dist : 0.3 + # If robot has differential drive, holonomic otherwise + differential_drive : true local_costmap: local_costmap: diff --git a/configs/mp_500/navigation.yaml b/configs/mp_500/navigation.yaml index 66073b8..5e23619 100644 --- a/configs/mp_500/navigation.yaml +++ b/configs/mp_500/navigation.yaml @@ -205,56 +205,70 @@ controller_server: yaw_goal_tolerance: 0.05 stateful: True # neo_local_planner controller parameters +controller_server: + ros__parameters: + # controller server parameters (see Controller Server for more info) + use_sim_time: False + controller_frequency: 50.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] + progress_checker_plugin: "progress_checker" + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 100.0 + general_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.05 + yaw_goal_tolerance: 0.05 + stateful: True FollowPath: - plugin: "neo_local_planner::NeoLocalPlanner" + plugin: "neo_local_planner2::NeoLocalPlanner" # The x acceleration limit of the robot in meters/sec^2 - acc_lim_x : 0.25 - # The y acceleration limit of the robot in meters/sec^2 - acc_lim_y : 0.25 + acc_lim_x : 0.2 # The rotational acceleration limit of the robot in radians/sec^2 - acc_lim_theta : 1.0 + acc_lim_theta : 0.6 # The maximum x velocity for the robot in m/s. - max_vel_x : 0.6 + max_vel_x : 0.8 # The minimum x velocity for the robot in m/s, negative for backwards motion. - min_vel_x : -0.2 - # The maximum y velocity for the robot in m/s - max_vel_y : 0.5 - # The minimum y velocity for the robot in m/s - min_vel_y : -0.5 + min_vel_x : -0.1 # The absolute value of the maximum rotational velocity for the robot in rad/s - max_rot_vel : 0.8 + max_rot_vel : 0.6 # The absolute value of the minimum rotational velocity for the robot in rad/s min_rot_vel : 0.1 # The absolute value of the maximum translational velocity for the robot in m/s - max_trans_vel : 0.6 + max_trans_vel : 0.8 # The absolute value of the minimum translational velocity for the robot in m/s min_trans_vel : 0.1 # The tolerance in radians for the controller in yaw/rotation when achieving its goal - yaw_goal_tolerance : 0.005 + yaw_goal_tolerance : 0.01 # The tolerance in meters for the controller in the x & y distance when achieving a goal - xy_goal_tolerance : 0.01 + xy_goal_tolerance : 0.1 # How long to fine tune for goal position after reaching tolerance limits [s] - goal_tune_time : 2.0 + goal_tune_time : 3.0 # How far to predict control pose into the future based on latest odometry [s] lookahead_time : 0.4 # How far to look ahead when computing path orientation [m] lookahead_dist : 0.5 # Threshold yaw error below which we consider to start moving [rad] - start_yaw_error : 0.5 + start_yaw_error : 0.2 # Gain when adjusting final x position for goal [1/s] pos_x_gain : 1.0 - # Gain when adjusting y position (holonomic only) [1/s] - pos_y_gain : 1.0 + # Gain for lane keeping based on y error (differential only) [rad/s^2] + pos_y_yaw_gain : 1.0 + # Gain for lane keeping based on yaw error (differential only) [1/s] + yaw_gain : 2.0 # Gain for adjusting yaw when not translating, or in case of holonomic drive [1/s] static_yaw_gain : 3.0 - # Gain for x cost avoidance (holonomic only) - cost_x_gain : 0.2 - # Gain for y cost avoidance (holonomic only) - cost_y_gain : 0.2 + # Gain for y cost avoidance (differential only) + cost_y_yaw_gain : 0.3 # How far ahead to compute y cost gradient (constant offset) [m] - cost_y_lookahead_dist : 0.0 + cost_y_lookahead_dist : 0.3 # How far ahead to compute y cost gradient (dynamic offset) [s] - cost_y_lookahead_time : 2.0 + cost_y_lookahead_time : 1.5 # Gain for yaw cost avoidance cost_yaw_gain : 2.0 # Gain for final control low pass filter @@ -264,15 +278,13 @@ controller_server: # Max velocity based on curvature [rad/s] max_curve_vel : 0.3 # Max distance to goal when looking for it [m] - max_goal_dist : 0.5 + max_goal_dist : 0.3 # Max distance allowable for backing up (zero = unlimited) [m] - max_backup_dist : 0.0 + max_backup_dist : 0.1 # Minimal distance for stopping [m] - min_stop_dist : 0.6 + min_stop_dist : 0.3 # If robot has differential drive, holonomic otherwise - differential_drive : false - # Enable or disable reversing - allow_reversing: false + differential_drive : true local_costmap: local_costmap: diff --git a/configs/mpo_500/navigation.yaml b/configs/mpo_500/navigation.yaml index cb1edaf..645bbec 100644 --- a/configs/mpo_500/navigation.yaml +++ b/configs/mpo_500/navigation.yaml @@ -198,7 +198,7 @@ controller_server: stateful: True # neo_local_planner parameters FollowPath: - plugin: "neo_local_planner::NeoLocalPlanner" + plugin: "neo_local_planner2::NeoLocalPlanner" # The x acceleration limit of the robot in meters/sec^2 acc_lim_x : 0.25 # The y acceleration limit of the robot in meters/sec^2 diff --git a/configs/mpo_700/navigation.yaml b/configs/mpo_700/navigation.yaml index e88037d..6d6873d 100644 --- a/configs/mpo_700/navigation.yaml +++ b/configs/mpo_700/navigation.yaml @@ -181,7 +181,7 @@ controller_server: # controller server parameters (see Controller Server for more info) controller_plugins: ["FollowPath"] controller_frequency: 100.0 - controller_plugin_types: ["neo_local_planner::NeoLocalPlanner"] + controller_plugin_types: ["neo_local_planner2::NeoLocalPlanner"] goal_checker_plugins: ["general_goal_checker"] progress_checker_plugin: "progress_checker" use_sim_time: True @@ -195,7 +195,7 @@ controller_server: yaw_goal_tolerance: 0.05 stateful: True FollowPath: - plugin: "neo_local_planner::NeoLocalPlanner" + plugin: "neo_local_planner2::NeoLocalPlanner" acc_lim_x : 0.25 acc_lim_y : 0.25 acc_lim_theta : 0.8