Problem with load and tune navigation stack params
Hi, I'm using the navigation stack of ROS with Rviz to autonomous navigation of my dual drive robot.
I'm tring to tune costmap and planner's params but I the robot is not able to avoid obstacles and walls.
For example, the global planner create a good path to follow but the local plane doesn't follow it, also tuning path_distance_bias
.
And I don't know why sometimes it reach the goal but other times it doesn't start to "walk" and after that the SW starts with in place rotation or recovery action.
Sometimes I find Trajectory Planner more reliable than DWAPlanner but It doesn't avoid obstacles.
This is my move_base launcher:
<launch>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find robot)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find robot)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find robot)/param/global_costmap_params.yaml" command="load" />
<rosparam file="$(find robot)/param/local_costmap_params.yaml" command="load" />
<!--I tried to comment and uncomment this for selecting Planner-->
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
<param name="base_global_planner" value="navfn/NavfnROS"/>
<rosparam file="$(find robot)/param/trajectory_planner_params.yaml" command="load" />
<rosparam file="$(find robot)/param/dwa_local_planner_params.yaml" command="load" />
<remap from="cmd_vel" to="/cmd_vel"/>
<remap from="odom" to="odom"/>
</node>
</launch>
This is my costmapcommon_params:
obstacle_range: 2.5 #aggiornamento della mappa solo all'interno di 2.5 metri, quindi gli ostacoli vengono considerati solo entro 2.5 metri
raytrace_range: 3.5 #spazio libero davanti al robot
# Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation)
robot_radius: 0.25 # forma che visualizzo in rviz - distance a circular robot should be clear of the obstacle (kobuki: 0.18)
inflation_radius: 1.0 #distanza massima da un ostacolo che mi minimizza un certo costo: ovvero tutti i percorsi che si trovoano a distanza di un metro da un ostacolo hanno costi uguali tra loro
cost_scaling_factor: 3.0 # maggiore è il valore, minore sara' il costo della mappa (default: 10)
subscribe_to_update: true #aggiorna la mappa
map_type: costmap #mappa dei costi
observation_source: scan #tipo di sensore che passa i dati alla mappa nel nostro caso il LaserScan
scan: {sensor_frame: camera_link, data_type: LaserScan, topic: scan, marking: true, clearing: true}
This is my global costmap param:
global_costmap:
global_frame: map
robot_base_frame: base_footprint
update_frequency: 1.0
publish_frequency: 0.5
static_map: true
transform_tolerance: 0.5
This is my local costmap param:
local_costmap:
global_frame: map
robot_base_frame: base_footprint
update_frequency: 5.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.05
transform_tolerance: 0.5
This is my dwa planner params:
DWAPlannerROS:
# Robot Configuration Parameters - Kobuki
max_vel_x: 0.45 # 0.55
min_vel_x: 0.0
max_vel_y: 0.0 # diff drive robot
min_vel_y: 0.0 # diff drive robot
max_vel_trans: 0.45 # choose slightly less than the base's capability
min_vel_trans: 0.1 # this is the min trans velocity when there is negligible rotational velocity
trans_stopped_vel: 0.1
# Warning!
# do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
# are non-negligible and small in place rotational velocities will be created.
max_vel_theta: 1.0 # choose slightly less than the base's capability
min_vel_theta: 0.4 # this is the min angular velocity when there is negligible translational velocity
theta_stopped_vel: 0.4
acc_lim_x: 1.5 # maximum is theoretically 2.0, but we
acc_lim_theta: 2.0
acc_lim_y: 0.0 # diff drive robot
# Goal Tolerance Parameters
yaw_goal_tolerance: 0.15 # 0.05
xy_goal_tolerance: 0.15 # 0.10
# latch_xy_goal_tolerance: false
# Forward Simulation Parameters
sim_time: 2.0 # 1.7
vx_samples: 20 # 3
vy_samples: 1 # diff drive robot, there is only one sample
vtheta_samples: 20 # 20
# Trajectory Scoring Parameters
path_distance_bias: 8.0 # 32.0 - weighting for how much it should stick to the global path plan
goal_distance_bias: 0.8 # 24.0 - wighting for how much it should attempt to reach its goal
occdist_scale: 0.30 # 0.01 - weighting for how much the controller should avoid obstacles
forward_point_distance: 0.325 # 0.325 - how far along to place an additional scoring point
stop_time_buffer: 0.2 # 0.2 - amount of time a robot must stop in before colliding for a valid traj.
scaling_speed: 0.25 # 0.25 - absolute velocity at which to start scaling the robot's footprint
max_scaling_factor: 0.2 # 0.2 - how much to scale the robot's footprint when at speed.
# Oscillation Prevention Parameters
oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags
# Debugging
publish_traj_pc : true
publish_cost_grid_pc: true
global_frame_id: odom
# Differential-drive robot configuration - necessary?
# holonomic_robot: false
This is my trajectory plannes params:
TrajectoryPlannerROS:
# Robot Configuration Parameters
acc_lim_x: 0.8
acc_lim_theta: 1.67
max_vel_x: 0.5
min_vel_x: 0.2
max_vel_theta: 1.0
min_vel_theta: -1.0
min_in_place_vel_theta: 0.4#0.4
holonomic_robot: false
escape_vel: -0.1
# Goal Tolerance Parameters
yaw_goal_tolerance: 0.2
xy_goal_tolerance: 0.3
latch_xy_goal_tolerance: false
# Forward Simulation Parameters
sim_time: 2.0
sim_granularity: 0.02
angular_sim_granularity: 0.02
vx_samples: 10 #Antes estaba a 6, asi puede necesitar mas procesamiento pero ira mejor
vtheta_samples: 20
controller_frequency: 10.0
# Trajectory scoring parameters
meter_scoring: true # Whether the gdist_scale and pdist_scale parameters should assume that goal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default (false).
occdist_scale: 0.1 #The weighting for how much the controller should attempt to avoid obstacles. default 0.01
pdist_scale: 0.6 # The weighting for how much the controller should stay close to the path it was given . default 0.6
gdist_scale: 0.8 # The weighting for how much the controller should attempt to reach its local goal, also controls speed default 0.8
heading_lookahead: 0.325 #How far to look ahead in meters when scoring different in-place-rotation trajectories
heading_scoring: false #Whether to score based on the robot's heading to the path or its distance from the path. default false
heading_scoring_timestep: 0.8 #How far to look ahead in time in seconds along the simulated trajectory when using heading scoring (double, default: 0.8)
dwa: false #Whether to use the Dynamic Window Approach (DWA)_ or whether to use Trajectory Rollout
simple_attractor: false
publish_cost_grid_pc: true
I tried to tune all of these params but I can't obtain a good result of navigation also reading a lot of guide for tuning. And sometimes these params are not loaded so I have to terminate "roscore" and restart it.
Can you check these params and give me some tips? Thanks
Asked by FabioZ96 on 2019-09-10 05:29:17 UTC
Comments