teb tuning

teb

TebLocalPlannerROS:

 # Misc
 odom_topic: odom                               # default odom
 map_frame: map                                 # default odom, 这里其实没用, 会使用global_frame 覆盖cfg_.map_frame
    
 # Trajectory
  
 teb_autosize: True
 dt_ref: 0.3            						#轨迹的局部分辨率,m
 dt_hysteresis: 0.1     						#dt_ref +-dt_hysteresis
 max_samples: 500       						# wiki not include ??
 min_samples:3         							#int, default: 3
 global_plan_overwrite_orientation: True
 allow_init_with_backwards_motion: False
 max_global_plan_lookahead_dist: 2.0	        #localcost half size
 global_plan_viapoint_sep: -1                   #负数是禁用; 如果正, 路径跟随模式时,决定了全局路径的分辨率? 与weight_viapoint 有关, defines the minimum separation between two consecutive via-points along the global plan (in meters). E.g. by setting the value to 0.5, 
 global_plan_prune_distance: 1
 exact_arc_length: False                        # 默认false, 用于欧几里得近似, 如果为真，规划器在速度、加速度和转弯率计算中使用精确的弧长[->增加的CPU时间]，否则使用欧几里德近似
 feasibility_check_no_poses: 4                  # 预测plan 上哪些点检查可行性
 publish_feedback: False                        # 默认false; True 用来检查完整轨迹和激活的障碍物, 看publisher 即可
 shrink_horizon_backup: True               # 当planner infeasibility时候允许临时缩小视野50%
 shrink_horizon_min_duration: 10           #允许缩小视野的最小时间
    
 # 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"                       # ~/teb_markers 查看footprint rviz

 # GoalTolerance
    
 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.1
 free_goal_vel: False                   # default false, 机器人朝goal以zero速度驶向目的地
 complete_global_plan: True
    
 # Obstacles
    
 min_obstacle_dist: 0.25 # This value must also include our robot radius, since footprint_model is set to "point"; If model is Circle this valude should subtract radius
 inflation_dist: 0.6     # a little bigger than min_obstacle_dist
 include_costmap_obstacles: True   # 默认True, 与costmap_conventor 话题/obstacles 有关
 costmap_obstacles_behind_robot_dist: 1.2
 obstacle_poses_affected: 30       #障碍物与附近路径点的关联影响, 策略从kinetic 版本已经更新, 该接口已经废弃, 只有legacy_obstacle_association 为True 时, 采取旧障碍物关联策略

 dynamic_obstacle_inflation_dist: 0.6      #wiki not include ??
 include_dynamic_obstacles: False    #默认false, 如果为True, 与costmap_conventer /obstacles 有关

 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5

 # legacy obstacle and pose association strategy
 legacy_obstacle_association: false     #默认false, kinetic 之前版本的策略, 新版本默认不用
 obstacle_poses_affected: 30            #legacy 对应
 obstacle_association_force_inclusion_factor: 1.5 #non-legacy, 即新策略, 2*min_obstacle_dist 之内的障碍物关联优化
 obstacle_association_cutoff_factor: 5  #non-legay, 即新策略, 超过5*min_obstacle_dist 的障碍物忽略
 

 # 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     #only for carlike
 weight_optimaltime: 1 # must be > 0
 weight_shortest_path: 0
 weight_obstacle: 100
 weight_inflation: 0.2
 weight_dynamic_obstacle: 10
 weight_dynamic_obstacle_inflation: 0.2
 weight_viapoint: 1                      # path following mode
 weight_adapt_factor: 2
 
 optimization_activate: False            # 激活优化
 optimization_verbose: False             # 默认假

 # Homotopy Class Planner

 enable_homotopy_class_planning: True    # default true, 消耗更多CPU, 别用了
 enable_multithreading: True
 max_number_classes: 4                   # 计算不过来的时候, 设为2
 selection_cost_hysteresis: 1.0
 selection_prefer_initial_plan: 0.9
 selection_obst_cost_scale: 100.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
 
 oscillation_recovery: True
 oscillation_v_eps: 0.1
 oscillation_omega_eps: 0.1
 oscillation_recovery_min_duration: 10
 oscillation_filter_duration: 10

