Autonomous_Exploration_Development_Environment的PathFollower学习笔记

1.PathFollow算法简介：

PathFollow算法是路径跟踪算法，是在得到由localplanner算法发布的无碰撞路径话题”/path”中的路径数据start_path(相对于车体坐标系的一系列路径点(101个点))，根据车体与目标之间的角度和距离，控制车辆的速度和角速度，使车辆精准按照路径到达目标点。

通过输入rqt_graph可看到各话题间的关系,Pathfollow算法接收1.来自localplanner发布的/path路径消息(start_path)2.遥控器的消息/jor 3./state_estimate消息（里程计）

2.相关坐标转换

3.算法流程：

4.注释代码：

 

#include <math.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <ros/ros.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <std_msgs/Int8.h>
#include <std_msgs/Float32.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TwistStamped.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Joy.h>

#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>


//该代码是根据localplanner中选择的路径，进行航路点跟踪
using namespace std;

const double PI = 3.1415926;

double sensorOffsetX = 0;
double sensorOffsetY = 0;
int pubSkipNum = 1;
int pubSkipCount = 0;
bool twoWayDrive = true;
double lookAheadDis = 0.5;
double yawRateGain = 7.5;
double stopYawRateGain = 7.5;
double maxYawRate = 45.0;
double maxSpeed = 1.0;
double maxAccel = 1.0;
double switchTimeThre = 1.0;
double dirDiffThre = 0.1;
double stopDisThre = 0.2;
double slowDwnDisThre = 1.0;
bool useInclRateToSlow = false;
double inclRateThre = 120.0;
double slowRate1 = 0.25;
double slowRate2 = 0.5;
double slowTime1 = 2.0;
double slowTime2 = 2.0;
bool useInclToStop = false;
double inclThre = 45.0;
double stopTime = 5.0;
bool noRotAtStop = false;
bool noRotAtGoal = true;
bool autonomyMode = false;
double autonomySpeed = 1.0;
double joyToSpeedDelay = 2.0;

float joySpeed = 0;
float joySpeedRaw = 0;
float joyYaw = 0;
int safetyStop = 0;

float vehicleX = 0;
float vehicleY = 0;
float vehicleZ = 0;
float vehicleRoll = 0;
float vehiclePitch = 0;
float vehicleYaw = 0;

float vehicleXRec = 0;
float vehicleYRec = 0;
float vehicleZRec = 0;
float vehicleRollRec = 0;
float vehiclePitchRec = 0;
float vehicleYawRec = 0;

float vehicleYawRate = 0;
float vehicleSpeed = 0;

double odomTime = 0;
double joyTime = 0;
double slowInitTime = 0;
double stopInitTime = false;
int pathPointID = 0;
bool pathInit = false;
bool navFwd = true;
double switchTime = 0;

nav_msgs::Path path;

//获取里程计消息回调函数，更新机器人位姿
void odomHandler(const nav_msgs::Odometry::ConstPtr& odomIn)
{
  odomTime = odomIn->header.stamp.toSec();//从里程计消息中获取时间戳，并转换为秒。

  double roll, pitch, yaw;
  geometry_msgs::Quaternion geoQuat = odomIn->pose.pose.orientation;//从里程计消息中提取四元数格式的姿态数据。
  tf::Matrix3x3(tf::Quaternion(geoQuat.x, geoQuat.y, geoQuat.z, geoQuat.w)).getRPY(roll, pitch, yaw);//将四元数转换为欧拉角，分别得到滚转（roll）、俯仰（pitch）和偏航（yaw）角

  vehicleRoll = roll;
  vehiclePitch = pitch;
  vehicleYaw = yaw;
  vehicleX = odomIn->pose.pose.position.x - cos(yaw) * sensorOffsetX + sin(yaw) * sensorOffsetY;//将点云数据从传感器中心变换到车辆中心
  vehicleY = odomIn->pose.pose.position.y - sin(yaw) * sensorOffsetX - cos(yaw) * sensorOffsetY;
  vehicleZ = odomIn->pose.pose.position.z;

  //滚转角 (roll) 或俯仰角 (pitch) 是超过了一个预设阈值 (inclThre=45)，或者收到停车指令useInclToStop进行停车
  if ((fabs(roll) > inclThre * PI / 180.0 || fabs(pitch) > inclThre * PI / 180.0) && useInclToStop) {
    stopInitTime = odomIn->header.stamp.toSec();//设置停车时间为当前时间，用于后续的实际检测到实际停车的延时判断等
  }

  //处理车辆的角速度数据，并决定是否初始化减速过程
  //车辆的角速度（在X轴和Y轴上，弧度每秒）是否超过了预设的角速率阈值 inclRateThre，或者收到减速指令
  if ((fabs(odomIn->twist.twist.angular.x) > inclRateThre * PI / 180.0 || fabs(odomIn->twist.twist.angular.y) > inclRateThre * PI / 180.0) && useInclRateToSlow) {
    slowInitTime = odomIn->header.stamp.toSec();//设置减速时间为当前时间，可用于计算从高角速率检测到实际减速的延迟
  }
}

