base_local_planner源码阅读笔记

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.

2 初始化函数initialize