hi, dear @croesmann , i test your local planner with stage, and find sometimes the local trajectory will cross the wall in stage, and in terminal it shows "TebLocalPlannerROS: trajectory is not feasible. Resetting planner..." and will abort finally.
please see below:

my parameter config is:

[ INFO] [1465295128.429697743, 456.200000000]: GOAL Reached!
[ WARN] [1465295133.355418858, 461.100000000]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1465295134.000557437, 461.700000000]: TebLocalPlannerROS: trajectory is not feasible, but the planner suggests a shorter horizon temporary. Complying with its wish for at least 10s...
[ INFO] [1465295144.194776376, 471.900000000]: Switching back to full horizon length.
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
[ WARN] [1465295163.258694130, 491.000000000]: TebLocalPlannerROS: trajectory is not feasible, but the planner suggests a shorter horizon temporary. Complying with its wish for at least 10s...
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int_): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
int g2o::csparse_extension::cs_cholsolsymb(const cs_di_, double_, const cs_dis_, double_, int*): cholesky failed!
Cholesky failure, writing debug.txt (Hessian loadable by Octave)
[ INFO] [1465295173.290844396, 501.000000000]: Switching back to full horizon length.
[ INFO] [1465295176.760568540, 504.500000000]: GOAL Reached!

when I set aim is in the middle of two block obstacle. but finally goal reached at last line above.

Thanks!

Just downloaded the planner today but I cannot get it to start. Complains about not finding a library, even though the library path env variable is correctly configured

echo $LD_LIBRARY_PATH
/opt/ros/kinetic/lib

This is the error I get:

[FATAL] [1486459900.966916979, 1.800000000]: Failed to create the teb_local_planner/TebLocalPlannerROS planner, are you sure it is properly registered and that the containing library is built? Exception: Failed to load library /opt/ros/kinetic/lib//libteb_local_planner.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = libg2o_csparse_extension.so: cannot open shared object file: No such file or directory)

and yet the file clearly exists

ls -l /opt/ros/kinetic/lib//libteb_local_planner.so
-rw-r--r-- 1 root root 1391328 nov 26 16:49 /opt/ros/kinetic/lib//libteb_local_planner.so

The command I'm using to launch is:

roslaunch teb_local_planner_tutorials robot_carlike_in_stage.launch 

but I've tried it in my own launch files and it doesn't work there either.

