fast planner代码解析--kino_replan_fsm.cpp

X uuuer.

已于 2023-03-09 21:09:51 修改

阅读量1.5k

点赞数 7

文章标签：经验分享

于 2022-07-25 12:04:36 首次发布

本文链接：https://blog.csdn.net/weixin_45868890/article/details/125972011

版权

该代码实现了一个无人机路径规划和重规划的执行状态机。状态机包括初始化、等待目标点、生成轨迹、重规划轨迹和执行轨迹等状态。通过订阅目标点和里程计信息，根据执行状态进行相应的操作，如规划、重规划或执行轨迹。同时，还包含了碰撞检测功能，当检测到目标点附近有障碍物时，会尝试寻找新的安全目标并进行重规划。整个过程通过ROS组件（定时器、订阅器、发布器）协调进行。

摘要由CSDN通过智能技术生成

KinoReplanFSM::init()函数

void KinoReplanFSM::waypointCallback()订阅目标点回调函数

void KinoReplanFSM::odometryCallback()订阅里程计回调函数

void KinoReplanFSM::changeFSMExecState() 改变执行状态机FSM函数

void KinoReplanFSM::printFSMExecState()输出执行状态机状态函数

void KinoReplanFSM::execFSMCallback()执行状态机FSM定时器回调函数

void KinoReplanFSM::checkCollisionCallback() 回调碰撞检测函数

路径规划并检查规划是否成功

功能：订阅无人机位置和目标点信息；

利用定时回调函数execFsmCallback判断当前状态；

利用exec_state判断是否等待目标点，是否进行重规划，是否执行轨迹。

利用定时回调函数checkCollisionCallback判断是否需要重新寻找目标点，是否需要进行重规划。

KinoReplanFSM::init()函数

//状态机初始化

//首先是设置了一系列参数

//然后对规划管理类的智能指针对象planner_manager_进行了初始化。

//接着，定义了一系列的ROS组件，包括定时器：执行时间、安全时间，订阅器：航迹点、里程计，发布器：重规划、规划、B样条。规划的具体步骤就在这些组件的调用及回调函数里实现。z

void KinoReplanFSM::init(ros::NodeHandle& nh) {
  //状态机初始化
  //首先是设置了一系列参数
  //然后对规划管理类的智能指针对象planner_manager_进行了初始化。
  //接着，定义了一系列的ROS组件，包括定时器：执行时间、安全时间，订阅器：航迹点、里程计，发布器：重规划、规划、B样条。
  //规划的具体步骤就在这些组件的调用及回调函数里实现。
  current_wp_  = 0;//当前航迹点
  exec_state_  = FSM_EXEC_STATE::INIT;
  have_target_ = false;
  have_odom_   = false;

  /*  fsm param  */
  //设置参数
  nh.param("fsm/flight_type", target_type_, -1);//飞行状态
  nh.param("fsm/thresh_replan", replan_thresh_, -1.0);//重规划阈值
  nh.param("fsm/thresh_no_replan", no_replan_thresh_, -1.0);//无需重规划阈值

  nh.param("fsm/waypoint_num", waypoint_num_, -1);//航迹点数目
  for (int i = 0; i < waypoint_num_; i++) {//遍历所有航迹点
    nh.param("fsm/waypoint" + to_string(i) + "_x", waypoints_[i][0], -1.0);
    nh.param("fsm/waypoint" + to_string(i) + "_y", waypoints_[i][1], -1.0);
    nh.param("fsm/waypoint" + to_string(i) + "_z", waypoints_[i][2], -1.0);
  }

  /* initialize main modules */
  //初始化主要模块
  planner_manager_.reset(new FastPlannerManager);
  //对规划管理类的智能指针对象planner_manager_进行了初始化
  planner_manager_->initPlanModules(nh);
  visualization_.reset(new PlanningVisualization(nh));//规划可视化

  /* callback *///执行时间和安全时间，通过执行状态与碰撞检测的回调
  //定时器：执行时间、安全时间
  exec_timer_   = nh.createTimer(ros::Duration(0.01), &KinoReplanFSM::execFSMCallback, this);
  safety_timer_ = nh.createTimer(ros::Duration(0.05), &KinoReplanFSM::checkCollisionCallback, this);
  //订阅器：航迹点、里程计
  waypoint_sub_ =
      nh.subscribe("/waypoint_generator/waypoints", 1, &KinoReplanFSM::waypointCallback, this);
  odom_sub_ = nh.subscribe("/odom_world", 1, &KinoReplanFSM::odometryCallback, this);
  //发布器：重规划、规划、B样条
  replan_pub_  = nh.advertise<std_msgs::Empty>("/planning/replan", 10);
  new_pub_     = nh.advertise<std_msgs::Empty>("/planning/new", 10);
  bspline_pub_ = nh.advertise<plan_manage::Bspline>("/planning/bspline", 10);

}

