前置知識:

costmap_2d::Costmap2DROS costmap;

costmap_2d::Costmap2DROS 是一個ROS包中提供的用于處理2D成本地圖的類。它是一個高級的接口，通常用于與ROS導(dǎo)航棧中的導(dǎo)航規(guī)劃器和本地路徑跟蹤器等模塊進行集成。

costmap 是一個指向 Costmap2DROS 對象的指針。通常，通過這個指針，你可以訪問與導(dǎo)航和路徑跟蹤相關(guān)的成本地圖信息，以及執(zhí)行一些與導(dǎo)航相關(guān)的操作，如查詢障礙物信息、獲取機器人的當前全局位置等。這個指針指向了整個ROS導(dǎo)航棧中的成本地圖管理器。

costmap_2d::Costmap2D costmap_map_;

costmap_2d::Costmap2D 是一個用于表示二維成本地圖的類，通常用于底層的地圖管理和處理。

costmap_map_ 是一個指向 Costmap2D 對象的指針。這個指針通常用于直接訪問成本地圖的數(shù)據(jù)，如地圖的大小、分辨率、柵格信息以及每個柵格的成本值等。這個指針通常用于執(zhí)行與成本地圖的低級操作，如路徑規(guī)劃和避障算法。

costmap->getOrientedFootprint(std::cector<geometry_msgs::Point>& oriented_footprint)

構(gòu)建機器人在當前姿態(tài)下的足跡，結(jié)果將存儲在參數(shù) oriented_footprint 中, 這個函數(shù)可以返回機器人當前位置的足跡, 獲取的是一個機器人在世界坐標系下的坐標, 通常用于碰撞檢測或生成局部代價地圖

costmap->getRobotFootprint()

獲取機器人的輪廓, 表現(xiàn)為一系列std::vector<geometry_msgs::Point>的形式

costmap->getRobotFootprintPolygon()

獲取機器人的輪廓, 表現(xiàn)為一個凸多邊形, geometry_msgs::Polygon 類型

initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros)

參數(shù):

name

tf

costmap_ros 一個ROS的上層接口

發(fā)布者:

global_plan_pub : 將global_planner中發(fā)過來的路徑不經(jīng)過任何處理發(fā)布到”/ftc_local_planner/global_plan”

global_point_pub: 發(fā)布當前控制點, 即ftc代碼判斷的機器人的位置, 由 current_index控制

obstacle_marker_pub: 發(fā)布障礙物的信息, 當靠近障礙物時, rviz中的障礙物會由綠色變?yōu)辄S色, 當障礙物變?yōu)榧t色時, 機器人停止移動

setPlan(const std::vector<geometry_msgs::PoseStamped> &plan)

參數(shù):

plan 包含位姿信息的向量, 從起始點指向結(jié)束點

邏輯:

1. 定義當前狀態(tài) PRE_ROTATE
2. 復(fù)制plan到global_plan
3. 如果規(guī)劃中大于兩個點(路徑規(guī)劃存在), 則將倒數(shù)第二個點的朝向改為倒數(shù)第三個點的朝向 global_plan[global_plan.size() - 2].pose.orientation = global_plan[global_plan.size() - 3].pose.orientation;
4. 創(chuàng)建導(dǎo)航路徑 nav_msgs::Path path;
5. 將收到的路徑plan原封不動放到path.poses中
6. 發(fā)布path到”/ftc_local_planner/global_plan”

distanceLookahead() #未知

