ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
2

navigation stack with teb_local_planner planning path through wall

asked 2017-05-24 08:11:41 -0600

JamesDoe gravatar image

updated 2017-05-31 06:49:27 -0600

Hello! I'm currently trying to use teb_local_planner to get my robot to navigate through the willow garage simulation. However, I have the issue that in specific situations the local path planning attemtps to drive through the wall. Ofcourse, this doesn't work and it figures that out quite quickly. After a while though, it attempts to go through the wall again. This sometimes loops for over 2 minutes. I have a video where you can see it happen two times.

I'm using mostly the default settings from navigation stack as you can see in the code-quotes below. Can anyone help me figure out why the local costmap is trying to drive through walls sometimes (and gets stuck in a loop of switching in between trying to drive through wall and the actual route it should take)

launch file:

<launch>
<!-- Run the map server -->
<node name="gmapping" pkg="gmapping" type="slam_gmapping"/>

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find my_robot_name_2dnav)/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find my_robot_name_2dnav)/costmap_common_params.yaml" command="load" ns="local_costmap"/>
   <rosparam file="$(find my_robot_name_2dnav)/local_costmap_params.yaml" command="load" />
   <rosparam file="$(find my_robot_name_2dnav)/global_costmap_params.yaml" command="load" />
   <rosparam file="$(find my_robot_name_2dnav)/base_local_planner_params.yaml" command="load" />

   <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
   <param name="controller_frequency" value="4.0" />
 </node>
</launch>

costmap_common_params.yaml

obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[-0.20, -0.20], [-0.20, 0.20], [0.20, 0.20], [0.20, -0.20]]
#robot_radius: ir_of_robot
inflation_radius: 1

observation_sources: scan
scan: {data_type: LaserScan, expected_update_rate: 0.4,
observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08}

cost_scaling_factor: 1

base_local_planner_params.yaml

TebLocalPlannerROS:

odom_topic: odom

# Trajectory

teb_autosize: True
dt_ref: 0.3
dt_hysteresis: 0.1
global_plan_overwrite_orientation: True
max_global_plan_lookahead_dist: 3.0
feasibility_check_no_poses: 5

# Robot

max_vel_x: 0.4
max_vel_x_backwards: 0.05 #0.2 by default
max_vel_theta: 0.3
acc_lim_x: 0.5
acc_lim_theta: 0.5
min_turning_radius: 0.0 # diff-drive robot (can turn on place!)

footprint_model:
  type: "point"

# GoalTolerance

xy_goal_tolerance: 0.2
yaw_goal_tolerance: 0.1
free_goal_vel: False

# Obstacles

min_obstacle_dist: 0.25 # This value must also include our robot radius, since footprint_model is set to "point".
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.0
obstacle_poses_affected: 30
costmap_converter_plugin: ""
costmap_converter_spin_thread: True
costmap_converter_rate: 5

# Optimization

no_inner_iterations: 5
no_outer_iterations: 4
optimization_activate: True
optimization_verbose: False
penalty_epsilon: 0.1
weight_max_vel_x: 2
weight_max_vel_theta: 1
weight_acc_lim_x: 1
weight_acc_lim_theta: 1
weight_kinematics_nh: 1000
weight_kinematics_forward_drive: 1
weight_kinematics_turning_radius: 1
weight_optimaltime: 1
weight_obstacle: 50
weight_dynamic_obstacle: 10 # not in use yet

# Homotopy Class Planner

enable_homotopy_class_planning: True
enable_multithreading: True
simple_exploration: False
max_number_classes: 4
selection_cost_hysteresis: 1.0
selection_obst_cost_scale: 1.0
selection_alternative_time_cost: False

roadmap_graph_no_samples: 15
roadmap_graph_area_width: 5
h_signature_prescaler: 0.5
h_signature_threshold: 0.1
obstacle_keypoint_offset: 0.1

global_costmap_params.yaml

 global_costmap:
 global_frame: /map
 robot_base_frame: base_link
 update_frequency: 5.0
 static_map: true

local_costmap_params.yaml

local_costmap:
 global_frame: odom
 robot_base_frame: base_link
 update_frequency: 5.0
 publish_frequency: 2.0
 static_map: false
 rolling_window: true
 width: 6.0
 height: 6.0
 resolution: 0.05
edit retag flag offensive close merge delete

2 Answers

Sort by » oldest newest most voted
1

answered 2017-06-02 08:04:43 -0600

janedoe gravatar image

By default the value of costmap_obstacles_behind_robot_dist is set to 1 meter, this wil cause some weird behaviour. I suggest changing the value of this parameter to something like 5 maybe 6 meters. Let me know if this works!

edit flag offensive delete link more

Comments

What a coincidence! I actually found this and tried this last friday at about 4pm. Hadn't actually reported back yet, but this is indeed the fix. Thank you! :)

JamesDoe gravatar image JamesDoe  ( 2017-06-06 02:11:04 -0600 )edit

I don't really understand what this parameter represents or how does the algorithm use it? Any more insight? Why would this trigger the plan-through-walls behavior?

bergercookie gravatar image bergercookie  ( 2020-09-14 09:27:13 -0600 )edit
0

answered 2017-06-01 01:43:08 -0600

Procópio gravatar image

updated 2017-06-01 01:49:13 -0600

Try to model the footprint as a circle instead, as explained here.

if that solutions does not solve the problem, it would help if you add a video where you show the local costmap markings and/or the TEB markers.

edit flag offensive delete link more

Comments

Haven't tried making the footprint a circle yet. I did make a video of local costmap and the TEB markers. Video

Will try changing my robot from a point to a square later (since my eventual robot will be squared)

JamesDoe gravatar image JamesDoe  ( 2017-06-01 08:19:29 -0600 )edit

Okay, so I just tried making it a square of several sizes. Once with 0.2 by 0.2 and once with 0.02 by 0.02 (actual local costmap size is 0.2 x 0.2). Both still have the same issues. Whereas 0.2 x 0.2 actually has issues just getting through the door, but it also still tried to go through the wall.

JamesDoe gravatar image JamesDoe  ( 2017-06-02 06:28:18 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2017-05-24 08:11:41 -0600

Seen: 2,174 times

Last updated: Jun 01 '17