Ask Your Question
0

teb_local_planner taking cornering in real life

asked 2021-01-06 20:50:53 -0600

darthShana gravatar image

Hi all

Im trying to get my car like robot to take a corner in real life. Im using the teb_local_planner. However it fails to take the corner in real life..

It seems to follow the plan on the way forward but it seems to go wrong when it reverses.. It then seem to go forward and back in a way which doesnt help taking the corner I have uploaded a video of r-viz here. I am running the google cartographer in localization mode. I do get a couple of warnings

[ WARN] [1607072269.122634379]: TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).

But im not sure if that relevant.. any help would be appreciated

My configuration is as follows

TebLocalPlannerROS:

 odom_topic: odom

 # Trajectory

 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 max_samples: 500
 global_plan_overwrite_orientation: True
 allow_init_with_backwards_motion: True
 max_global_plan_lookahead_dist: 3.0
 global_plan_viapoint_sep: -1
 global_plan_prune_distance: 1
 exact_arc_length: False
 feasibility_check_no_poses: 2
 publish_feedback: False

 # Robot

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

 # ********************** Carlike robot parameters ********************
 min_turning_radius: 1.5        # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
 wheelbase: 0.4                 # 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: "two_circles"
   radius: 0.2 # for type "circular"
   line_start: [0.0, 0.0] # for type "line"
   line_end: [0.4, 0.0] # for type "line"
   front_offset: 0.3 # for type "two_circles"
   front_radius: 0.2 # for type "two_circles"
   rear_offset: 0.0 # for type "two_circles"
   rear_radius: 0.2 # for type "two_circles"

   vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"

 # GoalTolerance

 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.1
 free_goal_vel: False
 complete_global_plan: True

 # Obstacles

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

 dynamic_obstacle_inflation_dist: 0.6
 include_dynamic_obstacles: True 

 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
 obstacle_cost_exponent: 4
 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 # must be > 0
 weight_shortest_path: 0
 weight_obstacle: 100
 weight_inflation: 0.2
 weight_dynamic_obstacle: 10 # not in use yet
 weight_dynamic_obstacle_inflation: 0.2
 weight_viapoint: 1
 weight_adapt_factor: 2

 # Homotopy Class Planner

 enable_homotopy_class_planning: True
 enable_multithreading: True
 max_number_classes: 4
 selection_cost_hysteresis: 1.0
 selection_prefer_initial_plan: 0.95
 selection_obst_cost_scale: 1.0
 selection_alternative_time_cost: False

 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 roadmap_graph_area_length_scale: 1.0
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_heading_threshold: 0.45
 switching_blocking_period: 0.0
 viapoints_all_candidates: True
 delete_detours_backwards: True
 max_ratio_detours_duration_best_duration: 3.0
 visualize_hc_graph: False
 visualize_with_time_as_z_axis_scale: False

# Recovery

 shrink_horizon_backup: True
 shrink_horizon_min_duration: 10
 oscillation_recovery: True
 oscillation_v_eps: 0 ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2021-01-17 14:53:29 -0600

soldierofhell gravatar image

updated 2021-01-17 14:54:03 -0600

The reverse steering angle sign should be already taken into account here. Maybe your vehicle has wrong interpretation. Inspect AckermannDrive message output

edit flag offensive delete link more
0

answered 2021-01-17 12:21:58 -0600

darthShana gravatar image

HI All.. I have managed to resolve the issue.. (at least i think)

What i had implemented was ros node with some code i found in the tutorial

  v = data.linear.x
  steering = convert_trans_rot_vel_to_steering_angle(v, data.angular.z, wheelbase)

  msg = AckermannDriveStamped()
  msg.header.stamp = rospy.Time.now()
  msg.header.frame_id = frame_id
  msg.drive.steering_angle = steering
  msg.drive.speed = v

  pub.publish(msg)

I simply added

if v > 0:
    steering = steering * -1

just to reverse the angle to the servo when travelling in reverse. I dont know if this is the correct way to resolve the issue.. certainly does not feel right.. But since i have made the change, the robot is working very well. Reaching all the goal posses i give it "in real life"

thanks

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2021-01-06 20:50:53 -0600

Seen: 263 times

Last updated: Jan 17