search

rst-tu-dortmund / teb_local_planner

An optimal trajectory planner considering distinctive topologies for mobile robots based on Timed-Elastic-Bands (ROS Package)
http://wiki.ros.org/teb_local_planner
BSD 3-Clause "New" or "Revised" License
1.06k stars 550 forks source link

max_global_plan_lookahead_dist problem when going in reverse direction #31

Closed NeryHenrique closed 7 years ago

NeryHenrique commented 8 years ago

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.

croesmann commented 7 years ago

Thanks for the feedback!

I investigated the relevant part in the code again, but I cannot find the cause for your issue. The local (reference) subset of the global plan is extracted in TebLocalPlannerROS::transformGlobalPlan(...). Hereby the Euclidean distance is compared with max_global_plan_lookahead_dist independent of the direction. Can you please provide me with more details or check whether I missed something?

Note, the lookahead distance does not bound the length of the local optimized plan but just the part of the global plan from which the current goal is inferred. For instance if you have a large bound on the minimum turning radius, your trajectory might be really long in order to accomplish the task. We could provide an upper bound on the dimension of the optimization problem by specifying max_samples (introduced recently in 217616d9a3fa39). The planner does not add new samples if the maximum number of samples is reached leading to a coarser discretization and hence approximations of the robot motion. Another drawback of providing a relatively low number of max samples is that obstacles might transit between two consecutive samples/poses since we are currently not investigating the segment between each two poses but only the poses itself for keeping a minimum distance to obstacles. I think it would be really interesting to implement both and do some benchmarks in the future.

NeryHenrique commented 7 years ago

I was investigating the issue to and maybe i have a lead: Its seems that the teb make the bound of the distance planned using the subset of poses of the global plan. But in my case this dont work, because i m using a very simple global planner (the carrot planner) that publish the pose as plan = [initial_pose, target_pose]..... So the teb cannot make any subset that limit the distance. Since that theory i discover that teb wasnt cutting any plan (forward or backward), what was limitating the forward plan was the costmap. I didnt seen that before because my robot is big and is deslocated from the center of the map...

By now i made an workaround making the teb planner publish the plan = [initial_pose,intermediate_pose] and keep constantly updating the plan..

Now i think I have two choices:

1- use a better global planner, but in this case i have to solve the problem the teb almost dont run bakwards, it always maneuver and then go forward and close to the goal maneuver again.. (well i see that you talked about that problem here http://answers.ros.org/question/250138/differential-drive-favor-backwards-movement-teb_local_planner/). So maybe i should study a global plan that publish the correct poses direction.

2 - make an commit to teb that is will make an interpolation with the global plan (with the near two poses) in the max distance asked.

Can you point me in the right direction?