| 453 | } |
| 454 | |
| 455 | bool HGrid::getNextGrid(const vector<int>& grid_ids, Eigen::Vector3d& grid_pos, double& grid_yaw) { |
| 456 | // Current level 1 grid id |
| 457 | const int grid_num1 = grid1_->grid_data_.size(); |
| 458 | int grid_id1; |
| 459 | if (grid_ids[0] < grid_num1) |
| 460 | grid_id1 = grid_ids[0]; |
| 461 | else { |
| 462 | int fine = grid_ids[0] - grid_num1; // level 2 id |
| 463 | fineToCoarseId(fine, grid_id1); |
| 464 | } |
| 465 | |
| 466 | // std::cout << "current level 1 id: " << grid_id1 << std::endl; |
| 467 | |
| 468 | // Find the next different level 1 grid id |
| 469 | int grid_id2 = -1; |
| 470 | for (int i = 1; i < grid_ids.size(); ++i) { |
| 471 | if (grid_ids[i] < grid_num1) { |
| 472 | grid_id2 = grid_ids[i]; |
| 473 | break; |
| 474 | } else { |
| 475 | int fine = grid_ids[i] - grid_num1; |
| 476 | int coarse; |
| 477 | fineToCoarseId(fine, coarse); |
| 478 | if (coarse != grid_id1) { |
| 479 | grid_id2 = grid_ids[i]; |
| 480 | break; |
| 481 | } |
| 482 | } |
| 483 | } |
| 484 | |
| 485 | // std::cout << "next level 1 id: " << grid_id2 << std::endl; |
| 486 | |
| 487 | if (grid_id2 == -1) return false; |
| 488 | |
| 489 | auto& grid1 = getGrid(grid_id1); |
| 490 | auto& grid2 = getGrid(grid_id2); |
| 491 | grid_pos = grid2.center_; |
| 492 | Eigen::Vector3d dir = grid2.center_ - grid1.center_; |
| 493 | grid_yaw = atan2(dir[1], dir[0]); |
| 494 | |
| 495 | // std::cout << "grid pos: " << grid_pos.transpose() << std::endl; |
| 496 | |
| 497 | return true; |
| 498 | } |
| 499 | |
| 500 | bool HGrid::isClose(const int& id1, const int& id2) { |
| 501 | // Convert to coarse level ids |
no test coverage detected