autoware astar_search hybrid A*

AstarSearch::AstarSearch()
{
  ros::NodeHandle private_nh_("~");

  // base configs
  private_nh_.param<bool>("use_back", use_back_, true); //车辆是否可以倒车
  private_nh_.param<bool>("use_potential_heuristic", use_potential_heuristic_, true);//障碍物概率值比较大的，给一个大的启发值，即避开疑似障碍物的点
  private_nh_.param<bool>("use_wavefront_heuristic", use_wavefront_heuristic_, false);//是否使用bfs的方法检查起点到终点是否可通行
  private_nh_.param<double>("time_limit", time_limit_, 5000.0); //搜索时限

  // robot configs
  private_nh_.param<double>("robot_length", robot_length_, 4.5);//车身长度
  private_nh_.param<double>("robot_width", robot_width_, 1.75);//车身宽度
  private_nh_.param<double>("robot_base2back", robot_base2back_, 1.0);//底盘中心即后轴的位置
  private_nh_.param<double>("minimum_turning_radius", minimum_turning_radius_, 6.0);//最小转弯半径

  // search configs
  private_nh_.param<int>("theta_size", theta_size_, 48);//将角度均分为多少份
  private_nh_.param<double>("angle_goal_range", angle_goal_range_, 6.0);//到达终点的角度阈值
  private_nh_.param<double>("curve_weight", curve_weight_, 1.2);//启发函数的曲率权重
  private_nh_.param<double>("reverse_weight", reverse_weight_, 2.00);//启发函数的倒车权重
  private_nh_.param<double>("lateral_goal_range", lateral_goal_range_, 0.5);//a距离终点的横向距离阈值
  private_nh_.param<double>("longitudinal_goal_range", longitudinal_goal_range_, 2.0);//距离终点的纵向距离阈值

  // costmap configs
  private_nh_.param<int>("obstacle_threshold", obstacle_threshold_, 100);//costmap判断是否是障碍物的阈值
  private_nh_.param<double>("potential_weight", potential_weight_, 10.0);//启发函数潜在障碍物权重
  private_nh_.param<double>("distance_heuristic_weight", distance_heuristic_weight_, 1.0);//启发函数距离权重

  createStateUpdateTable();
}

AstarSearch::~AstarSearch()
{
}

// state update table for hybrid astar以运动的最小距离做搜索，每步搜索六个方向，向前、前左、前右、向后、后左、后右，确定车在不同角度下，在不同的搜索方向搜索移动的距离和角度
void AstarSearch::createStateUpdateTable()
{
  // Vehicle moving for each angle
  state_update_table_.resize(theta_size_);
  double dtheta = 2.0 * M_PI / theta_size_;

  // Minimum moving distance with one state update
  //     arc  = r                       * theta
  double step = minimum_turning_radius_ * dtheta;

  for (int i = 0; i < theta_size_; i++)
  {
    double theta = dtheta * i;

    // Calculate right and left circle
    // Robot moves along these circles
    double right_circle_center_x = minimum_turning_radius_ * std::sin(theta);
    double right_circle_center_y = minimum_turning_radius_ * -std::cos(theta);
    double left_circle_center_x = -right_circle_center_x;
    double left_circle_center_y = -right_circle_center_y;

    // Calculate x and y shift to next state
    NodeUpdate nu;

    // forward
    nu.shift_x = step * std::cos(theta);
    nu.shift_y = step * std::sin(theta);
    nu.rotation = 0.0;
    nu.index_theta = 0;
    nu.step = step;
    nu.curve = false;
    nu.back = false;
    state_update_table_[i].emplace_back(nu);

    // forward right
    nu.shift_x = right_circle_center_x + minimum_turning_radius_ * std::cos(M_PI_2 + theta - dtheta);
    nu.shift_y = right_circle_center_y + minimum_turning_radius_ * std::sin(M_PI_2 + theta - dtheta);
    nu.rotation = -dtheta;
    nu.index_theta = -1;
    nu.step = step;
    nu.curve = true;
    nu.back = false;
    state_update_table_[i].emplace_back(nu);

    // forward left
    nu.shift_x = left_circle_center_x + minimum_turning_radius_ * std::cos(-M_PI_2 + theta + dtheta);
    nu.shift_y = left_circle_center_y + minimum_turning_radius_ * std::sin(-M_PI_2 + theta + dtheta);
    nu.rotation = dtheta;
    nu.index_theta = 1;
    nu.step = step;
    nu.curve = true;
    nu.back = false;
    state_update_table_[i].emplace_back(nu);

    if (use_back_)
    {
      // backward
      nu.shift_x = step * std::cos(theta) * -1.0;
      nu.shift_y = step * std::sin(theta) * -1.0;
      nu.rotation = 0;
      nu.index_theta = 0;
      nu.step = step;
      nu.curve = false;
      nu.back = true;
      state_update_table_[i].emplace_back(nu);

      // backward right
      nu.shift_x = right_circle_center_x + minimum_turning_radius_ * std::cos(M_PI_2 + theta + dtheta);
      nu.shift_y = right_circle_center_y + minimum_turning_radius_ * std::sin(M_PI_2 + theta + dtheta);
      nu.rotation = dtheta;
      nu.index_theta = 1;
      nu.step = step;
      nu.curve = true;
      nu.back = true;
      state_update_table_[i].emplace_back(nu);

      // backward left
      nu.shift_x = left_circle_center_x + minimum_turning_radius_ * std::cos(-1.0 * M_PI_2 + theta - dtheta);
      nu.shift_y = left_circle_center_y + minimum_turning_radius_ * std::sin(-1.0 * M_PI_2 + theta - dtheta);
      nu.rotation = dtheta * -1.0;
      nu.index_theta = -1;
      nu.step = step;
      nu.curve = true;
      nu.back = true;
      state_update_table_[i].emplace_back(nu);
    }
  }
}