void KinoReplanFSM::waypointCallback()订阅目标点回调函数

void KinoReplanFSM::waypointCallback(const nav_msgs::PathConstPtr& msg) {
  //订阅目标点回调函数
  if (msg->poses[0].pose.position.z < -0.1) return;//Z<-0.1即未起飞

  cout << "Triggered!" << endl;
  trigger_ = true;//触发

  if (target_type_ == TARGET_TYPE::MANUAL_TARGET) {//目标类型为手动目标
    end_pt_ << msg->poses[0].pose.position.x, msg->poses[0].pose.position.y, 1.0;
    //end_pt的位置，高度Z为1 
  } else if (target_type_ == TARGET_TYPE::PRESET_TARGET) {//目标类型为预设目标
    end_pt_(0)  = waypoints_[current_wp_][0];
    end_pt_(1)  = waypoints_[current_wp_][1];
    end_pt_(2)  = waypoints_[current_wp_][2];
    current_wp_ = (current_wp_ + 1) % waypoint_num_;
  }

  visualization_->drawGoal(end_pt_, 0.3, Eigen::Vector4d(1, 0, 0, 1.0));//可视化
  end_vel_.setZero();//目标速度
  have_target_ = true;//已有目标标识符

  if (exec_state_ == WAIT_TARGET)//若执行状态为等待触发
    changeFSMExecState(GEN_NEW_TRAJ, "TRIG");//改变(转移)状态机为触发
  else if (exec_state_ == EXEC_TRAJ)//若执行状态为执行轨迹
    changeFSMExecState(REPLAN_TRAJ, "TRIG");//改变(转移)状态机为重规划轨迹
}

void KinoReplanFSM::odometryCallback()订阅里程计回调函数

void KinoReplanFSM::odometryCallback(const nav_msgs::OdometryConstPtr& msg) {
  //订阅里程计回调函数
  odom_pos_(0) = msg->pose.pose.position.x;
  odom_pos_(1) = msg->pose.pose.position.y;//odom位置xyz
  odom_pos_(2) = msg->pose.pose.position.z;

  odom_vel_(0) = msg->twist.twist.linear.x;
  odom_vel_(1) = msg->twist.twist.linear.y;//odom的xyz方向速度
  odom_vel_(2) = msg->twist.twist.linear.z;

  odom_orient_.w() = msg->pose.pose.orientation.w;
  odom_orient_.x() = msg->pose.pose.orientation.x;//odom初始
  odom_orient_.y() = msg->pose.pose.orientation.y;
  odom_orient_.z() = msg->pose.pose.orientation.z;

  have_odom_ = true;
}

void KinoReplanFSM::changeFSMExecState() 改变执行状态机FSM函数

void KinoReplanFSM::changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call) {
  //改变执行状态机FSM函数
  string state_str[5] = { "INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ" };
  //执行状态机的状态分别为：初始化、等待触发、生成轨迹、重规划轨迹、执行轨迹
  int    pre_s        = int(exec_state_);//之前状态
  exec_state_         = new_state;//新状态为执行状态
  cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << endl;
}

