TebLocalPlannerROS: trajectory is not feasible error from previous successful goal point

[cosmicog]

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". 

[croesmann]

Hi Orhan,

thank you for submitting the issue. Probably, the issue is related to the collision model used for trajectory planning which differs from the actual robot model specified for the navigation stack.

Details: Currently, the robot is modeled with a circular shape for optimization (in order to simplify collision checking and therefore speeding up the computation time). The implementation specifies the robot as a single point and the distance to the obstacle is given by rosparam _min_obstacledist (which should include the robot radius itself and the desired separation; okay, the label is not describing this circumstance well). In contrast, the feasibility check (which is performed before actually controlling the robot) uses the robot footprint specified for the costmaps (e.g. circular or polygon, in your case a rectangle). The feasibility check iterates the next N poses (N = _feasibility_check_noposes; a ROS parameter) in order to verify the clearance from obstacles (according to the footprint).

In your example, the robot plans a trajectory which is composed of a short backward motion and a subsequent forward motion. According to your small _min_obstacledist (0.03 m), the planned trajectory is allowed to be really close to the wall. However, the planned trajectory cannot be executed, since one footprint of the upcoming 5 poses (_feasibility_check_noposes) hits the wall. Of course, this behavior is undesired. I already started some weeks ago to address this part of the planner for the upcoming version: extended support of robot shapes other than just circular (or actually point-shaped!). I will also think about how to connect that with the costmap footprint parameter to simplify the configuration of the planner and to make the obstacle avoidance parameters much more intuitive. Or even adding appropriate warning messages. Unfortunately, due to some limited resources and holidays I need to postpone the completion, probably to end of April.

As a workaround I suggest to increase the _min_obstacledist to a value higher than the circumscribed radius of the robot. You also might reduce _feasibility_check_noposes to 0 or 1, since the trajectory is refined during runtime and suboptimal future trajectory parts could collide with obstacles. Edit: After checking your robot shape in the video again, I think that increasing _min_obstacledist to much could prevent the robot from reaching goals close to the wall. You could also reduce the amount of backward motions (height penalty weight _weight_kinematics_forwarddrive and low backwards speed).

Please let me know, if the workaround helps or if you have further suggestions/issues.

[cosmicog]

Thank you Sir.

And sorry for the late response. I've just finished parameter tuning tests: Increased min_obstacle_dist parameter to 0.7 Decreased feasibility_check_no_poses parameter to 0, Increased weight_kinematics_forward_drive parameter to 1000.0.

Now, It's little bit better than before. And when it wasn't very close to wall, It isn't giving feasibility error.

But sometimes (especially close the wall), robot keeps hitting the wall because of the footprint. In this case below, when trying to follow local path's first part (including backward motion, Image 1) it hits the wall. But, it is first calculation of path and if I stop the robot with my joystick's X button before setting a new goal,TebLocalPlanner keeps calculating path, then changes path to better one (Image 2).

But this is slow process and before doing this, robot starts directly and keeps following first created path and hits the wall.

Now, I've changed dt_ref to 0.5 and decreased dt_hysteresis to 0.05. And my temporary solution is worked for this wall to wall case. The Robot calculates best path fast. I think all of problems caused of footprint, as you said...

And, TebLocalPlanner is definitely the best local planner I've ever seen in ROS Platform. Thank you for writing and publishing this planner.

[croesmann]

With the last commit (cdad3e36574281a2bf722911e1889c765468c669) I finally added the possibility to define other (customized) robot shapes (considered for optimization!) using the ROS parameter server.

I would be really interested in knowing if it solves your problems mentioned above @orhangazi44 . Is there any chance that you have time to test it with the new version of the "master" branch?

I will also test the new version before closing this issue and releasing a new version of teb_local_planner.

Checkout the following new tutorial for configuring the footprint model: Obstacle Avoidance and Robot Footprint Model

[cosmicog]

Hi Sir.

I'll try it first at the start of the working hour tomorrow.

[cosmicog]

Hi,

Sorry for late response, my boss gave another job at this morning :/ I've just finished trying two_circles, polygon, and line footprint models. But sorry, Robot is hitting the wall when start point is close the wall(Robot's side is parallel to wall). I'll try more configuration in simulation at this night at home.

Thank You.

[croesmann]

Thanks!

I found another possible cause for the issue. The strategy of incorporating costmap cells as obstacles behind the robot was not quite good. I ignored points that were located behind the goal-heading with more than a (small) hardcoded distance. Referring to your example images, this might exclude the wall from possible obstacle candidates (goal heading is 90° rotated w.r.t. robot pose!).

The new strategy (commit bc253c3c27a5d652a5f39ea83620a14c5753fb7d) should resolve the problem. Using the new parameter costmap_obstacles_behind_robot_dist, the area of incorporating obstacles behind the robot can be controlled. Just check _rqtreconfigure. The default value of 1.5m should work in most cases.

With the robot footprint and those new changes, my turtlebot navigates perfectly close to walls as long as the goal pose is physically reachable ;-)

Please let me know, if it works for you.

ps: do not forget to reduce the value of costmap_obstacle_dist (EDIT: sry, I meant min_obstacle_dist) if you add a proper footprint (see Tutorial).

[cosmicog]

:+1: My robot navigates perfectly too :-) (video)

Now, teb_local_planner is navigating my robot with two_circles model, like an expert, 40 years old, bus driver :) (_In video, footprint is coming late because of my robot needs some performance tuning in other packages. It is not about teb_local_planner. I have already tuned it via its tutorials before._)

I didn't find costmap_obstacle_dist parameter, increased min_obstacle_dist. But it causes struggling when goal point is close to an obstacle. Here is second video about differences between 0.6 and 0.3 values of min_obstacle_dist parameter.

However I think new commit(bc253c3) and its costmap_obstacles_behind_robot_dist parameter are solved this problem.

If turtle goes to somewhere with a safety, I'm closing this issue immediately :-) Thank You again!