//根据地图信息初始化node
void AstarSearch::initialize(const nav_msgs::OccupancyGrid& costmap)
{
  costmap_ = costmap;

  int height = costmap_.info.height;
  int width = costmap_.info.width;

  // size initialization
  nodes_.resize(height);
  for (int i = 0; i < height; i++)
  {
    nodes_[i].resize(width);
  }
  for (int i = 0; i < height; i++)
  {
    for (int j = 0; j < width; j++)
    {
      nodes_[i][j].resize(theta_size_);
    }
  }

  // cost initialization
  for (int i = 0; i < height; i++)
  {
    for (int j = 0; j < width; j++)
    {
      // Index of subscribing OccupancyGrid message
      int og_index = i * width + j;
      int cost = costmap_.data[og_index];

      // hc is set to be 0 when reset()
      if (cost == 0)
      {
        continue;
      }

      // obstacle or unknown area障碍物和未知区域直接认为障碍物
      if (cost < 0 || obstacle_threshold_ <= cost)
      {
        nodes_[i][j][0].status = STATUS::OBS;
      }

      // the cost more than threshold is regarded almost same as an obstacle
      // because of its very high cost疑似障碍物即占用概率较大的，直接赋值给一个大的启发值
      if (use_potential_heuristic_)
      {
        nodes_[i][j][0].hc = cost * potential_weight_;
      }
    }
  }
}

//设置起点和终点后进行搜索
bool AstarSearch::makePlan(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& goal_pose)
{
  if (!setStartNode(start_pose))
  {
    ROS_DEBUG("Invalid start pose");
    return false;
  }

  if (!setGoalNode(goal_pose))
  {
    ROS_DEBUG("Invalid goal pose");
    return false;
  }

  return search();
}

