Local costmap does not update obstacles on one side of the robot
Hi, I have been facing this weird issue and would appreciate any help possible.
My robot has a 2D LiDAR sensor and I have been trying to generate a global and a local costmap using the ROS Navigation's costmap_2d. The package does generate correct global costmap but the local costmap does not update any obstacles on the right half of the robot.
I tried to play around with the "expectedupdaterate" param and other ObstacleLayer params but I couldn't get the desired output. I also tried rotating the robot, but it's independent of it, it's always the right half of the robot that is ignored.
Here's the output on the rviz: https://drive.google.com/file/d/1lzLytuZi0IN-nGGicddwgB5wXsBbJ6Iv/view?usp=sharing
The following are the params for move_base and the launch file:
DWAPlannerROS: {acc_lim_theta: 2.0, acc_lim_trans: 0.1, acc_lim_x: 1.0, acc_lim_y: 0.0, angular_sim_granularity: 0.1, forward_point_distance: 0.325, global_frame_id: odom, goal_distance_bias: 24.0, max_scaling_factor: 0.2, max_vel_theta: 1.0, max_vel_trans:
0.5, max_vel_x: 0.5, max_vel_y: 0.5, min_vel_theta: 0.4, min_vel_trans:
0.1, min_vel_x: 0.0, min_vel_y: 0.0, occdist_scale: 0.5, oscillation_reset_angle: 0.2, oscillation_reset_dist: 0.05, path_distance_bias: 64.0, prune_plan: false, publish_cost_grid_pc: true, publish_traj_pc: true, restore_defaults: false, scaling_speed: 0.25, sim_granularity:
0.025, sim_time: 1.0, stop_time_buffer: 0.2, theta_stopped_vel: 0.4, trans_stopped_vel: 0.0, twirling_scale: 0.0, use_dwa: true, vth_samples: 20, vtheta_samples: 20, vx_samples: 6, vy_samples: 1, xy_goal_tolerance: 0.15, yaw_goal_tolerance: 0.3} GlobalPlanner: {allow_unknown: true, cost_factor: 3.0, default_tolerance:
0.0, lethal_cost: 253, neutral_cost: 50, old_navfn_behavior: false, planner_costmap_publish_frequency:
0.0, planner_window_x: 0.0, planner_window_y: 0.0, publish_potential: true, publish_scale: 100, use_dijkstra: true, use_grid_path: false, use_quadratic: true} NavfnROS: {allow_unknown: false, default_tolerance: 0.0, planner_window_x: 0.0, planner_window_y: 0.0, visualize_potential: false} aggressive_reset: {reset_distance:
1.84} base_global_planner: navfn/NavfnROS base_local_planner: dwa_local_planner/DWAPlannerROS clearing_rotation_allowed: true conservative_reset: {reset_distance:
3.0} conservative_reset_dist: 3.0 controller_frequency: 5.0 controller_patience: 3.0 global_costmap: footprint: '[]' footprint_padding: 0.01 global_frame: map height: 10 inflation_layer: {cost_scaling_factor:
5.0, enabled: true, inflate_unknown: false,
inflation_radius: 0.3} obstacle_layer:
combination_method: 1
enabled: true
footprint_clearing_enabled: true
max_obstacle_height: 2.0
observation_sources: scan
obstacle_range: 5.0
raytrace_range: 10.0
scan: {clearing: true, data_type: LaserScan, marking: true, sensor_frame: base_link,
topic: /scan} obstacle_range: 5.0 origin_x: 0.0 origin_y: 0.0 plugins:
- {name: static_layer, type: 'costmap_2d::StaticLayer'}
- {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
- {name: inflation_layer, type: 'costmap_2d::InflationLayer'} publish_frequency: 0.5 raytrace_range: 10.0 resolution:
0.05 robot_base_frame: base_link robot_radius: 0.2 rolling_window: false static_layer: {enabled: true, map_topic: map, subscribe_to_updates: true} static_map: true transform_tolerance: 0.5 update_frequency: 1.0 width: 10 local_costmap: combination_method: 0 footprint: '[]' footprint_padding:
0.01 global_frame: odom height: 10 inflation_layer: {cost_scaling_factor:
5.0, enabled: true, inflate_unknown: false,
inflation_radius: 0.3} obstacle_layer:
combination_method: 1
enabled: true
footprint_clearing_enabled: true
max_obstacle_height: 2.0
observation_sources: scan
obstacle_range: 5.0
raytrace_range: 10.0
scan: {clearing: true, data_type: LaserScan, expected_update_rate: 0.4, marking: true,
sensor_frame: base_link, topic: /scan} obstacle_range: 5.0 origin_x: 0.0 origin_y: 0.0 plugins:
- {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
- {name: inflation_layer, type: 'costmap_2d::InflationLayer'} publish_frequency: 5.0 raytrace_range: 10.0 resolution:
0.05 robot_base_frame: base_link robot_radius: 0.2 rolling_window: true static_map: false transform_tolerance: 0.5 update_frequency: 5.0 width: 10 make_plan_add_unreachable_goal: true make_plan_clear_costmap: true max_planning_retries: -1 oscillation_distance: 0.2 oscillation_timeout: 10.0 planner_frequency: 1.0 planner_patience: 5.0 recovery_behavior_enabled: true restore_defaults: false shutdown_costmaps: false
And this is the output on the terminal:
roslaunch quadrotor_navigation quadrotor_move_base.launch
... logging to /home/ubuntu/.ros/log/3f520dfc-a450-11eb-ac84-dca632b444f0/roslaunch-ubuntu-21099.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://192.168.50.153:46353/
SUMMARY
========
PARAMETERS
* /move_base/DWAPlannerROS/acc_lim_theta: 2.0
* /move_base/DWAPlannerROS/acc_lim_x: 1.0
* /move_base/DWAPlannerROS/acc_lim_y: 0.0
* /move_base/DWAPlannerROS/forward_point_distance: 0.325
* /move_base/DWAPlannerROS/global_frame_id: odom
* /move_base/DWAPlannerROS/goal_distance_bias: 24.0
* /move_base/DWAPlannerROS/max_scaling_factor: 0.2
* /move_base/DWAPlannerROS/max_vel_theta: 1.0
* /move_base/DWAPlannerROS/max_vel_trans: 0.5
* /move_base/DWAPlannerROS/max_vel_x: 0.5
* /move_base/DWAPlannerROS/max_vel_y: 0.5
* /move_base/DWAPlannerROS/min_vel_theta: 0.4
* /move_base/DWAPlannerROS/min_vel_trans: 0.1
* /move_base/DWAPlannerROS/min_vel_x: 0.0
* /move_base/DWAPlannerROS/min_vel_y: 0.0
* /move_base/DWAPlannerROS/occdist_scale: 0.5
* /move_base/DWAPlannerROS/oscillation_reset_dist: 0.05
* /move_base/DWAPlannerROS/path_distance_bias: 64.0
* /move_base/DWAPlannerROS/publish_cost_grid_pc: True
* /move_base/DWAPlannerROS/publish_traj_pc: True
* /move_base/DWAPlannerROS/scaling_speed: 0.25
* /move_base/DWAPlannerROS/sim_time: 1.0
* /move_base/DWAPlannerROS/stop_time_buffer: 0.2
* /move_base/DWAPlannerROS/theta_stopped_vel: 0.4
* /move_base/DWAPlannerROS/trans_stopped_vel: 0.0
* /move_base/DWAPlannerROS/vtheta_samples: 20
* /move_base/DWAPlannerROS/vx_samples: 6
* /move_base/DWAPlannerROS/vy_samples: 1
* /move_base/DWAPlannerROS/xy_goal_tolerance: 0.15
* /move_base/DWAPlannerROS/yaw_goal_tolerance: 0.3
* /move_base/GlobalPlanner/allow_unknown: True
* /move_base/GlobalPlanner/cost_factor: 3.0
* /move_base/GlobalPlanner/default_tolerance: 0.0
* /move_base/GlobalPlanner/lethal_cost: 253
* /move_base/GlobalPlanner/neutral_cost: 50
* /move_base/GlobalPlanner/old_navfn_behavior: False
* /move_base/GlobalPlanner/planner_costmap_publish_frequency: 0.0
* /move_base/GlobalPlanner/planner_window_x: 0.0
* /move_base/GlobalPlanner/planner_window_y: 0.0
* /move_base/GlobalPlanner/publish_potential: True
* /move_base/GlobalPlanner/publish_scale: 100
* /move_base/GlobalPlanner/use_dijkstra: True
* /move_base/GlobalPlanner/use_grid_path: False
* /move_base/GlobalPlanner/use_quadratic: True
* /move_base/NavfnROS/allow_unknown: False
* /move_base/NavfnROS/default_tolerance: 0.0
* /move_base/NavfnROS/planner_window_x: 0.0
* /move_base/NavfnROS/planner_window_y: 0.0
* /move_base/NavfnROS/visualize_potential: False
* /move_base/base_global_planner: navfn/NavfnROS
* /move_base/base_local_planner: dwa_local_planner...
* /move_base/controller_frequency: 5.0
* /move_base/controller_patience: 3.0
* /move_base/global_costmap/global_frame: map
* /move_base/global_costmap/inflation_layer/cost_scaling_factor: 5.0
* /move_base/global_costmap/inflation_layer/enabled: True
* /move_base/global_costmap/inflation_layer/inflation_radius: 0.3
* /move_base/global_costmap/obstacle_layer/observation_sources: scan
* /move_base/global_costmap/obstacle_layer/scan/clearing: True
* /move_base/global_costmap/obstacle_layer/scan/data_type: LaserScan
* /move_base/global_costmap/obstacle_layer/scan/marking: True
* /move_base/global_costmap/obstacle_layer/scan/sensor_frame: base_link
* /move_base/global_costmap/obstacle_layer/scan/topic: /scan
* /move_base/global_costmap/obstacle_range: 5.0
* /move_base/global_costmap/plugins: [{'type': 'costma...
* /move_base/global_costmap/publish_frequency: 0.5
* /move_base/global_costmap/raytrace_range: 10.0
* /move_base/global_costmap/robot_base_frame: base_link
* /move_base/global_costmap/robot_radius: 0.2
* /move_base/global_costmap/rolling_window: False
* /move_base/global_costmap/static_layer/enabled: True
* /move_base/global_costmap/static_layer/map_topic: map
* /move_base/global_costmap/static_layer/subscribe_to_updates: True
* /move_base/global_costmap/static_map: True
* /move_base/global_costmap/transform_tolerance: 0.5
* /move_base/global_costmap/update_frequency: 1.0
* /move_base/local_costmap/combination_method: 0
* /move_base/local_costmap/global_frame: odom
* /move_base/local_costmap/height: 10.0
* /move_base/local_costmap/inflation_layer/cost_scaling_factor: 5.0
* /move_base/local_costmap/inflation_layer/enabled: True
* /move_base/local_costmap/inflation_layer/inflation_radius: 0.3
* /move_base/local_costmap/obstacle_layer/enabled: True
* /move_base/local_costmap/obstacle_layer/observation_sources: scan
* /move_base/local_costmap/obstacle_layer/obstacle_range: 5.0
* /move_base/local_costmap/obstacle_layer/raytrace_range: 4.0
* /move_base/local_costmap/obstacle_layer/scan/clearing: True
* /move_base/local_costmap/obstacle_layer/scan/data_type: LaserScan
* /move_base/local_costmap/obstacle_layer/scan/expected_update_rate: 0.4
* /move_base/local_costmap/obstacle_layer/scan/marking: True
* /move_base/local_costmap/obstacle_layer/scan/sensor_frame: base_link
* /move_base/local_costmap/obstacle_layer/scan/topic: /scan
* /move_base/local_costmap/obstacle_range: 5.0
* /move_base/local_costmap/plugins: [{'type': 'costma...
* /move_base/local_costmap/publish_frequency: 5.0
* /move_base/local_costmap/raytrace_range: 10.0
* /move_base/local_costmap/resolution: 0.05
* /move_base/local_costmap/robot_base_frame: base_link
* /move_base/local_costmap/robot_radius: 0.2
* /move_base/local_costmap/rolling_window: True
* /move_base/local_costmap/static_map: False
* /move_base/local_costmap/transform_tolerance: 0.5
* /move_base/local_costmap/update_frequency: 5.0
* /move_base/local_costmap/width: 10.0
* /move_base/oscillation_distance: 0.2
* /move_base/oscillation_timeout: 10.0
* /move_base/planner_frequency: 1.0
* /move_base/planner_patience: 5.0
* /move_base/shutdown_costmaps: False
* /rosdistro: melodic
* /rosversion: 1.14.10
NODES
/
move_base (move_base/move_base)
ROS_MASTER_URI=http://192.168.50.153:11311
process[move_base-1]: started with pid [21151]
[ WARN] [1619201412.712404665]: global_costmap: Pre-Hydro parameter "static_map" unused since "plugins" is provided
[ INFO] [1619201412.719044026]: global_costmap: Using plugin "static_layer"
[ INFO] [1619201412.751356793]: Requesting the map...
[ INFO] [1619201412.960234006]: Resizing costmap to 520 X 427 at 0.050000 m/pix
[ INFO] [1619201413.058874145]: Received a 520 X 427 map at 0.050000 m/pix
[ INFO] [1619201413.059066548]: Subscribing to updates
[ INFO] [1619201413.083822526]: global_costmap: Using plugin "obstacle_layer"
[ INFO] [1619201413.112456692]: Subscribed to Topics: scan
[ INFO] [1619201413.166898242]: global_costmap: Using plugin "inflation_layer"
[ WARN] [1619201413.321659104]: local_costmap: Pre-Hydro parameter "static_map" unused since "plugins" is provided
[ INFO] [1619201413.325585051]: local_costmap: Using plugin "obstacle_layer"
[ INFO] [1619201413.341814387]: Subscribed to Topics: scan
[ INFO] [1619201413.395734650]: local_costmap: Using plugin "inflation_layer"
[ INFO] [1619201413.524319690]: Created local_planner dwa_local_planner/DWAPlannerROS
[ INFO] [1619201413.559936655]: Sim period is set to 0.20
[ WARN] [1619201413.886061170]: The /scan observation buffer has not been updated for 0.52 seconds, and it should be updated every 0.40 seconds.
[ INFO] [1619201413.911797167]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1619201413.930729231]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1619201414.044219489]: odom received!
Let me know if you need anything else, I apologize for the formatting, it's my first question here.
Thanks in advance!
Update
The behavior has changed today, with the local_costmap not updating in the +X axis. I haven't changed any major parameter I believe, I am pasting the params and the video below.
https://drive.google.com/file/d/1KOBDqsh5SjLJyHtXDh2mVjNZn3CCqzv4/view?usp=sharing
rosparam get /move_base/
DWAPlannerROS: {acc_lim_theta: 2.0, acc_lim_trans: 0.1, acc_lim_x: 1.0, acc_lim_y: 0.0,
angular_sim_granularity: 0.1, forward_point_distance: 0.325, global_frame_id: odom,
goal_distance_bias: 24.0, max_scaling_factor: 0.2, max_vel_theta: 1.0, max_vel_trans: 0.5,
max_vel_x: 0.5, max_vel_y: 0.5, min_vel_theta: 0.4, min_vel_trans: 0.1, min_vel_x: 0.0,
min_vel_y: 0.0, occdist_scale: 0.5, oscillation_reset_angle: 0.2, oscillation_reset_dist: 0.05,
path_distance_bias: 64.0, prune_plan: false, publish_cost_grid_pc: true, publish_traj_pc: true,
restore_defaults: false, scaling_speed: 0.25, sim_granularity: 0.025, sim_time: 1.0,
stop_time_buffer: 0.2, theta_stopped_vel: 0.4, trans_stopped_vel: 0.0, twirling_scale: 0.0,
use_dwa: true, vth_samples: 20, vtheta_samples: 20, vx_samples: 6, vy_samples: 1,
xy_goal_tolerance: 0.15, yaw_goal_tolerance: 0.3}
GlobalPlanner: {allow_unknown: true, cost_factor: 3.0, default_tolerance: 0.0, lethal_cost: 253,
neutral_cost: 50, old_navfn_behavior: false, planner_costmap_publish_frequency: 0.0,
planner_window_x: 0.0, planner_window_y: 0.0, publish_potential: true, publish_scale: 100,
use_dijkstra: true, use_grid_path: false, use_quadratic: true}
NavfnROS: {allow_unknown: false, default_tolerance: 0.0, planner_window_x: 0.0, planner_window_y: 0.0,
visualize_potential: false}
aggressive_reset: {reset_distance: 1.84}
base_global_planner: navfn/NavfnROS
base_local_planner: dwa_local_planner/DWAPlannerROS
clearing_rotation_allowed: true
conservative_reset: {reset_distance: 3.0}
conservative_reset_dist: 3.0
controller_frequency: 5.0
controller_patience: 3.0
global_costmap:
footprint: '[]'
footprint_padding: 0.01
global_frame: map
height: 10
inflation_layer: {cost_scaling_factor: 5.0, enabled: true, inflate_unknown: false,
inflation_radius: 0.3}
obstacle_layer:
combination_method: 1
enabled: true
footprint_clearing_enabled: true
max_obstacle_height: 2.0
observation_sources: scan
obstacle_range: 5.0
raytrace_range: 10.0
scan: {clearing: true, data_type: LaserScan, marking: true, sensor_frame: base_link,
topic: /scan}
obstacle_range: 5.0
origin_x: 0.0
origin_y: 0.0
plugins:
- {name: static_layer, type: 'costmap_2d::StaticLayer'}
- {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
- {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
publish_frequency: 0.5
raytrace_range: 10.0
resolution: 0.05
robot_base_frame: base_link
robot_radius: 0.2
rolling_window: false
static_layer: {enabled: true, map_topic: map, subscribe_to_updates: true}
static_map: true
transform_tolerance: 0.5
update_frequency: 1.0
width: 10
local_costmap:
combination_method: 0
footprint: '[]'
footprint_padding: 0.01
global_frame: odom
height: 5
inflation_layer: {cost_scaling_factor: 5.0, enabled: true, inflate_unknown: false,
inflation_radius: 0.3}
obstacle_layer:
combination_method: 1
enabled: true
footprint_clearing_enabled: true
max_obstacle_height: 3.0
min_obstacle_height: 0
observation_sources: scan
obstacle_range: 5.0
raytrace_range: 10.0
scan: {clearing: true, data_type: LaserScan, marking: true, sensor_frame: base_link,
topic: /scan}
obstacle_range: 5.0
origin_x: 0.0
origin_y: 0.0
plugins:
- {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
- {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
publish_frequency: 5.0
raytrace_range: 10.0
resolution: 0.05
robot_base_frame: base_link
robot_radius: 0.2
rolling_window: true
static_map: false
transform_tolerance: 0.1
update_frequency: 5.0
width: 5
make_plan_add_unreachable_goal: true
make_plan_clear_costmap: true
max_planning_retries: -1
oscillation_distance: 0.2
oscillation_timeout: 10.0
planner_frequency: 1.0
planner_patience: 5.0
recovery_behavior_enabled: true
restore_defaults: false
shutdown_costmaps: false
Asked by s_niket on 2021-04-23 13:22:59 UTC
Answers
- /move_base/local_costmap/obstacle_layer/raytrace_range: 4.0
I think the obstacle is erased because raytrace_range is smaller than obstacle_range.
Update
- /move_base/local_costmap/combination_method: 0
Have you tried setting 1?
Asked by miura on 2021-04-24 20:28:01 UTC
Comments
Yeah, I tried different combinations and it failed even then. The combinations being scan_range > raytrace_range, scan_range >>> raytrace_range, scan_range < raytrace_range and scan_range <<< raytrace_range
Asked by s_niket on 2021-04-26 10:33:13 UTC
Yes I've tried with combination_method as 0 and 1
Asked by s_niket on 2021-04-26 11:07:59 UTC
Comments
The LaserScan topic name you are using is
/cat_dog/correcte...Is that correct? When you view the topic in rviz, does the right half of the topic show up?Asked by miura on 2021-04-24 02:25:54 UTC
Yes, my bad, I had edited the description here to change the /scan topic name. Yes, the lidar scan topic is correct and the right half does show up. If you look at the image I have linked, it shows that the right half has inflated obstacles from the global_costmap, but nothing in the local even though the obstacles are within the range of the local_costmap.
Asked by s_niket on 2021-04-24 09:21:26 UTC
I honestly can't predict it anymore. (I'm a little concerned that the frame_id is different between global_costmap and local_costmap, but I don't think this leads to the event in question. )
I have a suggestion for an approach to research. Why don't you try setting global_costmap and local_costmap to the exact same settings? You should get the same cost map. From there, change the values back one by one to the current values. If it is the same as the current state, then something is lurking there.
Asked by miura on 2021-04-27 10:43:14 UTC
Hey, thanks for all the support. I was able to identify the bug yesterday, and currently working on solving it. So I published the pointcloud from ObservationBuffer object's latest observation and the pointcloud was only half of the actual pointcloud. The bufferCloud method in the ObservationBuffer class does a transformation of the original cloud to a global_frame_cloud and that transformation right now is causing a loss in data. If I passed the un-transformed pointcloud to the ObstacleLayer class, I could see the entire pointcloud.
Asked by s_niket on 2021-04-27 11:18:04 UTC