ROS的navigation stack中區域性規劃器的介面plugin類為nav_core::BaseLocalPlanner，給出的包有4種：

• base_local_planner - 提供了 Dynamic Window Approach(DWA)和Trajectory Rollout方法來區域性控制；
• dwa_local_planner - 模組化DWA方法應用，相比base_local_planner具有更多的恢復機制、更便於理解的介面和針對全向運動機器人更靈活的Y軸變數控制；
• eband_local_planner -運用了SE(2)流形上Elastic Band 方法；
• teb_local_planner
  - 應用 Timed-Elastic-Band 方法於線上軌跡優化。

本文將學習base_local_planner的TrajectoryPlannerROS類，對其兩種規劃演算法進行分析。該類使用的引數很多，先分析ros引數伺服器能配置的一些引數含義，再學習兩個主要函式initialize和computeVelocityCommands。

1 配置引數

此部分摘自官網wiki，引數可分為： robot configuration, goal tolerance, forward simulation, trajectory scoring, oscillation prevention, and global plan，對其中不能明顯看出的含義的進行了註釋。

機器人配置引數 Robot Configuration Parameters

• ~/acc_lim_x (double, default: 2.5) The x acceleration limit of the robot in meters/sec^2
• ~/acc_lim_y (double, default: 2.5) The y acceleration limit of the robot in meters/sec^2
• ~/acc_lim_theta (double, default: 3.2) The rotational acceleration limit of the robot in radians/sec^2
• ~/max_vel_x (double, default: 0.5) The maximum forward velocity allowed for the base in meters/sec
• ~/min_vel_x (double, default: 0.1) 最小速度：保證能夠克服摩擦力 The minimum forward velocity allowed for the base in meters/sec. It is useful to specify this to guarantee that velocity commands sent to a mobile base are high enough to allow the base to overcome friction.
• ~/max_vel_theta (double, default: 1.0) The maximum rotational velocity allowed for the base in radians/sec
• ~/min_vel_theta (double, default: -1.0) The minimum rotational velocity allowed for the base in radians/sec
• ~/min_in_place_vel_theta (double, default: 0.4) 原地旋轉最小角速度 The minimum rotational velocity allowed for the base while performing in-place rotations in radians/sec
• ~/backup_vel (double, default: -0.1) 已棄用，用escape_vel代替了 DEPRECATED (use escape_vel):Speed used for backing up during escapes in meters/sec. Note that it must be negative in order for the robot to actually reverse. A positive speed will cause the robot to move forward while attempting to escape.
• ~/escape_vel (double, default: -0.1) 後退速度：用於恢復行為時的速度，為負值 Speed used for driving during escapes in meters/sec. Note that it must be negative in order for the robot to actually reverse. A positive speed will cause the robot to move forward while attempting to escape. New in navigation 1.3.1
• ~/holonomic_robot (bool, default: true)是否為全向機器人，可以給定Y向速度 Determines whether velocity commands are generated for a holonomic or non-holonomic robot. For holonomic robots, strafing velocity commands may be issued to the base. For non-holonomic robots, no strafing velocity commands will be issued.

The following parameters are only used if holonomic_robot is set to true:

• ~/y_vels (list, default: [-0.3, -0.1, 0.1, 0.3])全向移動機器人y速度列表，用於TODO The strafing velocities that a holonomic robot will consider in meters/sec

全域性閾值引數 Goal Tolerance Parameters

• ~/yaw_goal_tolerance (double, default: 0.05) The tolerance in radians for the controller in yaw/rotation when achieving its goal
• ~/xy_goal_tolerance (double, default: 0.10) The tolerance in meters for the controller in the x & y distance when achieving a goal
• ~/latch_xy_goal_tolerance (bool, default: false) 鎖住xy_goal_tolerance閾值：TODO If goal tolerance is latched, if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so. - New in navigation 1.3.1

前進估計引數 Forward Simulation Parameters

