base_local_planner
Given a plan to follow and a costmap, the controller produces velocity commands to send to a mobile base.
他的功能是給一個global plan和local costmap，局部路徑規划器計算出可行的速度發送給機器人
base_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/sec. 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: false)
- 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)
#障礙物會影響的pose的個數,
#bool legacy_obstacle_association; //!< If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only "relevant" obstacles).
~<name>/obstacle_poses_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 have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor).
~<name>/selection_cost_hysteresis (double, default: 1.0)
#Extra scaling of obstacle cost terms just for selecting the 'best' candidate.
~<name>/selection_obst_cost_scale (double, default: 100.0)
#Extra scaling of via-point cost terms just for selecting the 'best' candidate. New in version 0.4
~<name>/selection_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_.hcp.enable_homotopy_class_planning)
{
planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
ROS_INFO("Parallel planning in distinctive topologies enabled.");
}
else
{
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_pose, global_plan_);
// Transform global plan to the frame of interest (w.r.t. the local costmap)
if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *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 false;
}
// check if we should enter any backup mode and apply settings
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_poses);
if (!planner_->getVelocityCommand(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z)){
}
}
bool TebOptimalPlanner::plan(const std::vector<geometry_msgs::PoseStamped>& 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);
}
else{
PoseSE2 start_(initial_plan.front().pose);
PoseSE2 goal_(initial_plan.back().pose);
if (teb_.sizePoses()>0 && (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist) // actual warm start!
teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB
else // 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_->optim.no_inner_iterations, cfg_->optim.no_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_->trajectory.teb_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 false;
}
//優化圖
success = optimizeGraph(iterations_innerloop, false);
if (!success)
{
clearGraph();
return false;
}
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);
else
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
else
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. Optimizing aborted...");
if (clear_after) clearGraph();
return false;
}
if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples)
{
ROS_WARN("optimizeGraph(): TEB is empty or has too less elements. Skipping optimization.");
if (clear_after) clearGraph();
return false;
}
// boost::shared_ptr<g2o::SparseOptimizer> optimizer_; //!< g2o optimizer for trajectory optimization
optimizer_->setVerbose(cfg_->optim.optimization_verbose);
optimizer_->initializeOptimization();
int iter = optimizer_->optimize(no_iterations);
if(!iter)
{
ROS_ERROR("optimizeGraph(): Optimization failed! iter=%i", iter);
return false;
}
if (clear_after) clearGraph();
}
g2o
boost::shared_ptr<g2o::SparseOptimizer> 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::SparseOptimizer> optimizer = boost::make_shared<g2o::SparseOptimizer>();
//typedef g2o::LinearSolverCSparse<TEBBlockSolver::PoseMatrixType> TEBLinearSolver;
TEBLinearSolver* linearSolver = new TEBLinearSolver(); // see typedef in optimization.h
linearSolver->setBlockOrdering(true);
//typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > TEBBlockSolver;
TEBBlockSolver* blockSolver = new TEBBlockSolver(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(blockSolver);
optimizer->setAlgorithm(solver);
optimizer->initMultiThreading(); // required for >Eigen 3.1
return optimizer;
}