double FTCPlannerROS::distanceLookahead(){if (global_plan.size() < 2){return 0;}Eigen::Quaternion<double> current_rot(current_control_point.linear());Eigen::Affine3d last_straight_point = current_control_point;for (uint32_t i = current_index + 1; i < global_plan.size(); i++){tf2::fromMsg(global_plan[i].pose, last_straight_point);// check, if direction is the same. if so, we add the distanceEigen::Quaternion<double> rot2(last_straight_point.linear());if (abs(rot2.angularDistance(current_rot)) > config.speed_fast_threshold_angle * (M_PI / 180.0)){break;}}return (last_straight_point.translation() - current_control_point.translation()).norm();}

void FTCPlannerROS::update_control_point(double dt)

作用:

根據(jù)當前狀態(tài)確定執(zhí)行內(nèi)容

狀態(tài): PRE_ROTATE, FOLLOWING, POST_ROTATE, WAITTING_FOR_GOAL_APPROACH, FINISHED

參數(shù):

dt: 當前時間 - 上次執(zhí)行的時間

邏輯: current_control_point: map坐標系下的點

local_control_point: base_link坐標系下的點

1. 判斷當前執(zhí)行狀態(tài)

   1. PRE_ROTATE
      1. 將global_plan[0].pose存入current_control_point
   2. FOLLOWING
      1. 計算前視距離 straight_dist=distanceLookahead()
      2. 如果前視距離大于滿速行進的閾值, 則全速前進
      3. 若目標速度大于當前速度, 則根據(jù)配置的加速度對當前速度進行提升
      4. 如果: 需要移動的距離大于零, 同時需要轉(zhuǎn)動的角度大于零且當前點不為全局路徑規(guī)劃的倒數(shù)第二個點, 進入循環(huán)
         1. 從全局規(guī)劃中獲取下兩個點, currentPose以及nextPose
         2. 計算兩個點的距離 pose_distance
         3. 如果兩個點之間的距離小于等于0, 則警告: 跳過重復(fù)點(Skipping duplicate point in global plan)
         4. 通過current_progress(0-1之間的數(shù)), 計算剩余距離與角度
         5. 如果剩余的距離和角度可以在當前時間步內(nèi)到達（remaining_distance_to_next_pose < distance_to_move*并且*remaining_angular_distance_to_next_pose < angle_to_move），則將機器人移動到下一個位置，更新**current_progress，減少distance_to_move和angle_to_move**
         6. 如果無法在當前時間步內(nèi)到達下一個位置，則更新**current_progress**以確保機器人在下一個時間步內(nèi)能夠到達下一個位置
      5. 最后，通過插值計算機器人在當前時間步的控制點位置和方向，然后將其存儲在**current_control_point**中，以供后續(xù)控制使用
   3. POST_ROTATE
      1. 將global_plan中的最后一個點保存到current_control_point中
   4. WAITTING_FOR_GOAL_APPROACH
      1. break
   5. FINISHED
      1. break

2. 創(chuàng)建目標點, viz用于發(fā)布current_control_point的坐標

3. 通過tf::doTransform(currnet_control_point, local_control_point, map_to_base)轉(zhuǎn)化current_control_point的坐標到 local_control_point. 注: current_control_point為基于map的全局坐標, 而local_control_point通過下面 兩行, 將map全局坐標系下的點, 轉(zhuǎn)化為從base_link到map的局部變換的點

   auto map_to_base = tf_buffer->lookupTransform("base_link", "map", ros::Time(), ros::Duration(1.0));
   tf2::doTransform(current_control_point, local_control_point, map_to_base);
   

4. 計算誤差 lat_error(橫向誤差), lon_error(縱向誤差), angle_error(角度誤差)

update_planner_state()

邏輯:

1. 根據(jù)當前狀態(tài)更新下一個狀態(tài)
   1. PRE_ROTATE
      1. 判斷setPlan的時間是否超時, 是, ERROR: Timeout in PRE_ROTATE phase; is_crashed=true; return FINISHED;
      2. 判斷angle_error是否小于max_goal_angle_error, 是, INFO: FTCLocalPlannerROS: PRE_ROTATE finished return FOLLOWING
      3. break
   2. FOLLOWING
      1. 計算distance = local_control_point.translation().norm()
      2. 判斷distance是否大于最大追蹤距離max_follow_distance 是, ERROR: FTCLocalPlannerROS: Robot is far away from global plan. distance is_crashed=true return FINISHED
      3. 判斷current_index是否為倒數(shù)第二個點 是, INFO: FTCLocalPlannerROS: switching planner to position mode return WAITING_FOR_GOAL_APPROACH
      4. break
   3. WAITING_FOR_GOAL_APPEOACH
      1. 計算distance = local_control_point.translation().norm()
      2. 判斷是否超時 是, WARN: FTCLocalPlannerROS: Could not reach goal position return POST_ROTATE
      3. 判斷距離是否小于max_goal_distance_error 是, INFO: FTCLocalPlannerROS: Reached goal position. return POST_ROTATE
      4. break
   4. POST_ROTATE
      1. 判斷是否超時 是, WARN: FTCLocalPlannerROS: Could not reach goal rotation return FINISHED
      2. 判斷角度誤差是否小于max_goal_angle_error 是, INFO: FTCLocalPlannerROS: POST_ROTATE finished. return FINISHED
      3. break
   5. FINISHED
      1. break
2. return current_state

bool checkCollision(int max_points)

參數(shù):

max_points 向前預(yù)判的點的個數(shù), 根據(jù)實際global_plan中每一個點之間的距離設(shè)置max_points

例如: 機器人當前點為 global_plan[current_index], 而max_points則負責(zé)檢測從global_plan[current_index]到global_plan[current_index + max_points]這段路徑中間是否有障礙物, 有則為true

邏輯:

1. 判斷是否開啟了障礙檢測
2. 如果global_plan.size() < max_points 則將最大值設(shè)為路徑中點的總數(shù)
3. 判斷是否開啟了obstacle_footprint 使用實際足跡檢測碰撞
   1. costmap->getOrientedFootprint(footprint);獲取當前機器人足跡, 保存到footprint
   2. 遍歷footprint, 將每一個點從world轉(zhuǎn)換到map
   3. 獲取每一個點的代價
   4. 判斷當前代價是否大于既定的致命值costs >= costmap_2d::LETHAL_OBSTACLE
   5. 是, WARN: FTCLocalPlannerROS: Possible collision of footprint at actual pose. Stop local planner. return true
4. for (int i = 0; i < max_points; i++) 判斷前視距離是否有障礙物
   1. 獲取current_index + i 點的坐標
   2. 將該坐標轉(zhuǎn)換到map坐標系
   3. 獲取該坐標的代價值
   4. 判斷 costs >127 && costs > previous_cost 是, WARN(FTCLocalPlannerROS: Possible collision. Stop local planner.) return true
   5. 更新previous_cost為costs
   6. 開始下個循環(huán)
5. return false

void calculate_velocity_commands(double dt, geometry_msgs::Twist &cmd_vel)

參數(shù):

dt 現(xiàn)在時間與上次執(zhí)行時間之差

cmd_vel 速度置零

邏輯:

1. 判斷current_state == FINISHED || is_crashed 速度置零, 返回
2. 計算PID參數(shù)
3. 判斷current_state==FOLLOWING
   1. 通過PID計算線速度
   2. 如果線速度 < 0 且 forward_only 為true 速度歸零
   3. 限制線速度在 -max_cmd_vel_speed 到 max_cmd_vel_speed之間
   4. cmd_vel.linear.x = lin_speed
4. 角速度的運算分兩部分, current_state == FOLLOWING以及current_state == PRE_ROTATE
   1. 判斷current_state==FOLLOWING
      1. 通過PID計算角速度
      2. 限制角速度
      3. cmd_vel.angular.z = ang_speed
   2. 否則(也就是機器人處于PRE_ROTATE階段)
      1. 通過PID計算角速度
      2. 限制角速度
      3. cmd_vel.angular.z = ang_speed
      4. 震蕩檢測, 如果震蕩, 則角速度拉滿

bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel)

參數(shù):

cmd_vel 速度

邏輯:

1. 計算dt 執(zhí)行該代碼時的時間 - 上次執(zhí)行改代碼的時間
2. 判斷是否碰撞(is_crashed) 是, 速度置零, return false
3. 判斷是否完成(current_state==FINISHED), 是, 速度置零, return true
4. 根據(jù)dt, 更新控制點 update_control_point(dt)
5. 更新當前狀態(tài) auto new_planner_state = update_planner_state()
6. 判斷: 新狀態(tài)與當前狀態(tài)是否一致 是, INFO: FTCLocalPlannerROS: Switching to state 更新state_entered_time (用于判斷一個狀態(tài)是否會超時) 更新當前狀態(tài)為新狀態(tài)
7. 判斷是否存在碰撞 checkCollision(obstacle_lookahead) 是, 速度置零 is_crashed = true return false
8. 計算速度 calculate_velocity_commands(dt, cmd_vel)