pal-teb-param
robotniq

teb 规划层面全部基于odom坐标系

    ros::NodeHandle nh("~/" + name);
    cfg_.loadRosParamFromNodeHandle(nh);

    ROS_INFO("teb0, global_frame %s", global_frame_.c_str());  // odom
    ROS_INFO("teb0, cfg_.map_frame %s", cfg_.map_frame.c_str()); // map

    //
    global_frame_ = costmap_ros_->getGlobalFrameID();
    cfg_.map_frame = global_frame_;
    robot_base_frame_ = costmap_ros_->getBaseFrameID();

    ROS_INFO("teb1, global_frame %s", global_frame_.c_str());  // local_costmap->odom
    ROS_INFO("teb1, cfg_.map_frame %s", cfg_.map_frame.c_str()); // reset map frame to odom

1 global_plan prune 可视化

// global_pose in odom
bool TebLocalPlannerROS::pruneGlobalPlan(const tf2_ros::Buffer& tf, const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& global_plan, double dist_behind_robot)
{ 
   
  if (global_plan.empty())
    return true;

  try
  { 
   
    // transform robot pose into the plan frame (we do not wait here, since pruning not crucial, if missed a few times), map -> odom
    geometry_msgs::TransformStamped global_to_plan_transform = tf.lookupTransform(global_plan.front().header.frame_id, global_pose.header.frame_id, ros::Time(0));
    geometry_msgs::PoseStamped robot;  // global_pose in map
    tf2::doTransform(global_pose, robot, global_to_plan_transform);

    double dist_thresh_sq = dist_behind_robot*dist_behind_robot;

    // iterate plan until a pose close the robot is found, 统一在map frame
    std::vector<geometry_msgs::PoseStamped>::iterator it = global_plan.begin();
    std::vector<geometry_msgs::PoseStamped>::iterator erase_end = it;
    while (it != global_plan.end())
    { 
   
      double dx = robot.pose.position.x - it->pose.position.x;
      double dy = robot.pose.position.y - it->pose.position.y;
      double dist_sq = dx * dx + dy * dy;
      if (dist_sq < dist_thresh_sq)
      { 
   
         erase_end = it;  // 如果第一个点<阈值, 后面不进行erase;
         break;           // 相反, 当机器人走远后, 找出erase 边界
      }
      ++it;
    }
    if (erase_end == global_plan.end())
      return false;

    if (erase_end != global_plan.begin())
      global_plan.erase(global_plan.begin(), erase_end);
  }
  catch (const tf::TransformException& ex)
  { 
   
    ROS_DEBUG("Cannot prune path since no transform is available: %s\n", ex.what());
    return false;
  }
  return true;
}

2 transformGlobalPlan, 将全局path 切换到 odom frame

