move_ba的局部路径规划代码研究
ba_local_planner
Given a plan to follow and a costmap, the controller produces velocity commands to nd to a mobile ba.
他的功能是给⼀个global plan和local costmap，局部路径规划器计算出可⾏的速度发送给机器⼈
ba_local_planner::TrajectoryPlanner provides implementations of the DWA and Trajectory Rollout
It should be possible to create custom local planners using the dwa_local_planner as template and just adding own cost functions or trajectory generators.
你可以参照DWA实现⾃⼰的局部规路径算法
算法主要是在局部的costmap中模拟计算沿着不同的⽅向进⾏定义的cost函数的⼤⼩，选择⼀个cost⼩的正的的⽅向前进。
主要是进⾏计算cost函数，每个cost可以有weight参数调整，这个可以算是灵活和也可以说是不稳定的因素(要⾃⼰调试)
ObstacleCostFunction
MapGridCostFunction
OscillationCostFunction
PreferForwardCostFunction
teb_local_planner
优化轨迹执⾏的时间，与障碍物的距离，满⾜最⼤的速度与加速度的要求
Support of holonomic robots is included since Kinetic
parameter
参数分为⼀下⼏类(记住有些参数他在ros wiki⾥⾯是没有说明的，在代码⾥⾯有的，不是所有的参数都可以通过rqt_reconfigure配置的，有很少的⼀部分是不⾏的):
所有的参数你都可以在teb_config.h中找到初始值和含义
robot configuration
跟机器⼈底盘是圆形，多边形，car-like有关，在后⾯的优化有⽤到，要设置正确
~<name>/max_vel_x_backwards (double, default: 0.2)
Maximum absolute translational velocity of the robot while driving backwards in meters/c. See optimization parameter weight_kinematics_forward_drive
goal tolerance
~<name>/xy_goal_tolerance (double, default: 0.2)
~<name>/yaw_goal_tolerance (double, default: 0.2)
#Remove the goal velocity constraint such that the robot can arrive at the goal with maximum speed
~<name>/free_goal_vel (bool, default: fal)
trajectory configuration
# 轨迹的空间分辨率，只是⼀个参考值，真实的分辨率跟别的还有关
~<name>/dt_ref (double, default: 0.3)
obstacles
#距离障碍物的最短距离
~<name>/min_obstacle_dist (double, default: 0.5)
#Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters).
~<name>/costmap_obstacles_behind_robot_dist (double, default: 1.0)
#障碍物会影响的po的个数,
#bool legacy_obstacle_association; //!< If true, the old association strategy is ud (for each obstacle, find the nearest TEB po), otherwi the new one (for each teb po, find only "relevant" obstacles). ~<name>/obstacle_pos_affected (int, default: 30)
陆游字
optimization
#只允许前进的权重
~
<name>/weight_kinematics_forward_drive (double, default: 1.0)
#远离障碍物⾄少min_obstacle_dist的权重所向披靡怎么读
~<name>/weight_obstacle (double, default: 50.0)
#紧跟global plan的权重
~<name>/weight_viapoint (double, default: 1.0)
parallel planning in distinctive topologies
#允许并进计算，消耗更多的计算资源
~<name>/enable_homotopy_class_planning (bool, default: true)
~<name>/enable_multithreading (bool, default: true)
#Specify how much trajectory cost must a new candidate a previously lected trajectory in order to be lected (lection if new_cost < old_cost*factor).
~<name>/lection_cost_hysteresis (double, default: 1.0)
#Extra scaling of obstacle cost terms just for lecting the 'best' candidate.
~<name>/lection_obst_cost_scale (double, default: 100.0)
#Extra scaling of via-point cost terms just for lecting the 'best' candidate. New in version 0.4
~<name>/lection_viapoint_cost_scale (double, default: 1.0)
miscellaneous parameters
code
void TebLocalPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)
{
// create the planner instance
if (cfg_.able_homotopy_class_planning)
{
planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));爱情攻略
ROS_INFO("Parallel planning in distinctive topologies enabled.");
}
el
{
planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
ROS_INFO("Parallel planning in distinctive topologies disabled.");
}
}
bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
// prune global plan to cut off parts of the past (spatially before the robot)
pruneGlobalPlan(*tf_, robot_po, global_plan_);
// Transform global plan to the frame of interest ( the local costmap)
if (!transformGlobalPlan(*tf_, global_plan_, robot_po, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist,
transformed_plan, &goal_idx, &tf_plan_to_global))
{
ROS_WARN("Could not transform the global plan to the frame of the controller");
return fal;
}
// check if we should enter any backup mode and apply ttings
configureBackupModes(transformed_plan, goal_idx);
updateObstacleContainerWithCostmap();
// Now perform the actual planning
bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);
bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_pos);
if (!planner_->getVelocityCommand(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z)){
}
}
bool TebOptimalPlanner::plan(const std::vector<geometry_msgs::PoStamped>& initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
if (!teb_.isInit()){
// init trajectory
teb_.initTEBtoGoal(initial_plan, cfg_->trajectory.dt_ref, cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); }
语录
el{
PoSE2 start_(initial_plan.front().po);
PoSE2 goal_(initial_plan.back().po);
if (teb_.sizePos()>0 && (goal_.position() - teb_.BackPo().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist) // actual warm start!
冷淡的反义词
teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB
el // goal too far away -> reinit
{
ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
teb_.clearTimedElasticBand();
teb_.initTEBtoGoal(initial_plan, cfg_->trajectory.dt_ref, true, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);
}
}
// now optimize
龙利鱼排return optimizeTEB(cfg_-&_inner_iterations, cfg_-&_outer_iterations);
}
bool TebOptimalPlanner::optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute
_cost_afterwards,
double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost)
{
for(int i=0; i<iterations_outerloop; ++i)
{
if (cfg_-&b_autosize)
teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples);
//构建图
success = buildGraph(weight_multiplier);
if (!success)
{
clearGraph();
return fal;
}
//优化图
success = optimizeGraph(iterations_innerloop, fal);
if (!success)
{
clearGraph();
return fal;
}
if (compute_cost_afterwards && i==iterations_outerloop-1) // compute cost vec only in the last iteration computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
clearGraph();
花枝鼠
weight_multiplier *= cfg_->optim.weight_adapt_factor;
}
}
bool TebOptimalPlanner::buildGraph(double weight_multiplier)
{
// add TEB vertices
AddTEBVertices();
// add Edges (local cost functions)
if (cfg_->obstacles.legacy_obstacle_association)
AddEdgesObstaclesLegacy(weight_multiplier);
el
AddEdgesObstacles(weight_multiplier);
//AddEdgesDynamicObstacles();
AddEdgesViaPoints();
AddEdgesVelocity();
AddEdgesAcceleration();
AddEdgesTimeOptimal();
if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0)
AddEdgesKinematicsDiffDrive(); // we have a differential drive robot
el
AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below. AddEdgesPreferRotDir();
}
bool TebOptimalPlanner::optimizeGraph(int no_iterations,bool clear_after)
{
if (cfg_->robot.max_vel_x<0.01)
{
ROS_WARN("optimizeGraph(): Robot Max Velocity is smaller than 0.01m/s. ");
if (clear_after) clearGraph();
return fal;
}
if (!teb_.isInit() || teb_.sizePos() < cfg_->trajectory.min_samples)
{
ROS_WARN("optimizeGraph(): TEB is empty or has too less elements. Skipping optimization.");
if (clear_after) clearGraph();
return fal;
}
// boost::shared_ptr<g2o::SparOptimizer> optimizer_; //!< g2o optimizer for trajectory optimization
optimizer_->tVerbo(cfg_->optim.optimization_verbo);
optimizer_->initializeOptimization();
int iter = optimizer_->optimize(no_iterations);
if(!iter)
{
ROS_ERROR("optimizeGraph(): Optimization failed! iter=%i", iter);
return fal;
}
if (clear_after) clearGraph();
}
g2o
boost::shared_ptr<g2o::SparOptimizer> TebOptimalPlanner::initOptimizer()
{
// Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe)
static boost::once_flag flag = BOOST_ONCE_INIT;
boost::call_once(®isterG2OTypes, flag);
// allocating the optimizer
boost::shared_ptr<g2o::SparOptimizer> optimizer = boost::make_shared<g2o::SparOptimizer>();
//typedef g2o::LinearSolverCSpar<TEBBlockSolver::PoMatrixType> TEBLinearSolver;
TEBLinearSolver* linearSolver = new TEBLinearSolver(); // e typedef in optimization.h
linearSolver->tBlockOrdering(true);
//typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > TEBBlockSolver;
TEBBlockSolver* blockSolver = new TEBBlockSolver(linearSolver);
左右脑的功能和作用g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(blockSolver);
optimizer->tAlgorithm(solver);
optimizer->initMultiThreading(); // required for >Eigen 3.1
return optimizer;
}