void KinoReplanFSM::printFSMExecState()输出执行状态机状态函数

void KinoReplanFSM::printFSMExecState() {
  //输出状态机状态函数
  string state_str[5] = { "INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ" };
   //初始化、等待触发、生成轨迹、重规划轨迹、执行轨迹
  cout << "[FSM]: state: " + state_str[int(exec_state_)] << endl;
}

void KinoReplanFSM::execFSMCallback()执行状态机FSM定时器回调函数

//如果状态是INIT（初始化），则判断有没有收到odometry 和 plan的 ttrigger是否被触发。have_odom 和 trigger 这两个bool变量分别在KinoReplanFSM::odometryCallback()和KinoReplanFSM::waypointCallback() 这两个消息订阅的回调函数中被改变。如果都通过，改变当前状态为WAIT_TARGET,并跳出当前循环。

//如果状态是WAIT_TARGET（等待目标点）, 则判断是否有target,如果有，则改变状态为GEN_NEW_TRAJ，并跳出当前循环。

//如果状态是GEN_NEW_TRAJ(生成轨迹)，则调用callKinodynamicReplan()函数进行规划。成功则改变执行状态为EXEC_TRAJ,失败则改变执行状态为GEN_NEW_TRAJ。callKinodynamicReplan()函数,如果规划成功的话，就通过Bspline_pub把相应的B样条轨迹发布出去，否则返回FALSE。

//如果状态为EXEC_TRAJ(执行轨迹)，则判断是否需要进行重规划。如果当前时间距离起始时间已经超过当前执行轨迹的时长，则将have_target置于false，将执行状态变为WAIT_TARGET。如果当前距离与终点距离小于不需要重规划的阈值，或者当前距离与起点距离小于horizon，那么直接返回。反之，则进入重规划阶段。

//如果状态为REPLAN_TRAJ(重新规划)，则利用当前的位置，速度，状态，进行callKinodynamicReplan重规划。如果成功，则将执行状态更改为EXEC_TRAJ,否则将执行状态更改为GEN_NEW_TRAJ。