//设置起点，将实际坐标转到地图坐标，同时做越界检查和碰撞检查，赋值实际距离和启发距离
bool AstarSearch::setStartNode(const geometry_msgs::Pose& start_pose)
{
  // Get index of start pose
  int index_x, index_y, index_theta;
  start_pose_local_.pose = start_pose;
  poseToIndex(start_pose_local_.pose, &index_x, &index_y, &index_theta);
  SimpleNode start_sn(index_x, index_y, index_theta, 0, 0);

  // Check if start is valid
  if (isOutOfRange(index_x, index_y) || detectCollision(start_sn))
  {
    return false;
  }

  // Set start node
  AstarNode& start_node = nodes_[index_y][index_x][index_theta];
  start_node.x = start_pose_local_.pose.position.x;
  start_node.y = start_pose_local_.pose.position.y;
  start_node.theta = 2.0 * M_PI / theta_size_ * index_theta;
  start_node.gc = 0;
  start_node.move_distance = 0;
  start_node.back = false;
  start_node.status = STATUS::OPEN;
  start_node.parent = NULL;

  // set euclidean distance heuristic cost
  if (!use_wavefront_heuristic_ && !use_potential_heuristic_)
  {
    start_node.hc = calcDistance(start_pose_local_.pose.position.x, start_pose_local_.pose.position.y,
                                 goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y) *
                    distance_heuristic_weight_;
  }
  else if (use_potential_heuristic_)
  {
    start_node.gc += start_node.hc;
    start_node.hc += calcDistance(start_pose_local_.pose.position.x, start_pose_local_.pose.position.y,
                                  goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y) +
                     distance_heuristic_weight_;
  }

  // Push start node to openlist
  start_sn.cost = start_node.gc + start_node.hc;
  openlist_.push(start_sn);

  return true;
}

//设置终点，将实际坐标转到地图坐标，同时做越界检查和碰撞检查，赋值实际距离和启发距离
bool AstarSearch::setGoalNode(const geometry_msgs::Pose& goal_pose)
{
  goal_pose_local_.pose = goal_pose;
  goal_yaw_ = modifyTheta(tf::getYaw(goal_pose_local_.pose.orientation));

  // Get index of goal pose
  int index_x, index_y, index_theta;
  poseToIndex(goal_pose_local_.pose, &index_x, &index_y, &index_theta);
  SimpleNode goal_sn(index_x, index_y, index_theta, 0, 0);

  // Check if goal is valid
  if (isOutOfRange(index_x, index_y) || detectCollision(goal_sn))
  {
    return false;
  }

  // Calculate wavefront heuristic cost
  if (use_wavefront_heuristic_)
  {
    // auto start = std::chrono::system_clock::now();
    bool wavefront_result = calcWaveFrontHeuristic(goal_sn);
    // auto end = std::chrono::system_clock::now();
    // auto usec = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
    // std::cout << "wavefront : " << usec / 1000.0 << "[msec]" << std::endl;

    if (!wavefront_result)
    {
      ROS_DEBUG("Reachable is false...");
      return false;
    }
  }

  return true;
}

// 实际坐标转地图坐标
void AstarSearch::poseToIndex(const geometry_msgs::Pose& pose, int* index_x, int* index_y, int* index_theta)
{
  tf::Transform orig_tf;
  tf::poseMsgToTF(costmap_.info.origin, orig_tf);
  geometry_msgs::Pose pose2d = transformPose(pose, orig_tf.inverse());

  *index_x = pose2d.position.x / costmap_.info.resolution;
  *index_y = pose2d.position.y / costmap_.info.resolution;

  tf::Quaternion quat;
  tf::quaternionMsgToTF(pose2d.orientation, quat);
  double yaw = tf::getYaw(quat);
  if (yaw < 0)
    yaw += 2.0 * M_PI;

  // Descretize angle
  static double one_angle_range = 2.0 * M_PI / theta_size_;
  *index_theta = yaw / one_angle_range;
  *index_theta %= theta_size_;
}

//实际坐标转地图坐标
void AstarSearch::pointToIndex(const geometry_msgs::Point& point, int* index_x, int* index_y)
{
  geometry_msgs::Pose pose;
  pose.position = point;
  int index_theta;
  poseToIndex(pose, index_x, index_y, &index_theta);
}

//越界检查：超出当前地图边界
bool AstarSearch::isOutOfRange(int index_x, int index_y)
{
  if (index_x < 0 || index_x >= static_cast<int>(costmap_.info.width) || index_y < 0 ||
      index_y >= static_cast<int>(costmap_.info.height))
  {
    return true;
  }
  return false;
}

