exploration parameters are updated
This commit is contained in:
parent
36090b407a
commit
b7d6503816
|
@ -15,6 +15,7 @@ RUN apt-get update && apt-get install -y \
|
|||
ros-humble-slam-toolbox \
|
||||
ros-humble-robot-localization\
|
||||
ros-humble-image-geometry \
|
||||
ros-humble-test-msgs \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
# Cheange the ROS2 RMW to CycloneDDS as instructed by Unitree
|
||||
|
|
|
@ -299,9 +299,9 @@ inline void SourceDriver::publishLaserScan(
|
|||
scan_msg->time_increment = 0.0;
|
||||
scan_msg->scan_time = 0.3333;
|
||||
scan_msg->range_min = 0.1;
|
||||
scan_msg->range_max = 15;
|
||||
scan_msg->range_max = 50;
|
||||
float max_height_ = 1.0;
|
||||
float min_height_ = -0.1;
|
||||
float min_height_ = -0.05;
|
||||
|
||||
// scan_msg->angle_min = -(3*3.1415)/4;
|
||||
// scan_msg->angle_max = -3.1415/4;
|
||||
|
|
|
@ -28,8 +28,8 @@ slam_toolbox:
|
|||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 5.0
|
||||
resolution: 0.05
|
||||
max_laser_range: 5.0 #for rastering images
|
||||
minimum_time_interval: 0.5
|
||||
max_laser_range: 8.0 #for rastering images
|
||||
minimum_time_interval: 0.1
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 30.
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
|
|
|
@ -0,0 +1,350 @@
|
|||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_link"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: 100.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "likelihood_field"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
odom_frame_id: "odom"
|
||||
pf_err: 0.05
|
||||
pf_z: 0.99
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 1.0
|
||||
update_min_a: 0.2
|
||||
update_min_d: 0.25
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
scan_topic: /go2/scan
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /go2/odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
# '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_assisted_teleop_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_goal_updated_controller_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_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
- nav2_is_battery_charging_condition_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
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
|
||||
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.5
|
||||
yaw_goal_tolerance: 1.
|
||||
# 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
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: False
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
# robot_radius: 0.22
|
||||
footprint: "[ [0.35, 0.195], [0.35, -0.195], [-0.35, -0.195], [-0.35, 0.195] ]"
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.2
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /go2/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: False
|
||||
robot_radius: 0.22
|
||||
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: /go2/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
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.65
|
||||
always_send_full_costmap: True
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
# 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: ""
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: False
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
behavior_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", "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"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
use_sim_time: False
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
loop_rate: 20
|
||||
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: False
|
||||
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: "/go2/odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
|
@ -110,74 +110,101 @@ bt_navigator_navigate_to_pose_rclcpp_node:
|
|||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
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
|
||||
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.5
|
||||
yaw_goal_tolerance: 1.
|
||||
# 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
|
||||
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
|
||||
TrajectoryVisualizer:
|
||||
trajectory_step: 5
|
||||
time_step: 3
|
||||
AckermannConstraints:
|
||||
min_turning_r: 0.05
|
||||
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
|
||||
# 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
|
||||
CostCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 20.0
|
||||
critical_cost: 300.0
|
||||
consider_footprint: true
|
||||
collision_cost: 1000000.0
|
||||
near_goal_distance: 1.0
|
||||
PathAlignCritic:
|
||||
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
|
||||
forward_preference: true
|
||||
# VelocityDeadbandCritic:
|
||||
# enabled: true
|
||||
# cost_power: 1
|
||||
# cost_weight: 35.0
|
||||
# deadband_velocities: [0.05, 0.05, 0.05]
|
||||
# TwirlingCritic:
|
||||
# enabled: true
|
||||
# twirling_cost_power: 1
|
||||
# twirling_cost_weight: 10.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
|
@ -191,7 +218,8 @@ local_costmap:
|
|||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.22
|
||||
# robot_radius: 0.22
|
||||
footprint: "[ [0.33, 0.2], [0.33 -0.2], [-0.48 -0.2], [-0.48, 0.2] ]"
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
|
@ -231,6 +259,7 @@ global_costmap:
|
|||
robot_base_frame: base_link
|
||||
use_sim_time: False
|
||||
robot_radius: 0.22
|
||||
# footprint: "[ [0.34, 0.2], [0.34 -0.2], [-0.34 -0.2], [-0.34, 0.2] ]"
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
|
@ -254,7 +283,7 @@ global_costmap:
|
|||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.65
|
||||
inflation_radius: 0.25
|
||||
always_send_full_costmap: True
|
||||
|
||||
map_server:
|
||||
|
@ -272,6 +301,43 @@ map_saver:
|
|||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
# planner_server:
|
||||
# ros__parameters:
|
||||
# expected_planner_frequency: 1.0
|
||||
# use_sim_time: False
|
||||
# planner_plugins: ["GridBased"]
|
||||
# GridBased:
|
||||
# plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
# tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters
|
||||
# downsample_costmap: false # whether or not to downsample the map
|
||||
# downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
|
||||
# allow_unknown: true # allow traveling in unknown space
|
||||
# max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
|
||||
# max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance
|
||||
# max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
|
||||
# motion_model_for_search: "REEDS_SHEPP" # For Hybrid Dubin, Redds-Shepp
|
||||
# cost_travel_multiplier: 1.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
|
||||
# angle_quantization_bins: 64 # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
|
||||
# analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach.
|
||||
# analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius
|
||||
# minimum_turning_radius: 0.3 # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle
|
||||
# reverse_penalty: 1.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
|
||||
# change_penalty: 0.2 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0
|
||||
# non_straight_penalty: 1.20 # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1
|
||||
# cost_penalty: 50.0 # For Hybrid nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
|
||||
# retrospective_penalty: 0.025 # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier along the path. Saves search time since earlier nodes are not expanded until it is necessary. Must be >= 0.0 and <= 1.0
|
||||
# rotation_penalty: 2.0 # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings.
|
||||
# lookup_table_size: 20.0 # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters.
|
||||
# cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
|
||||
# allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step.
|
||||
# smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes.
|
||||
# smoother:
|
||||
# max_iterations: 1000
|
||||
# w_smooth: 0.3
|
||||
# w_data: 0.2
|
||||
# tolerance: 1.0e-10
|
||||
# do_refinement: true # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
|
@ -282,7 +348,6 @@ planner_server:
|
|||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
|
|
|
@ -4,7 +4,7 @@ ekf_filter_node:
|
|||
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
|
||||
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
|
||||
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
|
||||
frequency: 50.0
|
||||
frequency: 30.0
|
||||
|
||||
# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
|
||||
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
|
||||
|
@ -15,7 +15,7 @@ ekf_filter_node:
|
|||
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
|
||||
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
|
||||
# by, for example, an IMU. Defaults to false if unspecified.
|
||||
two_d_mode: false
|
||||
two_d_mode: true
|
||||
|
||||
# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
|
||||
# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
|
||||
|
|
Loading…
Reference in New Issue