Clear obstacles of obstacle layer of costmap
Hello everyone.
I have a Realsense D435 camera on my application, and I use its depth_image to generate a laser scan topic with the package depthimage_to_laserscan. This works allright.
The problem I want to solve is when I generate obstacles with this laser topic, it marks me the obstacle but when I move and the obstacle get closer it updates the position of the obstacle but it does not lcear the previus marks of the obstacle.
Here you can see screenshots of the problems:
In the image above the obstacle layer is just generating marks where the sensor sees an obstacle (green line). But in the image below the obstacle gets close and it generates the new marks, but the old marks are not being cleared.
I think is an error of configuration parameters of local_costmap, so here it is the code:
costmap_common_params.yaml
footprint: [[-0.30 , 0.38], [0.70, 0.38], [0.70, -0.38], [-0.30, -0.38]]
laser_layer: #Laser
#track_unknown_space: false
#footprint_clearing_enabled: true
#combination_method: 1
observation_sources: laser
laser:
topic: /scan
sensor_frame: camera_depth_frame
#observation_persistance: 0.0
expected_update_rate: 0
data_type: LaserScan
clearing: true
marking: true
#max_obstacle_height: 2.0
#min_obstacle_height: 0.0
obstacle_range: 3.0
raytrace_range: 5.0
#inf_is_invalid: false
pointcloud_layer: #Nube de puntos
#origin_z: 0.0
#z_resolution: 0.2
#z_voxels: 10
#unknown_threshold: pointcloud_layer/z_voxels
#mark_threshold: 0
#publish_voxel_map: false
#footprint_clearing_enabled: true
observation_sources: pointcloud
pointcloud:
topic: /camera/depth/color/points
sensor_frame: camera_depth_frame
#observation_persistance: 0.0
expected_update_rate: 0
data_type: PointCloud2
clearing: true
marking: true
#max_obstacle_height: 2.0
#min_obstacle_height: 0.0
obstacle_range: 3.0
raytrace_range: 5.0
#inf_is_invalid: false
inflation_layer:
inflation_radius: 0.75
#cost_scaling_factor: 10.0
local_costmap_params.yaml
local_costmap:
plugins:
- {name: static_map, type: "costmap_2d::StaticLayer"}
- {name: laser_layer, type: "costmap_2d::ObstacleLayer"} #Laser sensors
#- {name: pointcloud_layer, type: "costmap_2d::VoxelLayer"} #Pointcloud sensors
#- {name: ultrasonic, type: "range_sensor_layer::RangeSensorLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
update_frequency: 2.0 #HIGH CPU usage with sensors
publish_frequency: 50.0 #Reducir para aligerar CPU
global_frame: "odom" #To inflate obstacles
robot_base_frame: "base_link"
#static_map: false
rolling_window: true
width: 6.0 #6
height: 6.0 #6
resolution: 0.05 #0.01
#always_send_full_costmap: true
global_costmap_params.yaml
global_costmap:
plugins:
- {name: static_map, type: "costmap_2d::StaticLayer"}
#- {name: ultrasonic, type: "range_sensor_layer::RangeSensorLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
global_frame: "map"
robot_base_frame: "base_link"
update_frequency: 2.0 #HIGH CPU usage with sensors
publish_frequency: 50.0 #Reducir para aligerar CPU
resolution: 0.5 #0.01 #The resolution of the map in meters/cell.
transform_tolerance: 0.2 #Specifies the delay in transform (tf) data that is tolerable in seconds
map_type: costmap
#always_send_full_costmap: true
base_local_planner_params.yaml
#recovery_behavior_enabled: false
#clearing_rotation_allowed: false
controller_frequency: 10 #Default 20 took many time
TrajectoryPlannerROS:
max_vel_x: 0.4 #meters/sec #0.6
min_vel_x: -0.1
max_vel_y: 0.0 # zero for a differential drive robot
min_vel_y: 0.0 #radians/sec
max_vel_theta: 1.0 #3
min_vel_theta: -1.0
min_in_place_vel_theta: 0.1 #radians/sec, in-place rotations
escape_vel: -0.1 #0.1
acc_lim_x: 0.4 #meters/sec^2
acc_lim_y: 0.0 # zero for a differential drive robot
acc_lim_theta: 1.0 #radians/sec^2
holonomic_robot: false
#####Trajectory Scoring Parameters#####
meter_scoring: true #goal_distance and path_distance are expressed in units ...
Please upload your images to the answer so they are retained.
The platform doesn't let me upload images because I am less than 5 points. Can't you see them using the links?
Obstacle far
Obstacle near
Here you are another links to see the images.
Thank you for your response.
Please attach the images to your question now. I've given you sufficient karma.
Thank you! It's done.