TEB plans and select infeasible trajectories throught obstacles

[Kaetera]

Hello,

Setup: Branch: foxy-devel OS: Ubuntu 20.04 64bit Robot: Physical real robot

Issue and actual behavior TEB generates trajectories through obstacles on my local costmap. This happens very often in my environment. The invalidity is correctly detected (dwb_critics::ObstacleFootprintCritics::scorePose() throw an exception and the controller "abort the handling"). Also, when an alternative homotopy without collision is present, the TEB planner do not select this homotopy.

Expected behavior

• TEB should not plan through obstacles
• Or TEB should switch to an alternative collision-free homotopy if available

Am I misunderstanding something ? Most of params are at their default. Param include_dynamic_obstacles is set to True. The footprint is of polygon type. The same issue is observed with line footprint type.

[Kaetera]

After investigation it seems that TEB uses dwb_critics::ObstacleFootprintCritics::scorePose() function from the Navigation2 stack to evaluate the feasibility of the trajectory. However TEB excpect a negative score from scorePose() if the trajectory is infeasible, but it now actually throw an exception that is not caught by TEB.

https://github.com/rst-tu-dortmund/teb_local_planner/blob/d6865b419ba70bf637f5125317c380e00f7944c3/teb_local_planner/src/optimal_planner.cpp#L1223

Adding a try/catch confirmed that the exception is raised: try { score = costmap_model->scorePose(pose2d, dwb_critics::getOrientedFootprint(pose2d, footprint_spec)); } catch (...) // (const dwb_core::IllegalTrajectoryException & e) { score = -1.0f; // Seems that older versions of scorePose() returned negative value instead of an exception for illegal pose }

allows TEB to be aware of an invalid trajectory but at the end, it rises: [controller_server-8] [ERROR] [1625851895.112989293] [controller_server]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner... [controller_server-8] [WARN] [1625851895.113134206] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.

The string "TebLocalPlannerROS: trajectory is not feasible" comes from TEB and confirm that it detected the invalid trajectory, but as said I expected it to at least just discard it and chose the alternative homotopy...

[teymurov27]

Hi, could you please share your local and global costmap parameters, as it seems there is an issue over there. I think TEB choses over obstacles because your costmap shows that you can pass from here, any delay can cause this. Check your global_frame in both local and global costmap.

[Kaetera]

Hey, Here were my parameters:

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 5.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: false
      rolling_window: true
      width: 4
      height: 4
      resolution: 0.05      
      footprint: "[ [-0.2, -0.25], [0.4, -0.25], [0.4, 0.25], [-0.2, 0.25] ]"   
      footprint_padding: 0.01
      plugins: ["obstacle_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 5.0
        inflation_radius: 0.6     
      obstacle_layer:
          plugin: "nav2_costmap_2d::ObstacleLayer"
          enabled: true
          observation_sources: scan
          scan:
            topic: /scan
            raytrace_max_range: 6.0
            obstacle_max_range: 6.0
            obstacle_min_range: 0.0
            max_obstacle_height: 2.0
            clearing: true
            marking: true
            inf_is_valid: true
            data_type: "LaserScan"  
      always_send_full_costmap: true             

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: false
      footprint: "[ [-0.2, -0.25], [0.4, -0.25], [0.4, 0.25], [-0.2, 0.25] ]" 
      footprint_padding: 0.01
      resolution: 0.05
      track_unknown_space: true      
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: false            
          marking: false            
          data_type: "LaserScan"
          inf_is_valid: true
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: false
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 5.0
        inflation_radius: 0.6
      always_send_full_costmap: true

However "the common inflation layer of the costmap_2d is not taken into account by the planner" (according https://github.com/rst-tu-dortmund/teb_local_planner/issues/41) and the global costmap isn't supposed to be used/affect the local planner...

By looking into the code, my guess is that the main problem is in the scoring of the pose for polygon-shaped footprint: the cost function related to the distance calculates the distance between the centre of the footprint and the edges of the polygon. But it seems there is no check for if the obstacle point is inside the polygon. So for quite large robots (e.g. 0.6m*0.8m in my case) and small min_obstacle_dist (e.g. 0.1m), the obstacle point can be far enough from the edge but inside the polygon, so this invalid pose has a small penalty.

[teymurov27]

Hi, I am not sure but it seems there is a problem in your local costmap parameters, in fact your cost scaling factor is too high. I suggest you tune your cost scaling factor based on this formula exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1) .From this formula we can say that if cost scaling factor is too high teb could include paths through obstacles. Hope it helps.

source: [http://wiki.ros.org/costmap_2d]() source: [http://wiki.ros.org/costmap_2d/hydro/inflation]()

[Today20180409]

I also encountered a similar situation. My robot shape is set to polygon，therefore, when the obstacle is in the robot, the path will pass directly through the obstacle. I think you are right, this should be a problem with the distance between the obstacle to the robot.Do you have any way to solve it? @Kaetera

[Kaetera]

I left out TEB for the moment, but I started to experiment with a different cost function (functions computeError() in https://github.com/rst-tu-dortmund/teb_local_planner/blob/melodic-devel/include/teb_local_planner/g2o_types/edge_obstacle.h)

My guess is that we need a function that:

• Has high cost when the point obstacle is inside the footprint/intersect with the footprint
• Decreases when the point obstacle is inside the footprint and that the footprint is moving away from the obstacle (so that the optimizer knows the gradient direction of a "better" position) (i.e. when the center or the edges is going away ?)

TEB embeds many functions to check lines intersection or that could be used to test if a point is in the footprint... But for now it's just a guess, I will not have the opportunity to work again on TEB before a long time.