OS Info (screenfetch output):

                          ./+o+-       andreas@andreas-XPS-13-9350
                  yyyyy- -yyyyyy+      OS: Ubuntu 16.04 xenial
               ://+//////-yyyyyyo      Kernel: x86_64 Linux 4.4.0-62-generic
           .++ .:/++++++/-.+sss/`      Uptime: 1m
         .:++o:  /++++++++/:--:/-      Packages: 3295
        o:+o+:++.`..```.-/oo+++++/     Shell: zsh 5.1.1
       .:+o:+o/.          `+sssoo+/    Resolution: 1920x1080
  .++/+:+oo+o:`             /sssooo.   DE: Gnome 
 /+++//+:`oo+o               /::--:.   WM: GNOME Shell
 \+/+o+++`o++o               ++////.   WM Theme: Adwaita
  .++.o+++oo+:`             /dddhhh.   GTK Theme: Adapta [GTK2/3]
       .+.o+oo:.          `oddhhhh+    Icon Theme: Ultra-Flat
        \+.++o+o``-````.:ohdhhhhh+     Font: Cantarell 11
         `:o+++ `ohhhhhhhhyo++os:      CPU: Intel Core i7-6560U CPU @ 3.2GHz
           .o:`.syhhhhhhh/.oo++o`      RAM: 1250MiB / 7830MiB
               /osyyyyyyo++ooo+++/    
                   ````` +oo+++o\:    
                          `oo++.      

Hello,
First of all, thank you very much for your work. The teb_local_planner is by far the most efficient and robust local planner I ve tested in a long time.
I was wondering if there is a way to relax the yaw goal constraint, i.e that the robot just stop on its goal without trying to respect the yaw constraint given by the goal message. Usually I achieve this kind of behavior by setting the yaw tolerance to 2*PI, but here it seems the maximal acceptable value is 2.0 rad.

I am following the tutorial to use the teb local planner with car-like robots. The function convert_trans_rot_vel_to_steering_angle(v, omega, wheelbase) considers that the linear velocity is taken at the center of the rear track, which is not exactly the same as at the center of mass (at the center of mass the radius is a bit greater, therefore the linear velocity must be a bit greater as well).

I assume the planner gives velocity commands for the center of mass of the vehicle, so, is this conversion a deliberate approximation? If so, the exact approach would be:
radius= v/omega //at the C.M.
return wheelbase/(std::sqrt(radius*radius-wheelbase*wheelbase/4))

This last line can be proven defining r = sqrt(radius*radius-(wheelbase/2)*(wheelbase/2)) - track/2 and steering_angle = atan(wheelbase /(r+track/2)).

Regards

Issue:
Cannot build on nVidia Jetson ARM computer.

Fix:
Change lines 58-65 of FindSUITESPARSE.cmake to the following:

FIND_PATH( SUITESPARSE_LIBRARY_DIR
                      NAMES libcholmod.so libcholmod.a
                      PATHS /usr/lib 
                            /usr/lib64
                            /usr/lib/x86_64-linux-gnu
                            /usr/lib/i386-linux-gnu
                            /usr/local/lib
                /usr/lib/arm-linux-gnueabihf/ )

ie: we require this path: /usr/lib/arm-linux-gnueabihf/

hi, dear @croesmann , i just combined with robot_localization, in order to get more better pose data, so i add:

in stage launch file. but when arrive at end, it seems not get very good result.
please see below link, especially sim time at 38 sec and at last:
https://youtu.be/pbxcLVV57rs

Thanks!

Hello,
When following a straight global path in a narrow corridor (wrt the width of the robot), i have oscillations on the trajectory that i can't explain. These oscillations are not present when navigation in wider spaces and i don't think they are due to localization noise.
Any tips or ideas to narrow the search?

Hi, first off thanks so much for the great algorithm and code! I came across this planner several weeks ago and it is such a huge improvement over the standard planners that come with move_base!

I am having trouble getting the inflation distance and penalties set correctly for my robot. I have the min_dist set low to pass through narrow gaps, but this is causing my robot to cut corners very tightly or drive very closely to people, which I don't want. I have tried turning up both the inflation_dist and weight_inflation but these don't seem to work as I was expecting. I was expecting turning these values up to cause the robot to try to stay away from obstacles if possible, but still get close to them if required. Instead, the robot still cut corners but now wouldn't go through narrow gaps. Are these parameters doing something other than what I am expecting?

Some information about my setup, in case it is useful
OS: Ubuntu 14.04
Distro: Indigo
Robot: Turtlebot w/ Kobuki Base
Lidar: Hokuyo URG-04LX
Config file: teb_local_planner_params.txt

Hi,
Sorry to disturb you, but when I run the teb local planner, it tells me that "Cannot build graph, because it is not empty. Call graphClear()!", so I write the function “printf("size..%d\n",optimizer->edges().size());” in the function boost::shared_ptrg2o::SparseOptimizer TebOptimalPlanner::initOptimizer(){}, and I get the result that the size of the edges is 31921488, what happened?
A few days ago, the teb planner works good, but I don't what I have done to my computer, today when I run it, it just tells me this error......
Thank you again!

hi, i am testing my teb_local_planner,and found that calculateHSignature cost too much time ,and also exploreHomotopyClassesAndInitTebs and optimizeAllTEBs , they almost take 50 seconds in sum sometimes!
[ WARN] [1476349506.531398927]: Control loop missed its desired rate of 3.0000Hz... the loop actually took 50.1355 seconds

may it be non-suited to some dependent lib in my environment?

I found an interesting problem when using the teb_local_planner for my omnidirectional robot.
I downloaded the planner from source and the problem also shows in the teb_local_planner robot_omnidir_in_stage.launch from the tutorial package.

When making a counter clockwise in place rotation, the maximum velocity is never reached.
Typically the theta veloctiy flucktuates at about 0.2.
When doing clockwise rotation the maximum velocity is reached, for example -0.3 in the tutorial example.

The problem only occurs when turning counter clockwise from standstill, if you start a rotation in clockwise direcation and than switch to counter clockwise, the max velocity is reached.

Here is a video showcasing the problem: video

Hi, your package is a very good solution!
but i come across some issue when doing some tests with default parameters.
I find trajectory optimization is not stable in some cases, especially the obstacle is close to start or end. issues:
1.The trajectory swithes from one to another all the time, and not stable. please see screenshot 1&2.
2. the final trajectory is not the best trajectory from human. please see screenshot 3.
pics show below:

thank you!

Would it be feasible to use a GPU to optimize the paths as opposed to multiple threads or do you suspect the overhead would surpass the benefit. From just quickly looking it seems like a good candidate for parallelization. Also, the matrix library used has an option to compile with GPU support. Would this substantially impact the planners performance?

Hello:
I want to add some movable obstacles to the scene in order to get the sensor reading from laser scanner and then avoid obstacles while planning trajectory. How can I do this in Tutorial 9 with stage simulator? Tutorial 9 only simply adding a Rviz Cylinder type marker but i want to add movable obstacles in stage simulator and that can be seen at rviz?
Please advise.

Hi,
Thank your for the update of the algorithm. But I find a new problem in the latest version of the Kinetic. When the planner sometimes replan the local plan, the plan changes into a mass and robot stuck in the place and doesn't move anymore (see the following picture.The red line is the teb_local_plan, and the green line is global plan of the move_base)
Please help me how to solve this problem?
Thank you very much.

Hi,
I am trying this local planner in auto-parking with a electric car. Now, I can successfully complete the auto-parking scenario in the simulation. But the problem is that the "/cmd_vel/angular/z" is frequently changed within a short time when the car is moving. This effect can be ignored in the simulation but when testing in the car,the controller steer can't adjust the route with a high frequently changeable direction. Especially, when the car is moving near some obstacles, the angular/z is more unstable.
So,is there any parameters can be tuned to make the angular/z change more stably? Or other ideas can solve this problem?

Thanks

When running the local planner (latest version), I get this error repeatedly.

[ERROR] [1451883955.408376514, 2197.280000000]: Lookup would require extrapolation into the past.  Requested time 2176.620000000 but the earliest data is at time 2188.240000000, when looking up transform from frame [map] to frame [odom]
[ WARN] [1451883955.410609732, 2197.280000000]: MSG to TF: Quaternion Not Properly Normalized
[ERROR] [1451883955.606963287, 2197.480000000]: Lookup would require extrapolation into the past.  Requested time 2176.620000000 but the earliest data is at time 2188.440000000, when looking up transform from frame [map] to frame [odom]
[ WARN] [1451883955.607110702, 2197.480000000]: MSG to TF: Quaternion Not Properly Normalized
[ERROR] [1451883955.809488921, 2197.680000000]: Lookup would require extrapolation into the past.  Requested time 2176.620000000 but the earliest data is at time 2188.640000000, when looking up transform from frame [map] to frame [odom]
[ WARN] [1451883955.813038204, 2197.680000000]: MSG to TF: Quaternion Not Properly Normalized
[ERROR] [1451883956.007636284, 2197.880000000]: Lookup would require extrapolation into the past.  Requested time 2176.620000000 but the earliest data is at time 2188.840000000, when looking up transform from frame [map] to frame [odom]
[ WARN] [1451883956.008115028, 2197.880000000]: MSG to TF: Quaternion Not Properly Normalized
[ERROR] [1451883956.209344183, 2198.080000000]: Lookup would require extrapolation into the past.  Requested time 2176.620000000 but the earliest data is at time 2189.040000000, when looking up transform from frame [map] to frame [odom]
[ WARN] [1451883956.209847509, 2198.080000000]: MSG to TF: Quaternion Not Properly Normalized
[ERROR] [1451883956.409329772, 2198.280000000]: Lookup would require extrapolation into the past.  Requested time 2176.620000000 but the earliest data is at time 2189.240000000, when looking up transform from frame [map] to frame [odom]
[ WARN] [1451883956.409824265, 2198.280000000]: MSG to TF: Quaternion Not Properly Normalized````

Any ideas on what could be causing this?

Hi,
Planner is failing in cases where robot should move backward first then rotate. This behaviour is visible in tight spaces. Please refer screenshot image. Red color arrow is goal pose. Blue color is global plan from navfn.

. Recent release of teb code is being used. The errors from planner on consoler are following:
Planner is not planning moving backward in straight line. It plans backward turns but not straight back. Planner works in open spaces and reaches all configurations. But not backward out of tight spaces. I am attaching configuration file as well.

move_base_config.zip
[ WARN] [1475840863.343512734]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1475840863.545510442]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1475840865.336069840]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1475840865.535774503]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1475840865.735026284]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1475840865.934701774]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1475840866.134523465]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1475840866.342080671]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1475840866.535884323]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1475840866.717715748]: Clearing costmap to unstuck robot (0.250000m).
[ WARN] [1475840867.150482669]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1475840867.317689624]: Rotate recovery behavior started.
[ERROR] [1475840867.318205513]: Rotate recovery can't rotate in place because there is a potential collision. Cost: -1.00
[ WARN] [1475840867.751328980]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1475840867.917722470]: Clearing costmap to unstuck robot (1.840000m).
[ WARN] [1475840868.354743782]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] [1475840868.517727835]: Rotate recovery behavior started.
[ERROR] [1475840868.518180329]: Rotate recovery can't rotate in place because there is a potential collision. Cost: -1.00
[ WARN] [1475840868.949969320]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ERROR] [1475840869.117699310]: Aborting because a valid control could not be found. Even after executing all recovery behaviors

Thank you.

Hi,

I am new to TEB_local_planner. I am using TEB_local_planner for navigating a robot from point A to point B (and I have a global path via global_planner). The problem is that when robot faces the obstacle in front of itself it does not find the "currect" trajectory and it blocks with the warning "trajectory not feasible".

What is strange to is that if I try to block the robot from sides of the robot (left or right) it easily changes its trajectory nd achives to avoid the obstacle.

Here also you can find my TEB local planner parameters. could you please help me to find where is the origin of this issue?

Thanks in advance,

`TebLocalPlannerROS:

