diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml new file mode 100644 index 0000000..480fd96 --- /dev/null +++ b/config/costmap_common_params.yaml @@ -0,0 +1,8 @@ +obstacle_range: 2.5 +raytrace_range: 3.0 +robot_radius: 0.25 +inflation_radius: 0.15 +max_obstacle_height: 0.6 +min_obstacle_height: 0.0 +observation_sources: scan +scan: {data_type: LaserScan, topic: /scan_filtered, marking: true, clearing: true, expected_update_rate: 0} \ No newline at end of file diff --git a/config/dwa_base_local_planner_params.yaml b/config/dwa_base_local_planner_params.yaml index a985f40..e0e9935 100644 --- a/config/dwa_base_local_planner_params.yaml +++ b/config/dwa_base_local_planner_params.yaml @@ -1,12 +1,12 @@ controller_frequency: 5.0 -recovery_behavior_enabled: false +recovery_behavior_enabled: true #false clearing_rotation_allowed: false DWAPlannerROS: max_vel_trans: 1.0 - min_vel_trans: 0.2 + min_vel_trans: 0.1 #0.2 max_vel_x: 1.0 - min_vel_x: 0.2 + min_vel_x: 0.1 #0.2 max_vel_y: 0.0 # zero for a differential drive robot min_vel_y: 0.0 min_in_place_vel_theta: 0.5 @@ -26,16 +26,26 @@ DWAPlannerROS: heading_lookahead: 0.325 heading_scoring: false heading_scoring_timestep: 0.8 - occdist_scale: 0.1 + #occdist_scale: 0.1 oscillation_reset_dist: 0.05 publish_cost_grid_pc: false prune_plan: true + # Forward Simulation Parameters sim_time: 2.5 sim_granularity: 0.025 angular_sim_granularity: 0.025 vx_samples: 8 - vy_samples: 0 # zero for a differential drive robot + vy_samples: 1 #0 # zero for a differential drive robot vtheta_samples: 20 dwa: true simple_attractor: false + + # Trajectory Scoring Parameters + path_distance_bias: 64.0 # 32.0 - weighting for how much it should stick to the global path plan + goal_distance_bias: 24.0 # 24.0 - wighting for how much it should attempt to reach its goal + occdist_scale: 0.5 # 0.01 - weighting for how much the controller should avoid obstacles + forward_point_distance: 0.325 # 0.325 - how far along to place an additional scoring point + stop_time_buffer: 0.2 # 0.2 - amount of time a robot must stop in before colliding for a valid traj. + scaling_speed: 0.25 # 0.25 - absolute velocity at which to start scaling the robot's footprint + max_scaling_factor: 0.2 # 0.2 - how much to scale the robot's footprint when at speed. \ No newline at end of file diff --git a/config/global_costmap_params.yaml b/config/global_costmap_params.yaml new file mode 100644 index 0000000..dd0f10e --- /dev/null +++ b/config/global_costmap_params.yaml @@ -0,0 +1,10 @@ +global_costmap: + global_frame: map + robot_base_frame: base_link + update_frequency: 5.0 # 1.0 + publish_frequency: 1.0 #0 + static_map: true + rolling_window: false + resolution: 0.01 + transform_tolerance: 0.5 + map_type: costmap \ No newline at end of file diff --git a/config/local_costmap_params.yaml b/config/local_costmap_params.yaml new file mode 100644 index 0000000..2cf8890 --- /dev/null +++ b/config/local_costmap_params.yaml @@ -0,0 +1,12 @@ +local_costmap: + global_frame: odom + robot_base_frame: base_link + update_frequency: 5.0 #1.0 + publish_frequency: 1.0 + static_map: false + rolling_window: true + width: 6.0 + height: 6.0 + resolution: 0.01 + transform_tolerance: 0.5 + map_type: costmap \ No newline at end of file diff --git a/launch/linus_amcl.launch b/launch/linus_amcl.launch index 66e99dc..6862a5a 100644 --- a/launch/linus_amcl.launch +++ b/launch/linus_amcl.launch @@ -12,22 +12,30 @@ - + + - + + - - - + + + + + + - + + - - + + + + @@ -37,11 +45,16 @@ + - - - - + + + + + + + +