void KinoReplanFSM::execFSMCallback(const ros::TimerEvent& e) {
  //执行状态机回调函数
  static int fsm_num = 0;//状态机的数目
  fsm_num++;
  if (fsm_num == 100) {//状态机数目为100
    printFSMExecState();//输出执行状态机的状态
    if (!have_odom_) cout << "no odom." << endl;//无里程计
    if (!trigger_) cout << "wait for goal." << endl;//等待目标
    fsm_num = 0;
  }

  switch (exec_state_) {
    //如果状态是INIT（初始化），则判断有没有收到odometry 和 plan的 ttrigger是否被触发。
    // have_odom 和 trigger 这两个bool变量分别在KinoReplanFSM::odometryCallback()和KinoReplanFSM::waypointCallback() 这两个消息订阅的回调函数中被改变。
    //如果都通过，改变当前状态为WAIT_TARGET,并跳出当前循环。
    case INIT: {
      if (!have_odom_) {
        return;
      }
      if (!trigger_) {
        return;
      }
      changeFSMExecState(WAIT_TARGET, "FSM");
      break;
    }

    case WAIT_TARGET: {
    //如果状态是WAIT_TARGET（等待目标点）, 则判断是否有target
    //如果有，则改变状态为GEN_NEW_TRAJ，并跳出当前循环。
      if (!have_target_)
        return;
      else {
        changeFSMExecState(GEN_NEW_TRAJ, "FSM");
      }
      break;
    }

    case GEN_NEW_TRAJ: {//生成轨迹
    //如果状态是GEN_NEW_TRAJ(生成轨迹)，则调用callKinodynamicReplan()函数进行规划。
    //成功则改变执行状态为EXEC_TRAJ,失败则改变执行状态为GEN_NEW_TRAJ。
    //callKinodynamicReplan()函数,如果规划成功的话，就通过Bspline_pub把相应的B样条轨迹发布出去，否则返回FALSE
      start_pt_  = odom_pos_;//开始位置
      start_vel_ = odom_vel_;//开始速度
      start_acc_.setZero();//开始加速度

      Eigen::Vector3d rot_x = odom_orient_.toRotationMatrix().block(0, 0, 3, 1);
      start_yaw_(0)         = atan2(rot_x(1), rot_x(0));//偏航
      start_yaw_(1) = start_yaw_(2) = 0.0;

      bool success = callKinodynamicReplan();//回调动态可行重规划进行规划
      if (success) {
        changeFSMExecState(EXEC_TRAJ, "FSM");//成功则改变执行状态状态机的状态为执行轨迹
      } else {
        // have_target_ = false;
        // changeFSMExecState(WAIT_TARGET, "FSM");
        changeFSMExecState(GEN_NEW_TRAJ, "FSM");//失败则改变执行状态为GEN_NEW_TRAJ
      }
      break;
    }

    case EXEC_TRAJ: {//执行轨迹
      /* determine if need to replan *///决定是否需要重规划
      //如果状态为EXEC_TRAJ(执行轨迹)，则判断是否需要进行重规划。
      //如果当前时间距离起始时间已经超过当前执行轨迹的时长，则将have_target置于false，将执行状态变为WAIT_TARGET。
      //如果当前距离与终点距离小于不需要重规划的阈值，或者当前距离与起点距离小于horizon，那么直接返回。
      //反之，则进入重规划阶段。
      LocalTrajData* info     = &planner_manager_->local_data_;
      ros::Time      time_now = ros::Time::now();//当前时间
      double         t_cur    = (time_now - info->start_time_).toSec();//当前时间距离起始时间
      t_cur                   = min(info->duration_, t_cur);

      Eigen::Vector3d pos = info->position_traj_.evaluateDeBoorT(t_cur);

      /* && (end_pt_ - pos).norm() < 0.5 */
      if (t_cur > info->duration_ - 1e-2) {//当前时间距离起始时间超过当前执行轨迹的时长
        have_target_ = false;
        changeFSMExecState(WAIT_TARGET, "FSM");//将执行状态变为WAIT_TARGET。
        return;

      } else if ((end_pt_ - pos).norm() < no_replan_thresh_) {//如果当前距离与终点距离小于不需要重规划的阈值
        // cout << "near end" << endl;
        return;

      } else if ((info->start_pos_ - pos).norm() < replan_thresh_) {//当前距离与起点距离小于规划阈值horizon
        // cout << "near start" << endl;
        return;//那么直接返回

      } else {
        changeFSMExecState(REPLAN_TRAJ, "FSM");//除上述情况外，进入重规划阶段。
      }
      break;
    }

    case REPLAN_TRAJ: {//重规划轨迹
    //如果状态为REPLAN_TRAJ(重新规划)，则利用当前的位置，速度，状态，进行callKinodynamicReplan重规划
    //如果成功，则将执行状态更改为EXEC_TRAJ,否则将执行状态更改为GEN_NEW_TRAJ.
      LocalTrajData* info     = &planner_manager_->local_data_;
      ros::Time      time_now = ros::Time::now();
      double         t_cur    = (time_now - info->start_time_).toSec();

      start_pt_  = info->position_traj_.evaluateDeBoorT(t_cur);
      start_vel_ = info->velocity_traj_.evaluateDeBoorT(t_cur);
      start_acc_ = info->acceleration_traj_.evaluateDeBoorT(t_cur);
      //当前的位置、速度、加速度
      //偏航
      start_yaw_(0) = info->yaw_traj_.evaluateDeBoorT(t_cur)[0];
      start_yaw_(1) = info->yawdot_traj_.evaluateDeBoorT(t_cur)[0];
      start_yaw_(2) = info->yawdotdot_traj_.evaluateDeBoorT(t_cur)[0];

      std_msgs::Empty replan_msg;
      replan_pub_.publish(replan_msg);

      bool success = callKinodynamicReplan();//回调callKinodynamicReplan()重规划
      if (success) {
        changeFSMExecState(EXEC_TRAJ, "FSM");//重规划成功，则将执行状态更改为EXEC_TRAJ，执行轨迹.
      } else {
        changeFSMExecState(GEN_NEW_TRAJ, "FSM");//重规划失败，则将执行状态更改为GEN_NEW_TRAJ生成轨迹.
      }
      break;
    }
  }
}