//路径搜索
bool AstarSearch::search()
{
  ros::WallTime begin = ros::WallTime::now();

  // Start A* search
  // If the openlist is empty, search failed
  while (!openlist_.empty())
  {
    // Check time and terminate if the search reaches the time limit
    ros::WallTime now = ros::WallTime::now();
    double msec = (now - begin).toSec() * 1000.0;
    if (msec > time_limit_)
    {
      ROS_DEBUG("Exceed time limit of %lf [ms]", time_limit_);
      return false;
    }

    // Pop minimum cost node from openlist
    SimpleNode top_sn = openlist_.top();
    openlist_.pop();

    // Expand nodes from this node
    AstarNode* current_an = &nodes_[top_sn.index_y][top_sn.index_x][top_sn.index_theta];
    current_an->status = STATUS::CLOSED;

    // Goal check
    if (isGoal(current_an->x, current_an->y, current_an->theta))
    {
      ROS_DEBUG("Search time: %lf [msec]", (now - begin).toSec() * 1000.0);
      setPath(top_sn);
      return true;
    }

    // Expand nodes搜索6个方向
    for (const auto& state : state_update_table_[top_sn.index_theta])
    {
      // Next state
      double next_x = current_an->x + state.shift_x;
      double next_y = current_an->y + state.shift_y;
      double next_theta = modifyTheta(current_an->theta + state.rotation);
      double move_cost = state.step;
      double move_distance = current_an->move_distance + state.step;

      // Increase reverse cost 一个倒车，一个不倒车，不期望频繁改变运动方向
      if (state.back != current_an->back)
        move_cost *= reverse_weight_;

      // Calculate index of the next state
      SimpleNode next_sn;
      geometry_msgs::Point next_pos;
      next_pos.x = next_x;
      next_pos.y = next_y;
      pointToIndex(next_pos, &next_sn.index_x, &next_sn.index_y);
      next_sn.index_theta = top_sn.index_theta + state.index_theta;

      // Avoid invalid index
      next_sn.index_theta = (next_sn.index_theta + theta_size_) % theta_size_;

      // Check if the index is valid
      if (isOutOfRange(next_sn.index_x, next_sn.index_y) || detectCollision(next_sn))
      {
        continue;
      }

      AstarNode* next_an = &nodes_[next_sn.index_y][next_sn.index_x][next_sn.index_theta];
      double next_gc = current_an->gc + move_cost;
      double next_hc = nodes_[next_sn.index_y][next_sn.index_x][0].hc;  // wavefront or distance transform heuristic

      // increase the cost with euclidean distance
      if (use_potential_heuristic_)
      {
        next_gc += nodes_[next_sn.index_y][next_sn.index_x][0].hc;
        next_hc += calcDistance(next_x, next_y, goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y) *
                   distance_heuristic_weight_;
      }

      // increase the cost with euclidean distance
      if (!use_wavefront_heuristic_ && !use_potential_heuristic_)
      {
        next_hc = calcDistance(next_x, next_y, goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y) *
                  distance_heuristic_weight_;
      }

      // NONE
      if (next_an->status == STATUS::NONE)
      {
        next_an->status = STATUS::OPEN;
        next_an->x = next_x;
        next_an->y = next_y;
        next_an->theta = next_theta;
        next_an->gc = next_gc;
        next_an->hc = next_hc;
        next_an->move_distance = move_distance;
        next_an->back = state.back;
        next_an->parent = current_an;
        next_sn.cost = next_an->gc + next_an->hc;
        openlist_.push(next_sn);
        continue;
      }

      // OPEN or CLOSED
      if (next_an->status == STATUS::OPEN || next_an->status == STATUS::CLOSED)
      {
        if (next_gc < next_an->gc)
        {
          next_an->status = STATUS::OPEN;
          next_an->x = next_x;
          next_an->y = next_y;
          next_an->theta = next_theta;
          next_an->gc = next_gc;
          next_an->hc = next_hc;  // already calculated ?
          next_an->move_distance = move_distance;
          next_an->back = state.back;
          next_an->parent = current_an;
          next_sn.cost = next_an->gc + next_an->hc;
          openlist_.push(next_sn);
          continue;
        }
      }
    }  // state update
  }

  // Failed to find path
  ROS_DEBUG("Open list is empty...");
  return false;
}

