| 655 | } |
| 656 | |
| 657 | void HGrid::getGridMarker2(vector<Eigen::Vector3d>& pts, vector<string>& texts) { |
| 658 | pts.clear(); |
| 659 | texts.clear(); |
| 660 | |
| 661 | for (int i = 0; i < grid1_->grid_data_.size(); ++i) { |
| 662 | auto& grid = grid1_->grid_data_[i]; |
| 663 | if (!grid.active_ || !grid.is_cur_relevant_) continue; |
| 664 | pts.push_back(grid.center_); |
| 665 | texts.push_back(to_string(i)); |
| 666 | } |
| 667 | const int grid_num1 = grid1_->grid_data_.size(); |
| 668 | for (int i = 0; i < grid2_->grid_data_.size(); ++i) { |
| 669 | auto& grid = grid2_->grid_data_[i]; |
| 670 | if (!grid.active_ || !grid.is_cur_relevant_) continue; |
| 671 | pts.push_back(grid.center_); |
| 672 | texts.push_back(to_string(i + grid_num1)); |
| 673 | } |
| 674 | } |
| 675 | |
| 676 | } // namespace fast_planner |