这个函数的触发时间是每0.01秒。首先，每1秒打印一次当前执行状态。然后，根据执行状态变量 exec_state_进入switch循环。

void KinoReplanFSM::checkCollisionCallback() 回调碰撞检测函数

//这一执行器回调函数是用来判断目标点是否有障碍物或者是轨迹执行过程中是否有障碍物的。如果目标点有障碍物，就在目标点周围通过离散的半径及角度循环来寻找新的安全的目标点。如果找到了，就直接改变状态进入REPLAN_TRAJ。如果目标点周围没有障碍物且目前状态是EXEX_TRAJ,则利用planner_manager的checkTrajCollision函数进行轨迹检查，如果轨迹不发生碰撞，则无事发生，如果轨迹碰撞，则状态变为REPLAN_TRAJ,进行重规划。

void KinoReplanFSM::checkCollisionCallback(const ros::TimerEvent& e) {
  //回调碰撞检测函数
  //这一执行器回调函数是用来判断目标点是否有障碍物或者是轨迹执行过程中是否有障碍物的。
  //如果目标点有障碍物，就在目标点周围通过离散的半径及角度循环来寻找新的安全的目标点。
  //如果找到了，就直接改变状态进入REPLAN_TRAJ。
  //如果目标点周围没有障碍物且目前状态是EXEX_TRAJ,则利用planner_manager的checkTrajCollision函数进行轨迹检查，
  //如果轨迹不发生碰撞，则无事发生，如果轨迹碰撞，则状态变为REPLAN_TRAJ,进行重规划

  LocalTrajData* info = &planner_manager_->local_data_;//局部数据信息

  if (have_target_) {
    auto edt_env = planner_manager_->edt_environment_;//环境

    double dist = planner_manager_->pp_.dynamic_ ?
        edt_env->evaluateCoarseEDT(end_pt_, /* time to program start + */ info->duration_) :
        edt_env->evaluateCoarseEDT(end_pt_, -1.0);//与障碍物的距离

    if (dist <= 0.3) {
      /* try to find a max distance goal around *///试着在周围找到一个最大距离目标
      bool            new_goal = false;
      const double    dr = 0.5, dtheta = 30, dz = 0.3;
      double          new_x, new_y, new_z, max_dist = -1.0;
      Eigen::Vector3d goal;

      for (double r = dr; r <= 5 * dr + 1e-3; r += dr) {
        for (double theta = -90; theta <= 270; theta += dtheta) {
          for (double nz = 1 * dz; nz >= -1 * dz; nz -= dz) {
            //在目标点周围通过离散的半径及角度循环来寻找新的安全的目标点

            new_x = end_pt_(0) + r * cos(theta / 57.3);
            new_y = end_pt_(1) + r * sin(theta / 57.3);
            new_z = end_pt_(2) + nz;

            Eigen::Vector3d new_pt(new_x, new_y, new_z);
            dist = planner_manager_->pp_.dynamic_ ?
                edt_env->evaluateCoarseEDT(new_pt, /* time to program start+ */ info->duration_) :
                edt_env->evaluateCoarseEDT(new_pt, -1.0);

            if (dist > max_dist) {//距离超过安全阈值，则为新的安全点
              /* reset end_pt_ */
              goal(0)  = new_x;
              goal(1)  = new_y;
              goal(2)  = new_z;
              max_dist = dist;
            }
          }
        }
      }

      if (max_dist > 0.3) {//即找到了新的安全的目标点
        cout << "change goal, replan." << endl;
        end_pt_      = goal;
        have_target_ = true;
        end_vel_.setZero();

        if (exec_state_ == EXEC_TRAJ) {
          changeFSMExecState(REPLAN_TRAJ, "SAFETY");//直接改变状态进入REPLAN_TRAJ
        }

        visualization_->drawGoal(end_pt_, 0.3, Eigen::Vector4d(1, 0, 0, 1.0));
      } else {
        // have_target_ = false;
        // cout << "Goal near collision, stop." << endl;
        // changeFSMExecState(WAIT_TARGET, "SAFETY");
        cout << "goal near collision, keep retry" << endl;//目标接近碰撞，继续重试
        changeFSMExecState(REPLAN_TRAJ, "FSM");//继续寻找重规划轨迹

        std_msgs::Empty emt;
        replan_pub_.publish(emt);
      }
    }
  }

  /* ---------- check trajectory ---------- *///检查轨迹
  if (exec_state_ == FSM_EXEC_STATE::EXEC_TRAJ) {//执行状态机状态为执行轨迹
    double dist;
    bool   safe = planner_manager_->checkTrajCollision(dist);//轨迹无碰撞则继续执行
    //利用planner_manager的checkTrajCollision函数进行轨迹检查

    if (!safe) {//如果轨迹碰撞，则状态变为REPLAN_TRAJ,进行重规划
      // cout << "current traj in collision." << endl;
      ROS_WARN("current traj in collision.");
      changeFSMExecState(REPLAN_TRAJ, "SAFETY");
    }
  }
}