odom_topic: /odom/filtered
map_frame: map

Trajectory

teb_autosize: True
dt_ref: 0.15
dt_hysteresis: 0.1
#global_plan_viapoint_sep: 1.0
global_plan_overwrite_orientation: True
max_global_plan_lookahead_dist: 5.0
force_reinit_new_goal_dist: 0.5
feasibility_check_no_poses: 5
shrink_horizon_backup: True

Robot

max_vel_x: 1.0
max_vel_x_backwards: 0.2
max_vel_theta: 0.2
acc_lim_x: 0.2
acc_lim_theta: 0.05
min_turning_radius: 0
footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
#type: "two_circles"
type: "polygon"
#radius: 0.7 # for type "circular"
#line_start: [-0.3, 0.0] # for type "line"
#line_end: [0.3, 0.0] # for type "line"
#front_offset: 0.425 # for type "two_circles"
#front_radius: 0.5 # for type "two_circles"
#rear_offset: -0.425 # for type "two_circles"
#rear_radius: 0.5 # for type "two_circles"
vertices: [ [0.725, -0.4], [0.725, 0.4], [-0.725, 0.4], [-0.725, -0.4] ] # for type "polygon"
#vertices: [[1.375, -0.4], [1.375, 0.4], [-0.725, 0.4], [-0.725,-0.4]]

GoalTolerance

xy_goal_tolerance: 0.5
yaw_goal_tolerance: 0.15
free_goal_vel: False

Obstacles

min_obstacle_dist: 0.5 #it was 0.9 by Patrick
inflation_dist: 0.6
obstacle_association_force_inclusion_factor: 4
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 2.0
obstacle_poses_affected: 30
costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
costmap_converter_spin_thread: True
costmap_converter_rate: 5

Optimization

no_inner_iterations: 3
no_outer_iterations: 3
optimization_activate: True
optimization_verbose: False
penalty_epsilon: 0.01
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: 10
weight_kinematics_turning_radius: 0
weight_optimaltime: 400
weight_obstacle: 50
weight_via_point: 100 # New in version 0.4
weight_dynamic_obstacle: 20

Homotopy Class Planner

