teb tuning

teb wiki
teb wiki tutorial

TebLocalPlannerROS:# Miscodom_topic: odom                               # default odommap_frame: map                                 # default odom, 这里其实没用, 会使用global_frame 覆盖cfg_.map_frame# Trajectoryteb_autosize: Truedt_ref: 0.3                                   #轨迹的局部分辨率,mdt_hysteresis: 0.1                           #dt_ref +-dt_hysteresismax_samples: 500                            # wiki not include ??min_samples:3                                  #int, default: 3global_plan_overwrite_orientation: Trueallow_init_with_backwards_motion: Falsemax_global_plan_lookahead_dist: 2.0           #localcost half sizeglobal_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: 1exact_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           #允许缩小视野的最小时间# Robotmax_vel_x: 0.4max_vel_x_backwards: 0.2max_vel_y: 0.0max_vel_theta: 0.3acc_lim_x: 0.5acc_lim_theta: 0.5min_turning_radius: 0.0 # diff-drive robot (can turn on place!)footprint_model:type: "point"                       # ~/teb_markers  查看footprint rviz# GoalTolerancexy_goal_tolerance: 0.2yaw_goal_tolerance: 0.1free_goal_vel: False                   # default false, 机器人朝goal以zero速度驶向目的地complete_global_plan: True# Obstaclesmin_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 radiusinflation_dist: 0.6     # a little bigger than min_obstacle_distinclude_costmap_obstacles: True   # 默认True, 与costmap_conventor 话题/obstacles 有关costmap_obstacles_behind_robot_dist: 1.2obstacle_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: Truecostmap_converter_rate: 5# legacy obstacle and pose association strategylegacy_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 的障碍物忽略# Optimizationno_inner_iterations: 5no_outer_iterations: 4optimization_activate: Trueoptimization_verbose: Falsepenalty_epsilon: 0.1obstacle_cost_exponent: 4weight_max_vel_x: 2weight_max_vel_theta: 1weight_acc_lim_x: 1weight_acc_lim_theta: 1weight_kinematics_nh: 1000              #按照非全向运动学来weight_kinematics_forward_drive: 1      #允许后退weight_kinematics_turning_radius: 1     #only for carlikeweight_optimaltime: 1 # must be > 0weight_shortest_path: 0weight_obstacle: 100weight_inflation: 0.2weight_dynamic_obstacle: 10weight_dynamic_obstacle_inflation: 0.2weight_viapoint: 1                      # path following modeweight_adapt_factor: 2optimization_activate: False            # 激活优化optimization_verbose: False             # 默认假# Homotopy Class Plannerenable_homotopy_class_planning: True    # default true, 消耗更多CPU, 别用了enable_multithreading: Truemax_number_classes: 4                   # 计算不过来的时候, 设为2selection_cost_hysteresis: 1.0selection_prefer_initial_plan: 0.9selection_obst_cost_scale: 100.0selection_alternative_time_cost: Falseroadmap_graph_no_samples: 15roadmap_graph_area_width: 5roadmap_graph_area_length_scale: 1.0h_signature_prescaler: 0.5h_signature_threshold: 0.1obstacle_heading_threshold: 0.45switching_blocking_period: 0.0viapoints_all_candidates: Truedelete_detours_backwards: Truemax_ratio_detours_duration_best_duration: 3.0visualize_hc_graph: Falsevisualize_with_time_as_z_axis_scale: False# Recoveryoscillation_recovery: Trueoscillation_v_eps: 0.1oscillation_omega_eps: 0.1oscillation_recovery_min_duration: 10oscillation_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());  // odomROS_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->odomROS_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 -> odomgeometry_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 maptf2::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 framestd::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.hconst 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 framegeometry_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 costmapdouble 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 costmapint 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.7if (new_sq_dist < sq_dist) // find closest distance{sq_dist = new_sq_dist;i = j;}}geometry_msgs::PoseStamped newer_pose;  // in mapdouble 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 goalwhile(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 mapdouble 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 poseif (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 desiredif (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;
}

teb 修改版本

添加链接描述

csdn分析与改进

qqfly

防止后退

teb tuning相关推荐

ROS Navigation Tuning Guide（导航调试指南）
ROS Navigation Tuning Guide 导航调试指南准备工作距离传感器里程计定位速度与加速度的设置获得最大速度获得最大加速度设置最小值 XY方向的速度 Global P ...
Oracle SQL Tuning Advisor 测试
如果面对一个需要优化的SQL语句,没有很好的想法,可以先试试Oracle的SQL Tuning Advisor. SQL> select * from v$version;BANNER ---- ...
Utilize Sql Tuning Advisor from Script
Sql Tuning Advisor是10g以后出现的一个十分有用的调优工具,大多数情况下我们可以通过dbconsole或者Grid Control的web界面调用SQL Advisor:但如果系统中 ...
R语言使用caret包对GBM模型进行参数调优实战：Model Training and Parameter Tuning
R语言使用caret包对GBM模型进行参数调优实战:Model Training and Parameter Tuning 目录 R语言使用caret包对GBM模型进行参数调优实战:Model Tra ...
语义分割--Mix-and-Match Tuning for Self-Supervised Semantic Segmentation
Mix-and-Match Tuning for Self-Supervised Semantic Segmentation AAAI Conference on Artificial Intelli ...
直播活动丨BMMeetup第1期：大模型Prompt Tuning技术，8场学术报告和Poster提前下载...
「Big Model Meetup」系列活动是由智源研究院悟道团队.青源会.清华大学计算机科学与技术系博硕论坛.中国中文信息学会青年工作委员会共同组织,智源社区提供社区支持,PaperWeekly提供 ...
线下活动丨Big Model Meetup 第1期：大模型Prompt Tuning技术，8场学术报告和Poster展示...
「Big Model Meetup」系列活动是由智源研究院悟道团队.青源会.清华大学计算机科学与技术系博硕论坛.中国中文信息学会青年工作委员会共同组织,智源社区提供社区支持,PaperWeekly提供 ...
Pivotal Tuning for Latent-based Editing of Real Images
近日,一篇关于StyleGAN的论文引起了讨论.该技术在保留源图像人物的同时,在细节编辑上实现了更逼真的效果. StyleGAN越玩越出色! 近日,以色列特拉维夫大学的研究人员对StyleGAN添加了 ...
SQL Tuning 基础概述10
在<SQL Tuning 基础概述05 - Oracle 索引类型及介绍>的1.5小节,提到了几种"索引的常见执行计划": INDEX FULL SCAN:索引的全扫描 ...

teb tuning

1 global_plan prune 可视化

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

teb tuning相关推荐

最新文章

热门文章