Local costmap not updating in Nav2
Hi!
I am trying to map and localize my robot in its environment using RTABMAP with an RGBD camera + Nav2. I have no problem with the global costmap, as it works perfectly, however, my local costmap seems to not update properly, since sometimes the obstacles appear in the local costmap and sometimes not (comparing to the global costmap).
Here you can see an image of my problem:
Here is my yaml configuration for Nav2:
bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
odom_topic: /odom/unfiltered
enable_groot_monitoring: True
groot_zmq_publisher_port: 1666
groot_zmq_server_port: 1667
default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_back_up_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_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_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_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_distance_traveled_condition_bt_node
- nav2_navigate_to_pose_action_bt_node
bt_navigator_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server:
ros__parameters:
use_sim_time: True
odom_topic: /odom/unfiltered
controller_frequency: 5.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.001
min_theta_velocity_threshold: 0.001
progress_checker_plugin: "progress_checker"
goal_checker_plugin: "goal_checker"
controller_plugins: ["FollowPath"]
# Progress checker parameters
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.2
movement_time_allowance: 20.0
# Goal checker parameters
goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
stateful: True
# DWB parameters
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: True
publish_cost_grid_pc: True
min_vel_x: 0.0
min_vel_y: 0.0
max_vel_x: 4.0
max_vel_y: 4.0
max_vel_theta: 4.5
min_speed_xy: 0.4
max_speed_xy: 4.0
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: 10
vtheta_samples: 20
sim_time: 1.2
linear_granularity: 0.05
time_granularity: 1.0
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", "ObstacleFootprint"]
BaseObstacle.scale: 0.02
ObstacleFootprint.scale: 32.0
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
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: world
robot_base_frame: base_link
use_sim_time: True
rolling_window: true
width: 3
height: 3
origin_x: -1.5
origin_y: -1.5
resolution: 0.05
always_send_full_costmap: True
robot_radius: 0.4
plugins: ["obstacle_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.1
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: pointcloud
pointcloud:
topic: /camera/points
max_obstacle_height: 10.0
min_obstacle_height: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
raytrace_max_range: 10.0
raytrace_min_range: 0.0
clearing: True
marking: True
data_type: "PointCloud2"
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: pointcloud
pointcloud:
topic: /camera/points
max_obstacle_height: 10.0
min_obstacle_height: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
raytrace_max_range: 10.0
raytrace_min_range: 0.0
clearing: True
marking: True
data_type: "PointCloud2"
local_costmap_client:
ros__parameters:
use_sim_time: True
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: 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: True
robot_radius: 0.4
resolution: 0.05
width: 3
height: 3
track_unknown_space: true ...
Can you write more about situations where you think you should have obstacles appearing? Why do you think they were incorrectly updated? I read your YAML configs and I didn't find any error or questionable parameter values
The thing is that the local costmap isn't matching the global costmap (if you see, the obstacle layer of the local costamp, which is brighter, doesn't allways cover the obstacle layer of the global costmap, which is darker).
When running in mapping mode, the global costmap keeps growing as so does the occupancygrid of the map being built, but the local costmap sometimes shows info about the walls, sometimes doesn't (The walls are always there).
I want the local costmap to be capturing the same info as the global costmap, but in a smaller plane that moves with the robot.
Moreover, I transformed the info provided by my depth camera sensor into a laser scan using depthimage_to_laserscan (and changed the corresponding observation sources in the layers of my costmap) and I am still getting the same behaviour. When the laser detects an obstacle, sometimes it gets printed in the obstacle layer of the local costmap and sometimes it doesn't.
Maybe you can try with toying with
observation_persistence
/inf_is_valid
obstacle parameters? Maybe you can have smaller inflate in the inflation layer too? Do you use this tuning tutorial?I tried both of them but still having the same problem... I followed the tuning tutorial too and inflated the costmap very little (as I am moving in a cave with very little space).
The thing is that the global costmap is working perfectly fine and updates the obstacles when it should in the mapping mode, but the local costmap, with almost the same config doesn't.
Other thing that I have detected is that when going to the wall of the right after being close to the left one or viceversa, the local costmap keeps drawing obstacles on the previous wall as if it had some kind of delay... Could the problem be something related to it?
I think this part about inflation potential fields contradicts a bit your approach to a little inflation in the cost map. Local map updates in real time with new information from the RGB-D camera, maybe the "clearing" factor can be adjusted a bit? So that only 1-2 scans would clear the obstacles in the local cost map? It can be these artifacts are your ghost obstacles
How do you change the clearing factor? Although the problem is that neither the global nor the local costmap detect the obstacle in navigation mode when placed it after the mapping mode, but when running the mapping mode using RTABMap the map is able to detect that obstacle if it is placed... I really don't know why is this... Does it have to do with the layers used?
I will correct my inflation layer too! Thank you mate!
I understand parameters like
lethal_cost_threshold
set higher, same with update frequencies, and in others:<voxel layer>.footprint_clearing_enabled
,<voxel layer>. <data source>.clearing
and similar - there is no single "clearing factor", of course.