navfn stability
Hi.
I encountered with the problem that global_planner in navfn package makes invalid plans. It means that they go through walls on a map like on this picture.
There is ROS Electric on board.
Cannot find out why it happens, so I'd appreciate any suggestions.
UPD. I tried with original navigation stack (as well as with forked base_local_planner and move_base) and it still makes plans through the walls sometimes.
Is it possible that it's a bug in navfn package?
Is there any others global planners?
Here are my configs:
1. base_local_planner_params
controller_frequency: 5.0
TrajectoryPlannerROS:
max_vel_x: 0.70
min_vel_x: 0.1
max_rotational_vel: 1.5
min_in_place_rotational_vel: 1.0
acc_lim_th: 0.75
acc_lim_x: 0.50
acc_lim_y: 0.35
holonomic_robot: false
yaw_goal_tolerance: 1.1
xy_goal_tolerance: 0.15
goal_distance_bias: 0.8
path_distance_bias: 0.6
sim_time: 1.5
heading_lookahead: 0.325
oscillation_reset_dist: 0.05
vx_samples: 6
vtheta_samples: 20
dwa: false
2. costmap_common_params
obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[-0.35, -0.245], [0.35,-0.245], [0.35,0.245], [-0.35, 0.245]]
footprint_padding: 0.01
inflation_radius: 1.0
observation_sources: filter
filter: {data_type: LaserScan, topic: /filter, marking: true, clearing: true}
3 global_costmap_params
global_costmap:
global_frame: /map
robot_base_frame: /base_link
update_frequency: 3.0
publish_frequency: 0.0
static_map: true
transform_tolerance: 0.5
4 local_costmap_params
local_costmap:
global_frame: /odom
robot_base_frame: /base_link
update_frequency: 5.0
publish_frequency: 5.0
static_map: false
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.1
transform_tolerance: 0.5
UPD2.
5 BaseLocalPlanner.cfg
# gen.add("inscribed_radius", double_t, 0, "The radius of the inscribed circle of the robot", 1, 0)
# gen.add("circumscribed_radius", double_t, 0, "The radius of the circumscribed circle of the robot", 1, 0)
gen.add("acc_lim_x", double_t, 0, "The acceleration limit of the robot in the x direction", 1.0, 0, 20.0)
gen.add("acc_lim_y", double_t, 0, "The acceleration limit of the robot in the y direction", 1.0, 0, 20.0)
gen.add("acc_lim_theta", double_t, 0, "The acceleration limit of the robot in the theta direction", 1.0, 0, 20.0)
gen.add("max_vel_x", double_t, 0, "The maximum x velocity for the robot in m/s", 0.55, 0, 20.0)
gen.add("min_vel_x", double_t, 0, "The minimum x velocity for the robot in m/s", 0.0, 0, 20.0)
gen.add("max_vel_theta", double_t, 0, "The absolute value of the maximum rotational velocity for the robot in rad/s", 1.0, 0, 20.0)
gen.add("min_vel_theta", double_t, 0, "The absolute value of the minimum rotational velocity for the robot in rad/s", -1.0, 0, 20.0)
gen.add("min_in_place_vel_theta", double_t, 0, "The absolute value of the minimum in-place rotational velocity the controller will explore", 0.4, 0, 20.0)
gen.add("sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 3, 0, 10)
gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0, 5)
gen.add("pdist_scale", double_t, 0, "The weight for the path distance part of the cost function ...
This should never happen and I've never seen it happen. I suspect it's probably more of a setup issue. Could you add your configuration?
Can't see anything obviously wrong, besides maybe the publish_frequency of 0.0. I can't look it up right now as ros.org is down. One other thing: Your rviz display seem not to correspond to each other at all. Can you explain that? Maybe something is wrong with tf/frames?
Sorry, did not get what is not corresponded? And one more clarification. As far as I understood navfn algorithm works standalone and my base_local_planner hacks will not affect global_planner. For example I've changed the weights for local trajectories.
The pink cells you display are the plan, etc. right? It doesn't seem to match anything (the map, the current robot pose). That seems odd. Yes, you understood this correctly. Changing things in the local planner is irrelevant for the global planner.
Yes, the pink cells are inflated obstacles. The green one are lethal_ostacles recognized by kinect. The red are laser scans. I think that they are dismatched with the static map a little because of errors in the navigation system.
OK, so this might only be for the local planner. What is the green plan then? Is that maybe from the local planner? This would explain everything. Given the obstacles you display the green plan is correct. Unfortunately this local planner isn't bound to the global plan and makes its own decisions.
If it doesn't consider the map the observations are consistent. Can you check which plan you display? Usually there are three different topics.
The green plan is the global plan computed by navfn. One interesting thing: the first plan is always correct. This problem popped up when I tried cope with oscillations of my robot. So I hacked move_base a little to remake the global plan once a minute.