ROS导航之base_local_planner

---

   局部规划模块的设计思路为，根据机器人当前位置（由costmapros提供）和速度（通过订阅odom消息提供），在速度和加速度以及模拟前进时间的限制下进行速度采样，并假设在此速度下行进模拟时长（sim_time）的时间。 然后根据时间间隔计算每一步的位置，根据经历位置的损失值之和计算轨迹的损失值。
   关于损失值的计算，局部规划模块使用了三张图来保存损失值信息，对应三种类型的损失值，分别是pathgrid、goalgrid、costmap， 其中，pathgrid保存的是局部窗口中每一个点到全局路径的距离损失， goalgrid存放的是局部窗口中每一个位置到目标点（全局路径被局部窗口截取的最后一个点）的损失。costmap提供了地图上的障碍物信息了，根据之可以计算每个点的障碍物损失值，具体可详见上一讲。在计算轨迹上每个点的损失时，是按照一定的比例将它们进行加成的，而控制的参数下面会做详细解释。

---

TrajectoryPlannerROS类

   A ROS wrapper for the trajectory controller that queries the param server to construct a controller.
1. 构造函数
   TrajectoryPlannerROS(std::string name,tf::TransformListener* tf,costmap_2d::Costmap2DROS* costmap_ros);
需要的参数是对坐标系的监听和局部地图。
2. move_base接口
   计算速度函数，主要入口
   bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
   传入全局路径，供内部计算更新pathGrid
   bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
   检查是否到达目标
   bool isGoalReached();

---

TrajectoryPlanner类

关键函数：

Trajectory findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,tf::Stamped<tf::Pose>& drive_velocities);

Trajectory createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta, double acc_x, double acc_y, double acc_theta);

void generateTrajectory(double x, double y, double theta, double vx, double vy, double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y,double acc_theta, double impossible_cost, Trajectory& traj);

以上三个函数的作用大概就是对速度进行采样，并对采样速度和模拟时间生成轨迹，评价轨迹得分，返回损失最少的轨迹作为返回，同时返回对应的最佳速度（包括线速度和角速度）。

---

参数详解

/*
   * @param world_model The WorldModel the trajectory controller uses to check for collisions 
   * @param costmap A reference to the Costmap the controller should use
   * @param footprint_spec A polygon representing the footprint of the robot. (Must be convex)， 该参数可以从paraserver中读取，也可以从costmap中获得。
   * @param inscribed_radius The radius of the inscribed circle of the robot
   * @param circumscribed_radius The radius of the circumscribed circle of the robot， 机器人内外半径
   * @param acc_lim_x The acceleration limit of the robot in the x direction
   * @param acc_lim_y The acceleration limit of the robot in the y direction
   * @param acc_lim_theta The acceleration limit of the robot in the theta direction
   * 加速度限制，值得注意的是，加速度的设置是配合速度的限制共同起作用的，加入加速度设置的很小，那么在一定时间内无法减速到采样的速度，那么就会以较大的速度继续行进。
   * @param sim_time The number of seconds to "roll-out" each trajectory
   * @param sim_granularity The distance between simulation points should be small enough that the robot doesn't hit things，时间采样间隔，越小说明要计算的轨迹点越多，计算负荷也会随之增加，但是可以增加轨迹损失计算的精确性。
   * @param vx_samples The number of trajectories to sample in the x dimension
   * @param vtheta_samples The number of trajectories to sample in the theta dimension
   * 速度空间的采样样本数，越多的话采样出来的轨迹会越多，但是相应的会增加计算负荷
   * @param pdist_scale A scaling factor for how close the robot should stay to the path， 损失值中到全局路径的损失权重，越大表明更多的靠近全局路径行进
   * @param gdist_scale A scaling factor for how aggresively the robot should pursue a local goal， 到局部目标点的损失权重，不能太小，否则机器人不往前走了。
   * @param occdist_scale A scaling factor for how much the robot should prefer to stay away from obstacles
   *  到障碍物的损失。 具体是对轨迹上每一个点，检查机器人脚印在障碍物图层的损失值，如果脚印与障碍物和unknown区域相交，则返回负值表示轨迹非法，否则返回costmap相应位置损失值的总和。
   * @param heading_lookahead How far the robot should look ahead of itself when differentiating between different rotational velocities
   * @param oscillation_reset_dist The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past
   * @param escape_reset_dist The distance the robot must travel before it can exit escape mode
   * @param escape_reset_theta The distance the robot must rotate before it can exit escape mode
   * @param holonomic_robot Set this to true if the robot being controlled can take y velocities and false otherwise
   * @param max_vel_x The maximum x velocity the controller will explore
   * @param min_vel_x The minimum x velocity the controller will explore
   * @param max_vel_th The maximum rotational velocity the controller will explore
   * @param min_vel_th The minimum rotational velocity the controller will explore
   * 这几个值不能设置得太小，否则的话转弯的时候很有可能转不过去，只能一直撞墙，这里我吃过大亏的。
   * @param min_in_place_vel_th The absolute value of the minimum in-place rotational velocity the controller will explore
   * @param backup_vel The velocity to use while backing up
   * @param dwa Set this to true to use the Dynamic Window Approach, false to use acceleration limits
   * @param heading_scoring Set this to true to score trajectories based on the robot's heading after 1 timestep
   * @param heading_scoring_timestep How far to look ahead in time when we score heading based trajectories
   * @param meter_scoring adapt parameters to costmap resolution
   * @param simple_attractor Set this to true to allow simple attraction to a goal point instead of intelligent cost propagation
   * @param y_vels A vector of the y velocities the controller will explore
   * @param angular_sim_granularity The distance between simulation points for angular velocity should be small enough that the robot doesn't hit things
   */

---

调试pdsit，gdist,以及occdist的参数时，可以将publish_cost_grid参数设置为true，这样就可以愉快地在rviz中观察设置的合理与否了。