astar_search.cpp !first i launch the astar_navi,then,it do not work!then,i found the error in the astar_search.cpp file.and i have test the code,the error happened at the detectCollision()function!

edit

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; }

this is the function!anyone can tell me why it could detect the collision!!!!!!!!!