selection_alternative_time_cost: True
enable_homotopy_class_planning: False
enable_multithreading: True
simple_exploration: False
max_number_classes: 2
selection_cost_hysteresis: 1.0
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
obstacle_heading_threshold: 0.45
visualize_hc_graph: False`

First things first... thanks by the awesome planner it is the most adaptable planner i have seen.

About the problem:

When i go forward the teb cut the total distance planned by the max_global_plan_lookahead_dist param. But, when i go in the reverse direction, the only thing that limit the planner is the costmap itself, making a plan very long to my processor (and causing it to miss some control loops, i guess).

Thanks, again.

Hi Christoph,

Sometimes, due a different number of factors, teb cannot converge the path to a good solution.
One very important thing, since in these cases the robot can go anywhere... is to detect that case and stop the cmd vel, like in the unfeasible plan.
I m thinking in analyse the poses generated (eg: if the poses are too distant from each other or a measure of how the angle between multiple poses were selected.
Maybe we can do it in the answer of G2o or in the cost step?
What to do think about this functionality?

Thanks for the awesome work!

Hi,
im currently working with TEB and a tricycle drive. TEB is most of the time working very good and driving the best solution. But sometimes, I could not determine why, he is planning and executing the inefficient way you could think of. Here an example of what i mean:

So the current goal is Node 1108 and the heading is in the same direction as the robot. So the best solution would be to drive straight forward, but teb is planning and executing a solution with two turns. I decreased the tolerances, checked the influence of the weight parameters and changed min_turning_radius, also like vel_theta and nothing worked.

Here my default config parameters:

`TebLocalPlannerROS:

odom_topic: agv_1/odom

Trajectory

teb_autosize: True
dt_ref: 0.275
dt_hysteresis: 0.1
global_plan_overwrite_orientation: true
allow_init_with_backwards_motion: true
max_global_plan_lookahead_dist: 5.0
feasibility_check_no_poses: 5
global_plan_viapoint_sep: -0.1
via_points_ordered: false

Robot

max_vel_x: 1.5
max_vel_x_backwards: 0.75
max_vel_theta: 1.0 # the angular velocity is also bounded by min_turning_radius in case of a carlike robot (r = v / omega)
max_vel_y: 0.0
acc_lim_x: 1.0
acc_lim_theta: 0.5

********************** Carlike robot parameters ********************

min_turning_radius: 0.5 # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
wheelbase: -1.204 # Wheelbase of our robot
cmd_angle_instead_rotvel: True # stage simulator takes the angle instead of the rotvel as input (twist message)

********************************************************************

footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
type: "polygon"
radius: 0.4 # for type "circular"
line_start: [-1.6, 0.0] # for type "line"
line_end: [1.440, 0.0] # for type "line"
front_offset: 0.75 # for type "two_circles"
front_radius: 0.75 # for type "two_circles"
rear_offset: 0.65 # for type "two_circles"
rear_radius: 0.65 # for type "two_circles"
vertices: [ [-1.6,-0.520], [1.440,-0.520], [1.440,0.520], [-1.6,0.520] ] # for type "polygon"

GoalTolerance

xy_goal_tolerance: 0.2
yaw_goal_tolerance: 0.05
free_goal_vel: True

Obstacles

