Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

teb_local_planner planning low velocity trajectories

Hi, I am trying to use Teb local planner for my robot. But robot is moving very slow (0.1 to 0.23 m/sec) and with jerky movements. Although robot can easily go upto 1m/sec. I have tried changing value of weights but it had no impact. I have tried changing size (width and lenght) of local cost map from 8 to 20m, but it had no impact. I am using latest download for git for Kinetic version. I have also observed that sometime teb plans local trajectorise through obstacles in case of 20m x 20m size local map. image description

the configuration file for teb is as follows:

 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: True
 allow_init_with_backwards_motion: False
 max_global_plan_lookahead_dist: 10.0 #increased lookahead from 20 to 30
 feasibility_check_no_poses: 10

 # Robot

 max_vel_x: 1.0
 max_vel_x_backwards: 0.12 #reduced from 0.2
 max_vel_y: 0.0
 max_vel_theta: 0.35
 acc_lim_x: 0.2
 acc_lim_theta: 0.20
 min_turning_radius: 0.0 # diff-drive robot (can turn on place!)

 footprint_model:
   type: "circular"
   radius: 0.35

 # GoalTolerance

 xy_goal_tolerance: 1
 yaw_goal_tolerance: 1.2
 free_goal_vel: true

 # Obstacles

 min_obstacle_dist: 0.1 # This value must also include our robot radius, if footprint_model is set to "point".
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0
 obstacle_poses_affected: 10
 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 #5
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1 #1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 100 #100
 weight_kinematics_forward_drive: 1 #1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet
 weight_adapt_factor: 2

costmap_common_params.yaml: #---(in meters)--- robot_radius: 0.35 footprint_padding: 0.05 transform_tolerance: 1.5 map_type: costmap obstacle_layer: enabled: true # expected_update_rate: 0.05 #enough margin for 50Hz laser obstacle_range: 10.0 raytrace_range: 10.0 inflation_radius: 0.25 track_unknown_space: false combination_method: 1 inf_is_valid: true observation_sources: laser_scan_sensor laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true}

inflation_layer:
  enabled:              true
  cost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     0.25  # max. distance from an obstacle at which costs are incurred for planning paths.
    static_layer:
  enabled:              true
  map_topic:            "map"

Local costmap params:

local_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 10.0
  publish_frequency: 1.0
  static_map: false
  rolling_window: true
  width: 8
  height: 8
  resolution: 0.05
  transform_tolerance: 0.5

  plugins:
#   - {name: static_layer,        type: "costmap_2d::StaticLayer"}
   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
   - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

global costmap params:

global_costmap:

  global_frame: map
  robot_base_frame: base_link
  update_frequency: 0.25
  publish_frequency: 0.5
  static_map: true
  rolling_window: false
  height: 50
  width: 50
  transform_tolerance: 0.5
  plugins:
    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,          type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

movebase launch file

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <remap from="cmd_vel" to="delta_robot/cmd_vel"/>
    <rosparam file="nav/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="nav/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="nav/local_costmap_params.yaml" command="load" />
    <rosparam file="nav/global_costmap_params.yaml" command="load" />

    <param name="base_global_planner" value="global_planner/GlobalPlanner" />
    <param name="planner_frequency" value="1.0" />
    <param name="planner_patience" value="5.0" />

    <rosparam file="nav/teb_local_planner_params.yaml" command="load" />
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
    <param name="controller_frequency" value="5.0" />
    <param name="controller_patience" value="15.0" />
</node>

Any ideas on what could go wrong.

On move_base console following errors were observed:

[ERROR] [1500245757.698989340]: NO PATH!
[ERROR] [1500245757.699021166]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ WARN] [1500245796.845504913]: TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).
[ERROR] [1500245805.857276353]: NO PATH!
[ERROR] [1500245805.857315590]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ INFO] [1500245807.845543897]: TebLocalPlannerROS: oscillation recovery disabled/expired.
[ WARN] [1500245920.845472769]: TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).
[ERROR] [1500245950.966385275]: NO PATH!
[ERROR] [1500245950.966428527]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ERROR] [1500245954.969274775]: NO PATH!
[ERROR] [1500245954.969317902]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ERROR] [1500245955.969465935]: NO PATH!
[ERROR] [1500245955.969508893]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ERROR] [1500245956.969964978]: NO PATH!
[ERROR] [1500245956.969994493]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ INFO] [1500245969.645546998]: TebLocalPlannerROS: oscillation recovery disabled/expired.
[ WARN] [1500245972.050677807]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1500245975.050173891]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1500245975.850364335]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1500245979.645506342]: TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).
[ERROR] [1500245997.046909984]: Failed to get a plan.
[ERROR] [1500245997.246924446]: Failed to get a plan.
[ERROR] [1500245997.446904198]: Failed to get a plan.
[ERROR] [1500245997.646913991]: Failed to get a plan.

The following errors are from global planner plugin. I am using navfn as global planner. What could be reason for the following error:

Thanks nitin

teb_local_planner planning low velocity trajectories