//pathHandler 函数是一个 ROS 回调函数，用于处理localplanner传入的路径数据
void pathHandler(const nav_msgs::Path::ConstPtr& pathIn)
{
  int pathSize = pathIn->poses.size();//整条路径，路径点数量
  path.poses.resize(pathSize);
  //复制路径数据
  for (int i = 0; i < pathSize; i++) {
    path.poses[i].pose.position.x = pathIn->poses[i].pose.position.x;
    path.poses[i].pose.position.y = pathIn->poses[i].pose.position.y;
    path.poses[i].pose.position.z = pathIn->poses[i].pose.position.z;
  }
  //vehicleXRec记录路径开始时刻的车辆位置
  vehicleXRec = vehicleX;
  vehicleYRec = vehicleY;
  vehicleZRec = vehicleZ;
  vehicleRollRec = vehicleRoll;
  vehiclePitchRec = vehiclePitch;
  vehicleYawRec = vehicleYaw;

  pathPointID = 0;//初始化路径点索引
  pathInit = true;
}

//处理来自操纵杆（遥控手柄）的输入。
void joystickHandler(const sensor_msgs::Joy::ConstPtr& joy)
{
  joyTime = ros::Time::now().toSec();

  joySpeedRaw = sqrt(joy->axes[3] * joy->axes[3] + joy->axes[4] * joy->axes[4]);//使用操纵杆上的轴（axes[3] 和 axes[4]）计算原始速度。这通常是通过操纵杆的倾斜程度来实现的。
  joySpeed = joySpeedRaw;
  if (joySpeed > 1.0) joySpeed = 1.0;
  if (joy->axes[4] == 0) joySpeed = 0;
  joyYaw = joy->axes[3];//计算操纵杆偏航角yaw
  if (joySpeed == 0 && noRotAtStop) joyYaw = 0;

  //理仅正向行驶的情况
  if (joy->axes[4] < 0 && !twoWayDrive) {//!twoWayDrive仅单向情况下，如果输入手柄速度小于0，将其设置为0
    joySpeed = 0;
    joyYaw = 0;
  }

  if (joy->axes[2] > -0.1) {//通过手柄切换手动/导航模式
    autonomyMode = false;
  } else {
    autonomyMode = true;
  }
}

//用于处理来自速度主题的消息。函数的目的是在自主模式下，根据接收到的速度数据来调整车辆的速度。
void speedHandler(const std_msgs::Float32::ConstPtr& speed)
{
  double speedTime = ros::Time::now().toSec();

  //1.导航模式2.检查自上次接收操纵杆输入以来已过了足够的时间3.手柄输入速度为0
  if (autonomyMode && speedTime - joyTime > joyToSpeedDelay && joySpeedRaw == 0) {//手柄干扰的判断
    joySpeed = speed->data / maxSpeed;//speed->data速度消息中获取速度值，并根据最大速度 maxSpeed 来归一化这个速度值

    //速度归一化到0-1
    if (joySpeed < 0) joySpeed = 0;
    else if (joySpeed > 1.0) joySpeed = 1.0;
  }
}

