From be1576c7ce46856cad9d07c9459677c2d09b954f Mon Sep 17 00:00:00 2001 From: pmusau17 Date: Tue, 4 May 2021 15:50:05 -0500 Subject: [PATCH] progress --- .../cfg/costmap_common_params.yaml | 25 ++++++---- rsband_local_planner/cfg/global_costmap.yaml | 9 +++- .../cfg/local_costmap_param.yaml | 46 ++++++++++--------- 3 files changed, 46 insertions(+), 34 deletions(-) diff --git a/rsband_local_planner/cfg/costmap_common_params.yaml b/rsband_local_planner/cfg/costmap_common_params.yaml index 59713ce..7377e80 100644 --- a/rsband_local_planner/cfg/costmap_common_params.yaml +++ b/rsband_local_planner/cfg/costmap_common_params.yaml @@ -1,26 +1,31 @@ -obstacle_range: 2.5 -raytrace_range: 1.0 -inf_is_valid: True +obstacle_range: 5.5 +raytrace_range: 6.0 +#inf_is_valid: True #origin_z: 0.0 -z_resolution: 0.2 +#z_resolution: 0.2 #min_obstacle_height: 0.01 -max_obstacle_height: 2.4 -min_obstacle_height: -5.0 +#max_obstacle_height: 2.4 +#min_obstacle_height: -5.0 #unknown_threshold: 9 #mark_threshold: 0 +#clearing: true, +observation_sources: laser_scan_sensor +recovery_behavior_enabled: false +inflation_radius: 0.55 +laser_scan_sensor: {data_type: LaserScan, sensor_frame: racecar/laser, topic: racecar/filtered_scan, marking: true, clearing: false} #footprint: [[0.406, 0.146], [-0.102, 0.146], [-0.102, -0.146],[0.406,-0.146]] # These are the vertices of the footprint the orign for car like robots in the rear axle footprint: [[0.1125, 0.1125], [-0.1125, 0.1125], [-0.1125, -0.1125],[0.1125,-0.1125]] -inflation_radius: 0.55 +#inflation_radius: 0.55 #combination_method: 0 #The cost at which a cell is considered an obstacle when a map is read from the map_server -lethal_cost_threshold: 100 +#lethal_cost_threshold: 100 #plugins: #- {name: obstacles_layer, type: "costmap_2d::ObstacleLayer"} #- {name: inflater_layer, type: "costmap_2d::InflationLayer"} cost_scaling_factor: 10.0 -observation_sources: laser_scan_sensor -laser_scan_sensor: {sensor_frame: racecar/laser, data_type: LaserScan, topic: /filtered_scan, marking: true, clearing: true,obstacle_range: 2.5, raytrace_range: 3.0} +#observation_sources: laser_scan_sensor +#laser_scan_sensor: {sensor_frame: racecar/laser, data_type: LaserScan, topic: /filtered_scan, marking: true, clearing: true,obstacle_range: 2.5, raytrace_range: 3.0} diff --git a/rsband_local_planner/cfg/global_costmap.yaml b/rsband_local_planner/cfg/global_costmap.yaml index 22fa312..9d6bc4c 100644 --- a/rsband_local_planner/cfg/global_costmap.yaml +++ b/rsband_local_planner/cfg/global_costmap.yaml @@ -11,6 +11,11 @@ global_costmap: #static_map: true plugins: - {name: static_layer, type: "costmap_2d::StaticLayer"} - - {name: obstacles_layer, type: "costmap_2d::ObstacleLayer"} - - {name: inflater_layer, type: "costmap_2d::InflationLayer"} + - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} + - {name: inflation_layer, type: "costmap_2d::InflationLayer"} + + + inflation_layer: + - inflation_radius: 0.35 + - cost_scaling_factor: 0.5 \ No newline at end of file diff --git a/rsband_local_planner/cfg/local_costmap_param.yaml b/rsband_local_planner/cfg/local_costmap_param.yaml index 6175e84..c77421a 100644 --- a/rsband_local_planner/cfg/local_costmap_param.yaml +++ b/rsband_local_planner/cfg/local_costmap_param.yaml @@ -1,34 +1,36 @@ local_costmap: + #global_frame global_frame: racecar/odom robot_base_frame: racecar/base_link + plugins: + - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} + - {name: static_layer, type: "costmap_2d::StaticLayer"} + - {name: voxel_layer, type: "costmap_2d::VoxelLayer"} + - {name: inflation_layer, type: "costmap_2d::InflationLayer"} update_frequency: 5.0 publish_frequency: 5.0 transform_tolerance: 0.5 + track_unknown_space: false rolling_window: true footprint: [[0.1125, 0.1125], [-0.1125, 0.1125], [-0.1125, -0.1125],[0.1125,-0.1125]] - width: 3.0 - height: 3.0 - # Note that its fine for the resolution of this grid to be different than the resolution of your static map, but most of the time we tend to set them equally. - resolution: 0.05 - observation_sources: laser_scan_sensor - laser_scan_sensor: - - sensor_frame: racecar/laser, - - data_type: LaserScan, - - topic: /racecar/filtered_scan, - - marking: true, - - clearing: true, - - obstacle_range: 2.5 - - raytrace_range: 3.0 + width: 6.0 + height: 6.0 + - inflater_layer: - - inflation_radius: 0.35 + # Note that its fine for the resolution of this grid to be different than the resolution of your static map, but most of the time we tend to set them equally. + resolution: 0.04 + obstacle_layer: + track_unknown_space: false + combination_method: 1 + enabled: true + observation_sources: laser_scan_sensor + laser_scan_sensor: {data_type: LaserScan, sensor_frame: racecar/laser, topic: racecar/filtered_scan, marking: true, clearing: false, obstacle_range: 2.5, raytrace_range: 3.0, observation_persistence: 0, min_obstacle_height: -5.01, max_obstacle_height: 5.01, inf_is_valid: true} + inflation_layer: + - inflation_radius: 0.55 - cost_scaling_factor: 0.5 + voxel_layer: + observation_sources: laser_scan_sensor + laser_scan_sensor: {data_type: LaserScan, sensor_frame: racecar/laser, topic: racecar/filtered_scan, marking: true, clearing: false, obstacle_range: 2.5, raytrace_range: 3.0, observation_persistence: 0, min_obstacle_height: -5.01, max_obstacle_height: 5.01, inf_is_valid: true, origin_z: 0.0, z_resolution: 0.2, unknown_threshold: 9} -# static_map: false - plugins: - #- {name: obstacles_layer, type: "costmap_2d::ObstacleLayer"} - #- {name: inflater_layer, type: "costmap_2d::InflationLayer"} - #- {name: static_layer, type: "costmap_2d::StaticLayer"} - - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} - - {name: inflation_layer, type: "costmap_2d::InflationLayer"} +