min_obstacle_dist: 0.27 # This value must also include our robot's expansion, since footprint_model is set to "line".
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.0
obstacle_poses_affected: 5
inflation_dist: 1.2

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: 0
weight_kinematics_turning_radius: 5
weight_optimaltime: 50
weight_obstacle: 50
weight_inflation: 0.1
weight_dynamic_obstacle: 10 # not in use yet
weight_adapt_factor: 2

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
obstacle_heading_threshold: 0.45
visualize_hc_graph: False
`

Would be very happy about some advices,
Cheer Rob

Hi,

I am facing a problem, where the teb_planner claims, that the trajectory which he wants to plan along isn't feasible I can't see any reasons this is the case.
My setup is following:

• Gazebo Simulation
• Local planner: teb_local_planner
• Global planner: sbpl_planner or self written carrot planner (both same problem)

After setting a new goal the teb planner plans along the global plan, which looks good, after driving the base a few meters, it changes in the short horizon mode and keeps resetting itself with following output:

Debug mode was on:

[ INFO] [1467789686.934077314, 1142.800000000]: odom received!
[ WARN] [1467789690.670318292, 1144.963000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 2.5520 seconds
[DEBUG] [1467789694.034116645, 1146.903000000]: initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...
[DEBUG] [1467789694.039371530, 1146.903000000]: TebOptimalPlanner::isHorizonReductionAppropriate(): Distance between consecutive poses > 0.9*min_obstacle_dist
[ WARN] [1467789694.039607752, 1146.903000000]: TebLocalPlannerROS: trajectory is not feasible, but the planner suggests a shorter horizon temporary. Complying with its wish for at least 10s...
[ WARN] [1467789701.844726254, 1151.502000000]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[DEBUG] [1467789704.087700070, 1152.800000000]: initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...
[ WARN] [1467789704.089528617, 1152.800000000]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[DEBUG] [1467789707.287139842, 1154.699000000]: initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...
[ WARN] [1467789707.289574536, 1154.699000000]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[DEBUG] [1467789710.816752726, 1156.699000000]: initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...
[ WARN] [1467789710.818900947, 1156.699000000]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...

In this video you see a video of the behavior:

Video Link Youtube

Here is the parameter file used for the teb_planner.

Greetings,
I have observed that teb have preference to turn back and move rather than moving forward as human do.
I am attaching two screen shots to help me pose this question. I have also observed that even in case of diff-drive robot vehicle do not considers rotation in place as one of the feasible options.
Kindly advise which parameters I should tune so that preference to forward movement is higher.

I am using following parameters for teb

TebLocalPlannerROS:

odom_topic: odom

Trajectory

cmd_angle_instead_rotvel: false #NITIN
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.2
max_vel_y: 0.0
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
obstacle_heading_threshold: 0.45
visualize_hc_graph: False

Hello,

as already posted on ROS Answers, I have the problem that the feasibility check regularly fails without any obstacles near by. I think the reason is, that my local costmap is too small so that the footprint is sometimes outside of it for poses further away from the robot.

Of course an obvious solution is to increase the costmap size or reduce feasibility_check_no_poses. But none of these give me a guarantee that the problem cannot occur. Is there some parameter with which I can restrict the maximum distance of poses to the current robot position, when doing the feasibility check (so that it will rather consider less poses for the check instead of moving outside of the costmap)?

Best, Felix

How can I set via-points through code? The tutorial (http://wiki.ros.org/teb_local_planner/Tutorials/Following%20the%20Global%20Plan%20%28Via-Points%29) only shows how to set them in Rviz.

Hi,

is it possible to implement a minimal velocity? We are using the teb_local_planner with success on either simulated or real ackermann driven robots.
But in some cases we are facing the problem that the robot base cannot execute the desired velocity.
E.g. it would only start driving at a minimal velocity of 0.1 m/s.

Best regards and thumbs up for the great work!

Hi Christoph,

First of all, thanks for contributing this awesome project.

However, something confuses me. I noticed that the produced TEB local plan sometimes crossed over the inflation margin of detected obstacles, which could cause collisions. Is that normal?

Line in pink in above screenshot was the problem TEB local plan

Look forward to your reply.

Kevin

Hello,

is it possible to apply a min. turning radius higher than 5m?

Kind regards.

I am trying to use this planner for a 'car-like' robot. On the robot, the rate of change of the steering wheel is limited, i.e. it can only change the steering angle at ~1 rad/s. I want to add this constraint to the planner to generate paths that are feasible. Do you have any suggestions about how to implement this constraint?

I got to know that libg2o supports used-supplied first derivatives, but can also run in derivative-free mode. You already implemented a lot of Jacobians, but commented them out . In this context, I stumbled upon some questions:

• Why did you comment them out?
• What is the benefit of supplying the Jacobian to the optimizer?
• Could one utilize automatic differentiation (AD, e.g. CppAD or Casadi) for computing them, within the libg2o framework?
• Did you use symbolic tools (matlab, octave, sympy, ..) for calculating them? Do you have sources lying around to continue the work or customize for my own edges?

Thanks, and keep going the impressive work :)

I am trying to use the TEB local planner plugin in the move_base ROS package. I am facing some problem with the costmap formed. Whenever someone/thing moves in front of the LiDAR an obstacle layer is formed as visible in Rviz.

Now, the problem is that after the person/thing has shifted from its previous position, the costmap doesn't clear the old layer unless there isn't any other obstacle behind that layer. Can you please help and resolve this issue.

Hello,

I am trying to use teb_local_planner for an engine powered robot car. Sometimes the planner produces a plan consisting a lot of small trajectory segments which switch between forward and reverse drive repeatedly. However, the shifting motor in the robot car actually takes some time to change the gear between forward and reverse drive, which makes the frequent switching undesirable. Would it be possible to add extra cost for the drive direction switching? If possible, could you please advise which part of the code should I look into?

Thank you.

Are there any potential issues with using the trajectory points from teb_local_planner's /feedback_msg topic as the primary method of control?

By this, I mean that there is a separate node which subscribes to the /feedback_msg topic, and based on the trajectory points, the node calculates a throttle, braking, and steering value to send to the vehicle controllers. /enable_homotopy_class_planning will probably be set to true.

Hi,

The planner is going easily to side of wall but it can't turn back; Here is video. And its parameters:

TebLocalPlannerROS:

  # ROBOT ----------------------------------------------------------------

  max_vel_x: 0.6 # (double, default: 0.4) #Maximum translational velocity of the robot in meters/sec
  max_vel_x_backwards: 0.6 # (double, default: 0.2) #Maximum absolute translational velocity of the robot while driving backwards in meters/sec. See optimization parameter weight_kinematics_forward_drive
  max_vel_theta: 1.0 # (double, default: 0.3) #Maximum angular velocity of the robot in radians/sec
  acc_lim_x: 1.0 # (double, default: 0.5) #Maximum translational acceleration of the robot in meters/sec^2
  acc_lim_theta: 2.0 # (double, default: 0.5) #Maximum angular acceleration of the robot in radians/sec^2
  # Ackermann vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
  min_turning_radius: 0.01 # (double, default: 0.0) #Minimum turning radius of a carlike robot (set to zero for a diff-drive robot). New in version 0.2.2
  wheelbase: 1.3 # (double, default: 1.0) #The distance between the drive shaft and steering axle. The value might be negative for back-wheeled robots (only required if ~<name>/cmd_angle_instead_rotvelis set to true). New in version 0.2.3
  cmd_angle_instead_rotvel: false # (bool, default: false) #Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle [-pi/2,pi/2]. Note, changing the semantics of yaw rate depending on the application is not preferable. Here, it just complies with the inputs required by the stage simulator. Datatypes in ackermann_msgs are more appropriate, but are not supported by move_base. The local planner is not intended to send commands by itself. New in version 0.2.3

  # GOAL TOLERANCE -------------------------------------------------------

  xy_goal_tolerance: 0.02 # (double, default: 0.2) #Allowed final euclidean distance to the goal position in meters
  yaw_goal_tolerance: 0.01 # (double, default: 0.2) #Allowed final orientation error in radians
  free_goal_vel: false # (bool, default: false) #Remove the goal velocity constraint such that the robot can arrive at the goal with maximum speed

  # TRAJECTORY -----------------------------------------------------------

  dt_ref: 0.3 # (double, default: 0.3) #Desired temporal resolution of the trajectory (the trajectory is not fixed to dt_ref since the temporal resolution is part of the optimization, but the trajectory will be resized between iterations if dt_ref +-dt_hysteresis is violated.
  dt_hysteresis: 0.1 # (double, default: 0.1) #Hysteresis for automatic resizing depending on the current temporal resolution, usually approx. 10% of dt_ref is recommended
  min_samples: 3 # (int, default: 3) #Minimum number of samples (should be always greater than 2)
  global_plan_overwrite_orientation: false # (bool, default: true) #Overwrite orientation of local subgoals provided by the global planner (since they often provide only a 2D path)
  force_reinit_new_goal_dist: 1.0 # (double, default: 1.0) #Reinitialize the trajectory if a previous goal is updated with a separation of more than the specified value in meters (skip hot-starting)
  feasibility_check_no_poses: 5 # (int, default: 5) #Specify up to which pose on the predicted plan the feasibility should be checked each sampling interval. New in version 0.2.2
  publish_feedback: false # (bool, default: false) #Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging). See list of publishers above. New in version 0.2.2
  shrink_horizon_backup: false # (bool, default: true) #Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues. New in version 0.2.3

  # OBSTACLE PARAMETERS --------------------------------------------------

  min_obstacle_dist: 0.03 # (double, default: 0.5) #Minimum desired separation from obstacles in meters
  include_costmap_obstacles: true # (bool, default: true) #Specify if obstacles of the local costmap should be taken into account. Each cell that is marked as obstacle is considered as a point-obstacle. Therefore do not choose a very small resolution of the costmap since it increases computation time. In future releases this circumstance is going to be addressed as well as providing an additional api for dynamic obstacles.
  costmap_obstacles_front_only: false # (bool, default: true) #Limit the considered costmap obstacles to the front of the robot in order to reduce computational effort.
  obstacle_poses_affected: 3 # (int, default: 10) #Each obstacle position is attached to the closest pose on the trajectory in order to keep a distance. Additional neighbors can be taken into account as well.
  line_obstacle_poses_affected: 10 # (int, default: 25) #See obstacle_poses_affected, but specialized for line-shaped obstacles.
  polygon_obstacle_poses_affected: 10 # (int, default: 25) #See obstacle_poses_affected, but specialized for polygon-shaped obstacles.
  #The following parameters are relevant only if costmap_converter plugins are desired (see tutorial): New in version 0.2.0 
  costmap_converter_plugin: ""
  # (string, default: "") #Define plugin name in order to convert costmap cells to points/lines/polygons. Set an empty string to disable the conversion such that all cells are treated as point-obstacles.
  costmap_converter_spin_thread: true # (bool, default: true) #If set to true, the costmap converter invokes its callback queue in a different thread.
  costmap_converter_rate: 5.0 # (double, default: 5.0) #Rate that defines how often the costmap_converter plugin processes the current costmap (the value should not be much higher than the costmap update rate) [in Hz].

  # OPTIMIZATION ---------------------------------------------------------

  no_inner_iterations: 3 # (int, default: 5) #Number of actual solver iterations called in each outerloop iteration. See param no_outer_iterations.
  no_outer_iterations: 3 # (int, default: 4) #Each outerloop iteration automatically resizes the trajectory according to the desired temporal resolution dt_ref and invokes the internal optimizer (that performs no_inner_iterations). The total number of solver iterations in each planning cycle is therefore the product of both values.
  penalty_epsilon: 0.05 # (double, default: 0.1) #Add a small safety margin to penalty functions for hard-constraint approximations
  weight_max_vel_x: 1.0 # (double, default: 2.0) #Optimization weight for satisfying the maximum allowed translational velocity
  weight_max_vel_theta: 1.0 # (double, default: 1.0) #Optimization weight for satisfying the maximum allowed angular velocity
  weight_acc_lim_x: 1.0 # (double, default: 1.0) #Optimization weight for satisfying the maximum allowed translational acceleration
  weight_acc_lim_theta: 1.0 # (double, default: 1.0) #Optimization weight for satisfying the maximum allowed angular acceleration
  weight_kinematics_nh: 1000.0 # (double, default: 1000.0) #Optimization weight for satisfying the non-holonomic kinematics (this parameter must be high since the kinematics equation constitutes an equality constraint, even a value of 1000 does not imply a bad matrix condition due to small 'raw' cost values in comparison to other costs).
  weight_kinematics_forward_drive: 0.001 # (double, default: 1.0) #Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities). A small weight (e.g. 1.0) still allows driving backwards.
  weight_kinematics_turning_radius: 1.0 # (double, default: 1.0) #Optimization weight for enforcing a minimum turning radius (only for carlike robots). New in version 0.2.2
  weight_optimaltime: 1.0 # (double, default: 1.0) #Optimization weight for contracting the trajectory w.r.t transition/execution time
  weight_point_obstacle: 10.0 # (double, default: 1.0) #Optimization weight for keeping a minimum distance from point obstacles
  weight_line_obstacle: 2.0 # (double, default: 10.0) #Optimization weight for satisfying a minimum separation from line obstacles.
  weight_poly_obstacle: 3.0 # (double, default: 10.0) #Optimization weight for satisfying a minimum separation from polygon obstacles.

  # PARALLEL PLANNING ----------------------------------------------------

  enable_homotopy_class_planning: false # (bool, default: true) #Activate parallel planning in distinctive topologies (requires much more CPU resources, since multiple trajectories are optimized at once)
  enable_multithreading: true # (bool, default: true) #Activate multiple threading in order to plan each trajectory in a different thread
  max_number_classes: 5 # (int, default: 5) #Specify the maximum number of distinctive trajectories taken into account (limits computational effort)
  roadmap_graph_no_samples: 15 # (int, default: 15) #Specify the number of samples generated for creating the roadmap graph
  roadmap_graph_area_width: 6 # (double, default: 6) #Random keypoints/waypoints are sampled in a rectangular region between start and goal. Specify the width of that region in meters.
  h_signature_prescaler: 1.0 # (double, default: 1.0) #Scale internal parameter: (H-signature) that is used to distinguish between homotopy classes. Warning: reduce this parameter only, if you observe problems with too many obstacles in the local cost map, do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2<value<=1).
  h_signature_threshold: 0.1 # (double, default: 0.1) #Two H-signatures are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold.
  obstacle_heading_threshold: 1.0 # (double, default: 1.0) #Specify the value of the scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration.
  visualize_hc_graph: false # (bool, default: false) #Visualize the graph that is created for exploring distinctive trajectories (check marker message in rviz)

  # MISCELLANEOUS --------------------------------------------------------

  odom_topic: "odom"
  # (string, default: "odom")
  #Topic name of the odometry message, provided by the robot driver or simulator.
  map_frame: "map"
  # (string, default: "odom")
  #Global planning frame (in case of a static map, this parameter must be usually changed to "/map". 

Dear Christoph,

I was thinking of implementing a feature that would allow for the following feature:
Dynamic Obstacle get's in the path of the robot. Robot slows down and ultimately stops and waits for X seconds. After x seconds, if the obstacle hasn't moved away, the robot replans the path.

So essentially instead of an immediate re-planning, I would like to add some lag, since in many instances, the dynamic obstacle will move out of the way by itself and a dynamic re-planning with the other dynamic obstacle also re-planning course will lead to many re-plannings...

What are your thoughts on this? Where and how would you start with such a change?

I'm using teb_local_planner (which is by far the best local planner I've tried) on a robot with a tricycle drive. I have a set of global goals that sends the robot on a serpentine pattern. Although the left turns and right turns are perfectly symmetric in terms of the goals, the teb local planner generates a weird local path only for the left turns. Right turns are planned very smoothly.

Do you have any ideas what could be wrong? I'll be glad to submit a pull request if you could kindly give me some pointers

Hello again,

I tried to add and move around objects in the scene in order to observer the behaviour using Teb_planner ,it seems that the object's position and size when detection at the first try is remembered and taken into account in future tries. Is there a parameter that I can change so that each time the object detected is not used more than once only during run-time.

I read about it on one of the issues and @croesmann has mentioned he will push some changes regarding with it this summer.

Thanks in advance and cheers.

Greetings,
I am attempting to run teb-local-planner on a bigger vehicle. The size of wheel base is 1.9 meters and turning radius is appx. 4.5 meters. I have observed below mentioned undesired behaviours by the planner. I request you to suggest possible remedy for these behaviours.

I have observed that local planner is unable to trace the planned path. It prefers to take a reverse and align vehicle with the global plan but is unable to execute the plan. Local plan continuously changes. Sometimes, local planned path moves through the walls.
When a global planner plans a straight line path, robot is moves in snake like fashion. This is more prominent at lower velocities.

It is also observed that acceleration limits on rotation velocities has less effect. Robot is unable to achieve max speed if dt_ref is small.

I am attaching configuration files used for stage simulation. I am using pre-compiled teb on ROS Kinetic for Ubuntu 16.04.

Thanks,
Nitin
teb_local_planner_launch-20160908T182842Z.zip

Hi,
do you have examples of a car-like gazebo model set up with teb local planner and an ackermann controller, which you could upload?

Thank you and kind regards.

Hi, All,

We are trying to use teb_local_planner for parallel parking application, anyone has tried this before? What would be the good set of parameters to start with? Thank you very much.

Regards
Ben

Good evening and thanks for all!.
i set my planner as teblocalplanner and i realized that when the direction where the robot has to go,is backwards,the turning is strange...it may get lost...it may go fully backwards(which i dont want to)...it spins fast....i did not touch the parametres at all.my global planner is the the "stock" one of ROS

all i want to do is :
Force the robot to spin on the same point when it is for backward movement...make a rotation with pi rad and move forward...
by the way my robot is differential drive

thanks

Hi again @croesmann
I have tried to create an empty world and modified the costmap accordingly (I think). However I am not able to use the 2D Nav Goal in Rviz to get it running. may you take a look and tell me what is wrong?
First I tried with no map at all, but apparantly /map topic is expected. Therefore I imported an empty while floor plan as a map. Can that be the issue?

NODES
/
amcl (amcl/amcl)
map_server (map_server/map_server)
move_base (move_base/move_base)
rviz (rviz/rviz)
stageros (stage_ros/stageros)

auto-starting new master
process[master]: started with pid [63457]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to de49d692-aaa8-11e7-b290-309c231e60e0
WARNING: Catkin package name "MiniUku_Teb_Planner" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, and underscores.
process[rosout-1]: started with pid [63470]
started core service [/rosout]
process[stageros-2]: started with pid [63494]
process[move_base-3]: started with pid [63495]
process[map_server-4]: started with pid [63496]
process[amcl-5]: started with pid [63497]
process[rviz-6]: started with pid [63506]
[ INFO] [1507302792.237333686]: Loading map from image "/home/iseauto/mini_Uku_ws/src/MiniUku_Teb_Planner/maps/White.png"
[ INFO] [1507302792.238679061]: Read a 200 X 200 map @ 0.050 m/cell
[ INFO] [1507302792.375877775]: Subscribed to map topic.
[ INFO] [1507302792.522438489]: Received a 200 X 200 map @ 0.050 m/pix

[ INFO] [1507302792.530153281]: Initializing likelihood field model; this can take some time on large maps...
[ INFO] [1507302792.540510948]: Done initializing likelihood field model.
[ INFO] [1507302792.945419174, 0.400000000]: Using plugin "obstacle_layer"
[ INFO] [1507302792.970763341, 0.400000000]: Subscribed to Topics: laser_scan_sensor
[ INFO] [1507302793.850044294, 1.300000000]: Using plugin "obstacle_layer"
[ INFO] [1507302793.871618545, 1.300000000]: Subscribed to Topics: laser_scan_sensor
[ INFO] [1507302793.961746216, 1.400000000]: Created local_planner teb_local_planner/TebLocalPlannerROS
[ INFO] [1507302794.070914637, 1.500000000]: Footprint model 'line' (line_start: [0,0]m, line_end: [0.7,0]m) loaded for trajectory optimization.
[ INFO] [1507302794.071000970, 1.500000000]: Parallel planning in distinctive topologies enabled.
[ INFO] [1507302794.071032429, 1.500000000]: No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles.
[ INFO] [1507302795.364663524, 2.800000000]: Recovery behavior will clear layer obstacles
[ INFO] [1507302795.388241567, 2.800000000]: Recovery behavior will clear layer obstacles
[ INFO] [1507302795.430094569, 2.800000000]: odom received!
[ERROR] [1507303162.893627732, 370.300000000]: NO PATH!
[ERROR] [1507303162.893725523, 370.300000000]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ERROR] [1507303163.045065321, 370.500000000]: NO PATH!
[ERROR] [1507303163.045403321, 370.500000000]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
[ERROR] [1507303163.245820288, 370.700000000]: NO PATH!

When i use teb_local_planner in kobuki , it can not compute Velocity Commands to move_base node in time ,so move_base node print these info always and no other info:
[ WARN] [1476253731.838476254]: Control loop missed its desired rate of 3.0000Hz... the loop actually took 3.9523 seconds
[ WARN] [1476253994.264856069]: Map update loop missed its desired rate of 3.0000Hz... the loop actually took 12.3793 seconds

My params is default. When i used dwa, it is all right.
By the way, the cpu top info is fined ,there is 80% idle.

base_local_planner_params.yaml-------------
controller_frequency: 3.0
recovery_behavior_enabled: false
clearing_rotation_allowed: false
base_local_planner: "teb_local_planner/TebLocalPlannerROS"

base_local_planner: "dwa_local_planner/DWAPlannerROS"

TrajectoryPlannerROS:
max_vel_x: 0.5
min_vel_x: 0.1
max_vel_y: 0.0 # zero for a differential drive robot
min_vel_y: 0.0
max_vel_theta: 1.0
min_vel_theta: -1.0
min_in_place_vel_theta: 0.4
escape_vel: -0.1
acc_lim_x: 1.5
acc_lim_y: 0.0 # zero for a differential drive robot
acc_lim_theta: 1.2

holonomic_robot: false
yaw_goal_tolerance: 0.1 # about 6 degrees
xy_goal_tolerance: 0.05 # 5 cm
latch_xy_goal_tolerance: false
pdist_scale: 0.4
gdist_scale: 0.8
meter_scoring: true

heading_lookahead: 0.325
heading_scoring: false
heading_scoring_timestep: 0.8
occdist_scale: 0.05
oscillation_reset_dist: 0.05
publish_cost_grid_pc: false
prune_plan: true

sim_time: 1.0
sim_granularity: 0.05
angular_sim_granularity: 0.1
vx_samples: 8
vy_samples: 0 # zero for a differential drive robot
vtheta_samples: 20
dwa: true
simple_attractor: false
teb_local_planner_params.txt

I have the same problem as the video: https://www.youtube.com/watch?v=L4Wvehzx818 , I get a Error message that Trajectory is not feasible when I set a goal to the robot. How can I slove this problem? Please help me.

Hi,

while creating an extended version of the optimal planner which will add additional poses into the teb, I can not call teb_.insertPose because it is declared as inline in timed_elastic_band.cpp, although it is public in the header. The same problem is with the insertTimeDiff function.

Because of the inline there are no function symbols in the compiled library.

The wheelbase parameter name and description are somewhat misleading in my case: We have a car-like robot with steering axle in the front as well as the drive shaft (like a normal car). So steering and drive happen on the front axle. According to the description ..

The distance between the drive shaft and steering axle. The value might be negative for back-wheeled robots (only required if ~/cmd_angle_instead_rotvel is set to true).

.. the value should be set to zero in our case, shouldn't it? However, the term wheelbase means the discance between front and rear axle. So which one is meant here, or did I misunderstand the parameter description?

Also I'm not quite sure where you expect /base_link to be with respect to the robot model (resp. the robot frame) and how it matters.
Related: http://answers.ros.org/question/237035/steering-axis-of-carlike-robot-with-teb_local_planner/

Hello sir,
I am trying to use teb_local_planner for passing though narrow paths efficiently. The planner works fine for broad pathways but when I am trying to pass a Robot:
type : "line"
line_start: [-0.42, -0.21]
line_end: [0.42, -0.21]
through a door with width ~0.7m. The robot gets stuck to this:

I am currently using ros-kinetic on ubuntu 15.10.
The following are the configurations used :

And sometimes at this (I cannot increase the inflation radius further because it does not allow the robot to move through narrow pathways ) :

These are the configurations for global and local planner:

I have also tried the same with SBPL lattice planner. Please help!