//获取路径，从终点回溯路径，再反转
void AstarSearch::setPath(const SimpleNode& goal)
{
  std_msgs::Header header;
  header.stamp = ros::Time::now();
  header.frame_id = costmap_.header.frame_id;
  path_.header = header;

  // From the goal node to the start node
  AstarNode* node = &nodes_[goal.index_y][goal.index_x][goal.index_theta];

  while (node != NULL)
  {
    // Set tf pose
    tf::Vector3 origin(node->x, node->y, 0);
    tf::Pose tf_pose;
    tf_pose.setOrigin(origin);
    tf_pose.setRotation(tf::createQuaternionFromYaw(node->theta));

    // Set path as ros message
    geometry_msgs::PoseStamped ros_pose;
    tf::poseTFToMsg(tf_pose, ros_pose.pose);
    ros_pose.header = header;
    path_.poses.push_back(ros_pose);

    // To the next node
    node = node->parent;
  }

  // Reverse the vector to be start to goal order
  std::reverse(path_.poses.begin(), path_.poses.end());
}

// Check if the next state is the goal
// Check lateral offset, longitudinal offset and angle
//检查是否到达了目标点，有一定阈值，其实是在终点附近
bool AstarSearch::isGoal(double x, double y, double theta)
{
  // To reduce computation time, we use square value for distance
  static const double lateral_goal_range =
      lateral_goal_range_ / 2.0;  // [meter], divide by 2 means we check left and right
  static const double longitudinal_goal_range =
      longitudinal_goal_range_ / 2.0;                                         // [meter], check only behind of the goal
  static const double goal_angle = M_PI * (angle_goal_range_ / 2.0) / 180.0;  // degrees -> radian

  // Calculate the node coordinate seen from the goal point
  tf::Point p(x, y, 0);
  geometry_msgs::Point relative_node_point = calcRelativeCoordinate(goal_pose_local_.pose, p);

  // Check Pose of goal
  if (relative_node_point.x < 0 &&  // shoud be behind of goal
      std::fabs(relative_node_point.x) < longitudinal_goal_range &&
      std::fabs(relative_node_point.y) < lateral_goal_range)
  {
    // Check the orientation of goal
    if (calcDiffOfRadian(goal_yaw_, theta) < goal_angle)
    {
      return true;
    }
  }

  return false;
}

//检查是否是障碍物
bool AstarSearch::isObs(int index_x, int index_y)
{
  if (nodes_[index_y][index_x][0].status == STATUS::OBS)
  {
    return true;
  }

  return false;
}

//障碍物检查 ：以机器人后轴为中心，同时视车身为一个矩形，检查该矩形内是否有障碍物
bool AstarSearch::detectCollision(const SimpleNode& sn)
{
  // Define the robot as rectangle
  static double left = -1.0 * robot_base2back_;
  static double right = robot_length_ - robot_base2back_;
  static double top = robot_width_ / 2.0;
  static double bottom = -1.0 * robot_width_ / 2.0;
  static double resolution = costmap_.info.resolution;

  // Coordinate of base_link in OccupancyGrid frame
  static double one_angle_range = 2.0 * M_PI / theta_size_;
  double base_x = sn.index_x * resolution;
  double base_y = sn.index_y * resolution;
  double base_theta = sn.index_theta * one_angle_range;//车身朝向

  // Calculate cos and sin in advance
  double cos_theta = std::cos(base_theta);
  double sin_theta = std::sin(base_theta);

  // Convert each point to index and check if the node is Obstacle
  for (double x = left; x < right; x += resolution)
  {
    for (double y = top; y > bottom; y -= resolution)
    {
      // 2D point rotation
      int index_x = (x * cos_theta - y * sin_theta + base_x) / resolution;
      int index_y = (x * sin_theta + y * cos_theta + base_y) / resolution;

      if (isOutOfRange(index_x, index_y))
      {
        return true;
      }
      else if (nodes_[index_y][index_x][0].status == STATUS::OBS)
      {
        return true;
      }
    }
  }

  return false;
}