//根据接收到的停止信号来更新车辆的安全停止状态
void stopHandler(const std_msgs::Int8::ConstPtr& stop)
{
  safetyStop = stop->data;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "pathFollower");//初始化节点名称
  ros::NodeHandle nh;
  ros::NodeHandle nhPrivate = ros::NodeHandle("~");

  nhPrivate.getParam("sensorOffsetX", sensorOffsetX);//传感器在车辆上的物理偏移
  nhPrivate.getParam("sensorOffsetY", sensorOffsetY);
  nhPrivate.getParam("pubSkipNum", pubSkipNum);//发布跳过数:？
  nhPrivate.getParam("twoWayDrive", twoWayDrive);//车辆是否可以双向运动
  nhPrivate.getParam("lookAheadDis", lookAheadDis);//前瞻距离？
  nhPrivate.getParam("yawRateGain", yawRateGain);//偏航率增益
  nhPrivate.getParam("stopYawRateGain", stopYawRateGain);
  nhPrivate.getParam("maxYawRate", maxYawRate);//最大偏航率
  nhPrivate.getParam("maxSpeed", maxSpeed);//最大速度
  nhPrivate.getParam("maxAccel", maxAccel);//最大加速度
  nhPrivate.getParam("switchTimeThre", switchTimeThre);
  nhPrivate.getParam("dirDiffThre", dirDiffThre);
  nhPrivate.getParam("stopDisThre", stopDisThre);
  nhPrivate.getParam("slowDwnDisThre", slowDwnDisThre);
  nhPrivate.getParam("useInclRateToSlow", useInclRateToSlow);//是否减速
  nhPrivate.getParam("inclRateThre", inclRateThre);//角速度阈值
  nhPrivate.getParam("slowRate1", slowRate1);
  nhPrivate.getParam("slowRate2", slowRate2);
  nhPrivate.getParam("slowTime1", slowTime1);
  nhPrivate.getParam("slowTime2", slowTime2);
  nhPrivate.getParam("useInclToStop", useInclToStop);
  nhPrivate.getParam("inclThre", inclThre);
  nhPrivate.getParam("stopTime", stopTime);
  nhPrivate.getParam("noRotAtStop", noRotAtStop);
  nhPrivate.getParam("noRotAtGoal", noRotAtGoal);
  nhPrivate.getParam("autonomyMode", autonomyMode);
  nhPrivate.getParam("autonomySpeed", autonomySpeed);
  nhPrivate.getParam("joyToSpeedDelay", joyToSpeedDelay);

  ros::Subscriber subOdom = nh.subscribe<nav_msgs::Odometry> ("/state_estimation", 5, odomHandler);

  ros::Subscriber subPath = nh.subscribe<nav_msgs::Path> ("/path", 5, pathHandler);//获取localplanner发布的path路径消息

  ros::Subscriber subJoystick = nh.subscribe<sensor_msgs::Joy> ("/joy", 5, joystickHandler);

  ros::Subscriber subSpeed = nh.subscribe<std_msgs::Float32> ("/speed", 5, speedHandler);

  ros::Subscriber subStop = nh.subscribe<std_msgs::Int8> ("/stop", 5, stopHandler);

  ros::Publisher pubSpeed = nh.advertise<geometry_msgs::TwistStamped> ("/cmd_vel", 5);//将速度发布到cmd_vel中
  geometry_msgs::TwistStamped cmd_vel;
  cmd_vel.header.frame_id = "vehicle";//cmd_vel的运动话题坐标系是车体坐标系

  if (autonomyMode) {//归一化自动模式下的速度
    joySpeed = autonomySpeed / maxSpeed;

    if (joySpeed < 0) joySpeed = 0;
    else if (joySpeed > 1.0) joySpeed = 1.0;
  }

  ros::Rate rate(100);
  bool status = ros::ok();
  while (status) {
    ros::spinOnce();

    if (pathInit) {//是否经过pathHandler进行路径初始化

    //vehicleXRel车辆当前位置相对于其在接收到路径时位置的偏移量（相对坐标）
    //vehicleX车辆当前的全局X坐标
    //vehicleXRec车辆在接收到路径时的全局坐标
      float vehicleXRel = cos(vehicleYawRec) * (vehicleX - vehicleXRec) //
                        + sin(vehicleYawRec) * (vehicleY - vehicleYRec);
      float vehicleYRel = -sin(vehicleYawRec) * (vehicleX - vehicleXRec) 
                        + cos(vehicleYawRec) * (vehicleY - vehicleYRec);

      //算路径终点（path_start中的坐标都是相对起始导航车体）与车辆当前位置的X轴和Y轴差（path.poses和vehicleXRel两者都在同一个坐标系下--收到导航起始位置为坐标系）
      int pathSize = path.poses.size();
      float endDisX = path.poses[pathSize - 1].pose.position.x - vehicleXRel;
      float endDisY = path.poses[pathSize - 1].pose.position.y - vehicleYRel;
      float endDis = sqrt(endDisX * endDisX + endDisY * endDisY);

      float disX, disY, dis;
      while (pathPointID < pathSize - 1) {//遍历路径点
        disX = path.poses[pathPointID].pose.position.x - vehicleXRel;//车辆当前位置（考虑相对偏移 vehicleXRel 和 vehicleYRel）与路径点的X轴和Y轴差
        disY = path.poses[pathPointID].pose.position.y - vehicleYRel;
        dis = sqrt(disX * disX + disY * disY);
        if (dis < lookAheadDis) {//如果计算出的距离小于预设的“前瞻距离”（lookAheadDis），
          pathPointID++;//则增加 pathPointID +1，跳过下一个路径点（去到>lookAheadDis距离的第一个路径点）
        } else {
          break;
        }
      }
      
      //计算要运动过去的下一个路径点(通过lookAheadDis跳过了一部分)的x,y,角度，以及距离
      disX = path.poses[pathPointID].pose.position.x - vehicleXRel;
      disY = path.poses[pathPointID].pose.position.y - vehicleYRel;
      dis = sqrt(disX * disX + disY * disY);
      float pathDir = atan2(disY, disX);

      //vehicleYaw车辆当前位置相对map的yaw角
      //vehicleYawRec车辆在获取路径起点时相对map的yaw角
      //pathDir:下一个路径点与当前位置的夹角（以车导航起始时刻为坐标系）
      //dirDiff：车体当前位置和目标点之间的夹角(yaw)
      float dirDiff = vehicleYaw - vehicleYawRec - pathDir;
      //dirDiff角度转换到 -π 到 π 的范围内
      if (dirDiff > PI) dirDiff -= 2 * PI;
      else if (dirDiff < -PI) dirDiff += 2 * PI;
      if (dirDiff > PI) dirDiff -= 2 * PI;
      else if (dirDiff < -PI) dirDiff += 2 * PI;

      if (twoWayDrive) {//如果可以双向行使
        double time = ros::Time::now().toSec();
        //如果方向差 (dirDiff) 的绝对值大于90度,且当前是正向行驶(navFwd=true，并且自上次改变方向以来已经超过了一定的时间阈值 (switchTimeThre)，则切换到反向行驶
        if (fabs(dirDiff) > PI / 2 && navFwd && time - switchTime > switchTimeThre) {
          navFwd = false;
          switchTime = time;//记录反向行驶时间
        } else if (fabs(dirDiff) < PI / 2 && !navFwd && time - switchTime > switchTimeThre) {
          navFwd = true;//否则保持原来方向
          switchTime = time;
        }
      }

      float joySpeed2 = maxSpeed * joySpeed;//计算出车辆的目标速度（最大速度*比例）
      if (!navFwd) {//如果反向行使
        dirDiff += PI;//将角度反180度，计算反向行使角度
        if (dirDiff > PI) dirDiff -= 2 * PI;//如果调整后的 dirDiff 超出了 -π 到 π 的标准化范围，再次进行标准化
        joySpeed2 *= -1;//计算出的速度乘以 -1，以反映反向行驶的实际速度。
      }
      
      //自动模式计算车辆的偏航率（vehicleYawRate），为了将车辆yaw角调整到目标点的yaw角
      //车辆的速度是否低于一个特定阈值2.0 * maxAccel / 100.0（认为其停止运动）， stopYawRateGain 是在低速时使用的偏航率增益，dirDiff 是车辆与目标方向差
      if (fabs(vehicleSpeed) < 2.0 * maxAccel / 100.0) vehicleYawRate = -stopYawRateGain * dirDiff;
      else vehicleYawRate = -yawRateGain * dirDiff;//在正常速度下，偏航率被设置为 -yawRateGain * dirDiff。这里 yawRateGain 是正常速度下使用的偏航率增益

      //限制偏航率范围，保证车辆运动安全
      if (vehicleYawRate > maxYawRate * PI / 180.0) vehicleYawRate = maxYawRate * PI / 180.0;
      else if (vehicleYawRate < -maxYawRate * PI / 180.0) vehicleYawRate = -maxYawRate * PI / 180.0;
      
      //手动下且操车辆的目标速度 joySpeed2=0。如果是，表示车辆应该原地旋转。
      if (joySpeed2 == 0 && !autonomyMode) {
        vehicleYawRate = maxYawRate * joyYaw * PI / 180.0;//将偏航率设置为基于操纵杆偏航输入 (joyYaw) 的值
      } else if (pathSize <= 1 || (dis < stopDisThre && noRotAtGoal)) {//检查是否路径点非常少（可能意味着没有路径或路径结束了）或者车辆接近路径的终点（dis < stopDisThre），且在达到目标时不应该旋转
        vehicleYawRate = 0;//偏航率=0，车辆不会进行旋转
      }

      if (pathSize <= 1) {//如果路径中的点数小于或等于1，这通常意味着没有有效路径或路径已经走完
        joySpeed2 = 0;//（车辆的目标速度）设置为0
      } else if (endDis / slowDwnDisThre < joySpeed) {//根据距离终点的距离调整速度，slowDwnDisThre 是一个预设的距离阈值，用于开始减速
        joySpeed2 *= endDis / slowDwnDisThre;//随着车辆接近终点，endDis减小，joySpeed2会逐渐减速。
      }

      float joySpeed3 = joySpeed2;//最终速度
      //考虑突发情况,joySpeed3改变
      //当前时间 (odomTime) 是否在某个初始减速时间点 (slowInitTime) 加上一个时间间隔 (slowTime1) 之内。
      //在某个事件（如倾斜、接近障碍物等）发生后的 slowTime1 时间内，将 joySpeed3（车辆速度）乘以一个减速率 slowRate1，从而减慢车辆
      if (odomTime < slowInitTime + slowTime1 && slowInitTime > 0) joySpeed3 *= slowRate1;
      else if (odomTime < slowInitTime + slowTime1 + slowTime2 && slowInitTime > 0) joySpeed3 *= slowRate2;//离slowInitTime时间较长时，将速度再次降低（*slowRate2）

      //当车辆与目标夹角小于阈值，且与目标距离大于停车距离（车辆正朝着正确的方向前进，并且距离目标点有足够的空间）
      if (fabs(dirDiff) < dirDiffThre && dis > stopDisThre) {
        if (vehicleSpeed < joySpeed3) vehicleSpeed += maxAccel / 100.0;//如果车辆速度小于（joySpeed3），车辆加速
        else if (vehicleSpeed > joySpeed3) vehicleSpeed -= maxAccel / 100.0;//如果车辆当前速度高于目标速度，减少车辆速度
      } else {//如果车辆未朝着正确方向或太接近目标点，将车辆速度慢慢调整为0
        if (vehicleSpeed > 0) vehicleSpeed -= maxAccel / 100.0;
        else if (vehicleSpeed < 0) vehicleSpeed += maxAccel / 100.0;
      }

      //如果当前时间odomTime在（停车开始stopInitTime+完全停止stopTime）时间内，将车辆速度和角速度都设置为0，进行停车
      if (odomTime < stopInitTime + stopTime && stopInitTime > 0) {
        vehicleSpeed = 0;
        vehicleYawRate = 0;
      }

      //接收到不同级别的安全停止信号时，确保车辆能够适当地响应。
      if (safetyStop >= 1) vehicleSpeed = 0;
      if (safetyStop >= 2) vehicleYawRate = 0;

      pubSkipCount--;//减少发布跳过计数 
      if (pubSkipCount < 0) {//如果 pubSkipCount 小于0，表示已经达到了发布新消息的时间
        cmd_vel.header.stamp = ros::Time().fromSec(odomTime);//对时，将 cmd_vel 消息的时间戳设置为当前里程计时间 (odomTime)。
        if (fabs(vehicleSpeed) <= maxAccel / 100.0) cmd_vel.twist.linear.x = 0;//如果车辆的速度非常小（小于或等于最大加速度的百分之一），则将线速度设置为0，表示车辆停止
        else cmd_vel.twist.linear.x = vehicleSpeed;
        cmd_vel.twist.angular.z = vehicleYawRate;//将消息的角速度部分设置为当前的偏航率 (vehicleYawRate)
        pubSpeed.publish(cmd_vel);

        pubSkipCount = pubSkipNum;//重置 pubSkipCount ，以间隔pubSkipNum发布速度消息
      }
    }

    status = ros::ok();
    rate.sleep();
  }

  return 0;
}