Hi, I am trying to use Teb local planner for my robot. But robot is moving very slow (0.1 to 0.23 m/sec) and with jerky movements. Although robot can easily go upto 1m/sec. I have tried changing value of weights but it had no impact. I have tried changing size (width and lenght) of local cost map from 8 to 20m, but it had no impact. I am using latest download for git for Kinetic version. I have also observed that sometime teb plans local trajectorise through obstacles in case of 20m x 20m size local map. image description

the configuration file for teb is as follows:

 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: True
 allow_init_with_backwards_motion: False
 max_global_plan_lookahead_dist: 10.0 #increased lookahead from 20 to 30
 feasibility_check_no_poses: 10

 # Robot

 max_vel_x: 1.0
 max_vel_x_backwards: 0.12 #reduced from 0.2
 max_vel_y: 0.0
 max_vel_theta: 0.35
 acc_lim_x: 0.2
 acc_lim_theta: 0.20
 min_turning_radius: 0.0 # diff-drive robot (can turn on place!)

 footprint_model:
   type: "circular"
   radius: 0.35

 # GoalTolerance

 xy_goal_tolerance: 1
 yaw_goal_tolerance: 1.2
 free_goal_vel: true

 # Obstacles

 min_obstacle_dist: 0.1 # This value must also include our robot radius, if footprint_model is set to "point".
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0
 obstacle_poses_affected: 10
 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 #5
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1 #1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 100 #100
 weight_kinematics_forward_drive: 1 #1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet
 weight_adapt_factor: 2

costmap_common_params.yaml: #---(in meters)--- robot_radius: 0.35 footprint_padding: 0.05 transform_tolerance: 1.5 map_type: costmap obstacle_layer: enabled: true # expected_update_rate: 0.05 #enough margin for 50Hz laser obstacle_range: 10.0 raytrace_range: 10.0 inflation_radius: 0.25 track_unknown_space: false combination_method: 1 inf_is_valid: true observation_sources: laser_scan_sensor laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true}

inflation_layer:
  enabled:              true
  cost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     0.25  # max. distance from an obstacle at which costs are incurred for planning paths.
    static_layer:
  enabled:              true
  map_topic:            "map"

Local costmap params:

local_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 10.0
  publish_frequency: 1.0
  static_map: false
  rolling_window: true
  width: 8
  height: 8
  resolution: 0.05
  transform_tolerance: 0.5

  plugins:
#   - {name: static_layer,        type: "costmap_2d::StaticLayer"}
   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
   - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

global costmap params:

global_costmap:

  global_frame: map
  robot_base_frame: base_link
  update_frequency: 0.25
  publish_frequency: 0.5
  static_map: true
  rolling_window: false
  height: 50
  width: 50
  transform_tolerance: 0.5
  plugins:
    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,          type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

movebase launch file

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <remap from="cmd_vel" to="delta_robot/cmd_vel"/>
    <rosparam file="nav/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="nav/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="nav/local_costmap_params.yaml" command="load" />
    <rosparam file="nav/global_costmap_params.yaml" command="load" />

    <param name="base_global_planner" value="global_planner/GlobalPlanner" />
    <param name="planner_frequency" value="1.0" />
    <param name="planner_patience" value="5.0" />

    <rosparam file="nav/teb_local_planner_params.yaml" command="load" />
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
    <param name="controller_frequency" value="5.0" />
    <param name="controller_patience" value="15.0" />
</node>

Any ideas on what could go wrong.

On move_base console following errors were observed:

[ERROR] [1500245757.698989340]: NO PATH!
[ERROR] [1500245757.699021166]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ WARN] [1500245796.845504913]: TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).
[ERROR] [1500245805.857276353]: NO PATH!
[ERROR] [1500245805.857315590]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ INFO] [1500245807.845543897]: TebLocalPlannerROS: oscillation recovery disabled/expired.
[ WARN] [1500245920.845472769]: TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).
[ERROR] [1500245950.966385275]: NO PATH!
[ERROR] [1500245950.966428527]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ERROR] [1500245954.969274775]: NO PATH!
[ERROR] [1500245954.969317902]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ERROR] [1500245955.969465935]: NO PATH!
[ERROR] [1500245955.969508893]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ERROR] [1500245956.969964978]: NO PATH!
[ERROR] [1500245956.969994493]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ INFO] [1500245969.645546998]: TebLocalPlannerROS: oscillation recovery disabled/expired.
[ WARN] [1500245972.050677807]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1500245975.050173891]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1500245975.850364335]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1500245979.645506342]: TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).
[ERROR] [1500245997.046909984]: Failed to get a plan.
[ERROR] [1500245997.246924446]: Failed to get a plan.
[ERROR] [1500245997.446904198]: Failed to get a plan.
[ERROR] [1500245997.646913991]: Failed to get a plan.

The following errors are from global planner plugin. I am using navfn as global planner. What could be reason for the following error:

[ERROR] [1500245955.969508893]: Failed to get a plan from potential when a legal potential was found.

Thanks nitin

teb_local_planner planning low velocity trajectories