// bfs的方法不考虑车的运动学约束，检查终点能否到起点，如果到不了，那就说明没有路可以到达，没有继续下去的必要了
bool AstarSearch::calcWaveFrontHeuristic(const SimpleNode& sn)
{
  // Set start point for wavefront search
  // This is goal for Astar search
  nodes_[sn.index_y][sn.index_x][0].hc = 0;
  WaveFrontNode wf_node(sn.index_x, sn.index_y, 1e-10);
  std::queue<WaveFrontNode> qu;
  qu.push(wf_node);

  // State update table for wavefront search
  // Nodes are expanded for each neighborhood cells (moore neighborhood)
  double resolution = costmap_.info.resolution;
  static std::vector<WaveFrontNode> updates = {
    getWaveFrontNode(0, 1, resolution),
    getWaveFrontNode(-1, 0, resolution),
    getWaveFrontNode(1, 0, resolution),
    getWaveFrontNode(0, -1, resolution),
    getWaveFrontNode(-1, 1, std::hypot(resolution, resolution)),
    getWaveFrontNode(1, 1, std::hypot(resolution, resolution)),
    getWaveFrontNode(-1, -1, std::hypot(resolution, resolution)),
    getWaveFrontNode(1, -1, std::hypot(resolution, resolution)),
  };

  // Get start index
  int start_index_x;
  int start_index_y;
  int start_index_theta;
  poseToIndex(start_pose_local_.pose, &start_index_x, &start_index_y, &start_index_theta);

  // Whether the robot can reach goal
  bool reachable = false;

  // Start wavefront search
  while (!qu.empty())
  {
    WaveFrontNode ref = qu.front();
    qu.pop();

    WaveFrontNode next;
    for (const auto& u : updates)
    {
      next.index_x = ref.index_x + u.index_x;
      next.index_y = ref.index_y + u.index_y;

      // out of range OR already visited OR obstacle node
      if (isOutOfRange(next.index_x, next.index_y) || nodes_[next.index_y][next.index_x][0].hc > 0 ||
          nodes_[next.index_y][next.index_x][0].status == STATUS::OBS)
      {
        continue;
      }

      // Take the size of robot into account
      if (detectCollisionWaveFront(next))
      {
        continue;
      }

      // Check if we can reach from start to goal
      if (next.index_x == start_index_x && next.index_y == start_index_y)
      {
        reachable = true;
      }

      // Set wavefront heuristic cost
      next.hc = ref.hc + u.hc;
      nodes_[next.index_y][next.index_x][0].hc = next.hc;

      qu.push(next);
    }
  }

  // End of search
  return reachable;
}

// Simple collidion detection for wavefront search以矩形做车身的碰撞检查
bool AstarSearch::detectCollisionWaveFront(const WaveFrontNode& ref)
{
  // Define the robot as square
  static double half = robot_width_ / 2;
  double robot_x = ref.index_x * costmap_.info.resolution;
  double robot_y = ref.index_y * costmap_.info.resolution;

  for (double y = half; y > -1.0 * half; y -= costmap_.info.resolution)
  {
    for (double x = -1.0 * half; x < half; x += costmap_.info.resolution)
    {
      int index_x = (robot_x + x) / costmap_.info.resolution;
      int index_y = (robot_y + y) / costmap_.info.resolution;

      if (isOutOfRange(index_x, index_y))
      {
        return true;
      }

      if (nodes_[index_y][index_x][0].status == STATUS::OBS)
      {
        return true;
      }
    }
  }

  return false;
}

void AstarSearch::reset()
{
  path_.poses.clear();

  // Clear queue
  std::priority_queue<SimpleNode, std::vector<SimpleNode>, std::greater<SimpleNode>> empty;
  std::swap(openlist_, empty);

  // ros::WallTime begin = ros::WallTime::now();

  // Reset node info here ...?
  for (size_t i = 0; i < costmap_.info.height; i++)
  {
    for (size_t j = 0; j < costmap_.info.width; j++)
    {
      for (int k = 0; k < theta_size_; k++)
      {
        // other values will be updated during the search
        nodes_[i][j][k].status = STATUS::NONE;
        nodes_[i][j][k].hc = 0;
      }
    }
  }

  // ros::WallTime end = ros::WallTime::now();

  // ROS_INFO("Reset time: %lf [ms]", (end - begin).toSec() * 1000);
}