// global_pose , 当前odom pose in odom frame
// global frame, 还是odom
// max_plan_length, 即max_global_plan_lookahead_dist, 不超过2m
// current_goal_idx
// tf_plan_to_global, odom -> map, 将global plan map点, 转成odom point, 填充transformed_plan
bool TebLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
                  const geometry_msgs::PoseStamped& global_pose, const costmap_2d::Costmap2D& costmap, const std::string& global_frame, double max_plan_length,
                  std::vector<geometry_msgs::PoseStamped>& transformed_plan, int* current_goal_idx, geometry_msgs::TransformStamped* tf_plan_to_global) const
{ 
   
  // this method is a slightly modified version of base_local_planner/goal_functions.h

  const geometry_msgs::PoseStamped& plan_pose = global_plan[0];

  transformed_plan.clear();

  try
  { 
   
    if (global_plan.empty())
    { 
   
      ROS_ERROR("Received plan with zero length");
      *current_goal_idx = 0;
      return false;
    }

    // get plan_to_global_transform from plan frame to global_frame
    // odom -> map (child)
    geometry_msgs::TransformStamped plan_to_global_transform = tf.lookupTransform(global_frame, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp,
                                                                                  plan_pose.header.frame_id, ros::Duration(0.5));

    //let's get the pose of the robot in the frame of the plan, get current pose in map frame
    geometry_msgs::PoseStamped robot_pose;  // 当前map下位置
    tf.transform(global_pose, robot_pose, plan_pose.header.frame_id);

    //we'll discard points on the plan that are outside the local costmap
    double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
                                     costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
    dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are
                           // located on the border of the local costmap


    int i = 0; // 寻找global path 距离现在最近的点
    double sq_dist_threshold = dist_threshold * dist_threshold;
    double sq_dist = 1e10;

    //we need to loop to a point on the plan that is within a certain distance of the robot
    // robot_pose, 当下map位姿, 找到全局路径距离当前位置最近的点
    for(int j=0; j < (int)global_plan.size(); ++j)
    { 
   
      double x_diff = robot_pose.pose.position.x - global_plan[j].pose.position.x;
      double y_diff = robot_pose.pose.position.y - global_plan[j].pose.position.y;
      double new_sq_dist = x_diff * x_diff + y_diff * y_diff;
      if (new_sq_dist > sq_dist_threshold) 
        break;  // force stop if we have reached the costmap border, 2 * 0.85 = 1.7

      if (new_sq_dist < sq_dist) // find closest distance
      { 
   
        sq_dist = new_sq_dist;
        i = j;
      }
    }

    geometry_msgs::PoseStamped newer_pose;  // in map

    double plan_length = 0; // check cumulative Euclidean distance along the plan

    //now we'll transform until points are outside of our distance threshold
    // i 之前找到全局path, 距离当前最近的点index, 然后根据 plan_length 找出carrot goal
    while(i < (int)global_plan.size() && sq_dist <= sq_dist_threshold && (max_plan_length<=0 || plan_length <= max_plan_length))
    { 
   
      const geometry_msgs::PoseStamped& pose = global_plan[i];
      tf2::doTransform(pose, newer_pose, plan_to_global_transform);

      transformed_plan.push_back(newer_pose); // in map

      double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x;
      double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y;
      sq_dist = x_diff * x_diff + y_diff * y_diff;

      // caclulate distance to previous pose
      if (i>0 && max_plan_length>0)
        plan_length += distance_points2d(global_plan[i-1].pose.position, global_plan[i].pose.position);

      ++i;
    }
    // i 变成了carrot goal
    // if we are really close to the goal (<sq_dist_threshold) and the goal is not yet reached (e.g. orientation error >>0)
    // the resulting transformed plan can be empty. In that case we explicitly inject the global goal.
    if (transformed_plan.empty())
    { 
   
      tf2::doTransform(global_plan.back(), newer_pose, plan_to_global_transform);

      transformed_plan.push_back(newer_pose);

      // Return the index of the current goal point (inside the distance threshold)
      if (current_goal_idx) *current_goal_idx = int(global_plan.size())-1;
    }
    else
    { 
   
      // Return the index of the current goal point (inside the distance threshold)
      if (current_goal_idx) *current_goal_idx = i-1; // subtract 1, since i was increased once before leaving the loop
    }

    // Return the transformation from the global plan to the global planning frame if desired
    if (tf_plan_to_global) *tf_plan_to_global = plan_to_global_transform;
  }
  catch(tf::LookupException& ex)
  { 
   
    ROS_ERROR("No Transform available Error: %s\n", ex.what());
    return false;
  }
  catch(tf::ConnectivityException& ex)
  { 
   
    ROS_ERROR("Connectivity Error: %s\n", ex.what());
    return false;
  }
  catch(tf::ExtrapolationException& ex)
  { 
   
    ROS_ERROR("Extrapolation Error: %s\n", ex.what());
    if (global_plan.size() > 0)
      ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());

    return false;
  }

  return true;
}