路径规划并检查规划是否成功

bool KinoReplanFSM::callKinodynamicReplan() {
  //路径规划并检查规划是否成功
  bool plan_success =
      planner_manager_->kinodynamicReplan(start_pt_, start_vel_, start_acc_, end_pt_, end_vel_);
      //判断是否规划成功

  if (plan_success) {//规划成功

    planner_manager_->planYaw(start_yaw_);

    auto info = &planner_manager_->local_data_;

    /* publish traj *///发布轨迹
    plan_manage::Bspline bspline;//B样条
    bspline.order      = 3;//B样条阶数
    bspline.start_time = info->start_time_;//开始时间
    bspline.traj_id    = info->traj_id_;//轨迹编号

    Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();//位置(航迹点)为B样条的控制点

    for (int i = 0; i < pos_pts.rows(); ++i) {
      geometry_msgs::Point pt;//位置
      pt.x = pos_pts(i, 0);
      pt.y = pos_pts(i, 1);
      pt.z = pos_pts(i, 2);
      bspline.pos_pts.push_back(pt);
    }

    Eigen::VectorXd knots = info->position_traj_.getKnot();//获取节点
    for (int i = 0; i < knots.rows(); ++i) {
      bspline.knots.push_back(knots(i));//B样条的节点
    }

    Eigen::MatrixXd yaw_pts = info->yaw_traj_.getControlPoint();
    for (int i = 0; i < yaw_pts.rows(); ++i) {
      double yaw = yaw_pts(i, 0);
      bspline.yaw_pts.push_back(yaw);//偏航角
    }
    bspline.yaw_dt = info->yaw_traj_.getInterval();//获取时间截，即两个控制点之间的时间

    bspline_pub_.publish(bspline);//B样条发布

    /* visulization *///可视化
    auto plan_data = &planner_manager_->plan_data_;//规划的数据
    visualization_->drawGeometricPath(plan_data->kino_path_, 0.075, Eigen::Vector4d(1, 1, 0, 0.4));//路径
    visualization_->drawBspline(info->position_traj_, 0.1, Eigen::Vector4d(1.0, 0, 0.0, 1), true, 0.2,
                                Eigen::Vector4d(1, 0, 0, 1));//B样条

    return true;//即规划成功返回true

  } else {
    cout << "generate new traj fail." << endl;//规划失败则返回generate new traj fail
    return false;
  }
}