• ~/sim_time (double, default: 1.0) 預估未來多長時間內的軌跡 The amount of time to forward-simulate trajectories in seconds
• ~/sim_granularity (double, default: 0.025)估計步長TODO The step size, in meters, to take between points on a given trajectory
• ~/angular_sim_granularity (double, default: ~/sim_granularity)TODO The step size, in radians, to take between angular samples on a given trajectory. - New in navigation 1.3.1
• ~/vx_samples (integer, default: 3) The number of samples to use when exploring the x velocity space
• ~/vtheta_samples (integer, default: 20) The number of samples to use when exploring the theta velocity space
• ~/controller_frequency (double, default: 20.0) 不設定會讀取父名稱空間的該引數，比如用move_base則可以不設定該引數 The frequency at which this controller will be called in Hz. Uses searchParam to read the parameter from parent namespaces if not set in the namespace of the controller. For use with move_base, this means that you only need to set its “controller_frequency” parameter and can safely leave this one unset. - New in navigation 1.3.1

路徑評分引數 Trajectory Scoring Parameters

The cost function used to score each trajectory is in the following form:

cost = 
  pdist_scale * (distance to path from the endpoint of the trajectory in map cells or meters depending on the meter_scoring parameter) 
  + gdist_scale * (distance to local goal from the endpoint of the trajectory in map cells or meters depending on the meter_scoring parameter) 
  + occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0-254))

• ~/meter_scoring (bool, default: false)以米為距離單位評分，決定gdist_scale和pdist_scale中距離的單位 Whether the gdist_scale and pdist_scale parameters should assume that goal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default. New in navigation 1.3.1
• ~/pdist_scale (double, default: 0.6)全域性路徑權重 The weighting for how much the controller should stay close to the path it was given, maximal possible value is 5.0
• ~/gdist_scale (double, default: 0.8)區域性規劃權重 The weighting for how much the controller should attempt to reach its local goal, also controls speed, maximal possible value is 5.0
• ~/occdist_scale (double, default: 0.01)避障權重 The weighting for how much the controller should attempt to avoid obstacles
• ~/heading_lookahead (double, default: 0.325)TODO How far to look ahead in meters when scoring different in-place-rotation trajectories
• ~/heading_scoring (bool, default: false)是否以機器人與全域性路徑的朝向和距離來評分 Whether to score based on the robot’s heading to the path or its distance from the path
• ~/heading_scoring_timestep (double, default: 0.8)TODO How far to look ahead in time in seconds along the simulated trajectory when using heading scoring
• ~/dwa (bool, default: true)採用dwa方法 Whether to use the Dynamic Window Approach (DWA)_ or whether to use Trajectory Rollout (NOTE: In our experience DWA worked as well as Trajectory Rollout and is computationally less expensive. It is possible that robots with extremely poor acceleration limits could gain from running Trajectory Rollout, but we recommend trying DWA first.) 注：在我們試驗中，DWA方法和Trajectory Rollout方法工作相當，但是計算量更小。沒有加速度限制的機器人採用Trajectory Rollout方法可能有益處，但是推薦使用DWA
• ~/publish_cost_grid_pc (bool, default: false)是否釋出代價地圖 Whether or not to publish the cost grid that the planner will use when planning. When true, a sensor_msgs/PointCloud2 will be available on the ~/cost_cloud topic. Each point cloud represents the cost grid and has a field for each individual scoring function component as well as the overall cost for each cell, taking the scoring parameters into account. New in navigation 1.4.0
• ~/global_frame_id (string, default: odom) The frame to set for the cost_cloud. Should be set to the same frame as the local costmap’s global frame. New in navigation 1.4.0

防止振盪引數 Oscillation Prevention Parameters

• ~/oscillation_reset_dist (double, default: 0.05)機器人至少走多遠振盪標誌位才復位 How far the robot must travel in meters before oscillation flags are reset

全域性規劃路徑引數 Global Plan Parameters

• ~/prune_plan (bool, default: true)TODO Defines whether or not to eat up the plan as the robot moves along the path. If set to true, points will fall off the end of the plan once the robot moves 1 meter past them.

初始化函式initialize