Hi, I am trying to use Teb local planner for my robot. But robot is moving very slow (0.1 to 0.23 m/sec) and with jerky movements. Although robot can easily go upto 1m/sec. I have tried changing value of weights but it had no impact. I have tried changing size (width and lenght) of local cost map from 8 to 20m, but it had no impact. I am using latest download for git for Kinetic version. I have also observed that sometime teb plans local trajectorise through obstacles in case of 20m x 20m size local map. image description

the configuration file for teb is as follows:

 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: True
 allow_init_with_backwards_motion: False
 max_global_plan_lookahead_dist: 10.0 #increased lookahead from 20 to 30
 feasibility_check_no_poses: 10

 # Robot

 max_vel_x: 1.0
 max_vel_x_backwards: 0.12 #reduced from 0.2
 max_vel_y: 0.0
 max_vel_theta: 0.35
 acc_lim_x: 0.2
 acc_lim_theta: 0.20
 min_turning_radius: 0.0 # diff-drive robot (can turn on place!)

 footprint_model:
   type: "circular"
   radius: 0.35

 # GoalTolerance

 xy_goal_tolerance: 1
 yaw_goal_tolerance: 1.2
 free_goal_vel: true

 # Obstacles

 min_obstacle_dist: 0.1 # This value must also include our robot radius, if footprint_model is set to "point".
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0
 obstacle_poses_affected: 10
 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 #5
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1 #1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 100 #100
 weight_kinematics_forward_drive: 1 #1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet
 weight_adapt_factor: 2

costmap_common_params.yaml: costmap_common_params.yaml:

#---(in meters)---
 robot_radius: 0.35
 footprint_padding: 0.05
     transform_tolerance: 1.5
 map_type: costmap
     obstacle_layer:
  enabled: true
 # expected_update_rate: 0.05 #enough margin for 50Hz laser
  obstacle_range: 10.0
  raytrace_range: 10.0
  inflation_radius: 0.25
  track_unknown_space: false
  combination_method: 1
  inf_is_valid: true
  observation_sources: laser_scan_sensor
  laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true}

true}

inflation_layer:
  enabled:              true
  cost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     0.25  # max. distance from an obstacle at which costs are incurred for planning paths.
    static_layer:
  enabled:              true
  map_topic:            "map"

Local costmap params:

local_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 10.0
  publish_frequency: 1.0
  static_map: false
  rolling_window: true
  width: 8
  height: 8
  resolution: 0.05
  transform_tolerance: 0.5

  plugins:
#   - {name: static_layer,        type: "costmap_2d::StaticLayer"}
   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
   - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

global costmap params:

global_costmap:

  global_frame: map
  robot_base_frame: base_link
  update_frequency: 0.25
  publish_frequency: 0.5
  static_map: true
  rolling_window: false
  height: 50
  width: 50
  transform_tolerance: 0.5
  plugins:
    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,          type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

movebase launch file

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <remap from="cmd_vel" to="delta_robot/cmd_vel"/>
    <rosparam file="nav/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="nav/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="nav/local_costmap_params.yaml" command="load" />
    <rosparam file="nav/global_costmap_params.yaml" command="load" />

    <param name="base_global_planner" value="global_planner/GlobalPlanner" />
    <param name="planner_frequency" value="1.0" />
    <param name="planner_patience" value="5.0" />

    <rosparam file="nav/teb_local_planner_params.yaml" command="load" />
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
    <param name="controller_frequency" value="5.0" />
    <param name="controller_patience" value="15.0" />
</node>

Any ideas on what could go wrong.

On move_base console following errors were observed:

[ERROR] [1500245757.698989340]: NO PATH!
[ERROR] [1500245757.699021166]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ WARN] [1500245796.845504913]: TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).
[ERROR] [1500245805.857276353]: NO PATH!
[ERROR] [1500245805.857315590]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ INFO] [1500245807.845543897]: TebLocalPlannerROS: oscillation recovery disabled/expired.
[ WARN] [1500245920.845472769]: TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).
[ERROR] [1500245950.966385275]: NO PATH!
[ERROR] [1500245950.966428527]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ERROR] [1500245954.969274775]: NO PATH!
[ERROR] [1500245954.969317902]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ERROR] [1500245955.969465935]: NO PATH!
[ERROR] [1500245955.969508893]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ERROR] [1500245956.969964978]: NO PATH!
[ERROR] [1500245956.969994493]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ INFO] [1500245969.645546998]: TebLocalPlannerROS: oscillation recovery disabled/expired.
[ WARN] [1500245972.050677807]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1500245975.050173891]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1500245975.850364335]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1500245979.645506342]: TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).
[ERROR] [1500245997.046909984]: Failed to get a plan.
[ERROR] [1500245997.246924446]: Failed to get a plan.
[ERROR] [1500245997.446904198]: Failed to get a plan.
[ERROR] [1500245997.646913991]: Failed to get a plan.

The following errors are from global planner plugin. I am using navfn as global planner. What could be reason for the following error:

[ERROR] [1500245955.969508893]: Failed to get a plan from potential when a legal potential was found.

Thanks nitin