| 186 | } |
| 187 | |
| 188 | void Astar::init() { |
| 189 | /* ---------- map params ---------- */ |
| 190 | this->inv_resolution_ = 1.0 / resolution_; |
| 191 | inv_time_resolution_ = 1.0 / time_resolution_; |
| 192 | edt_environment_->sdf_map_->getRegion(origin_, map_size_3d_); |
| 193 | cout << "origin_: " << origin_.transpose() << endl; |
| 194 | cout << "map size: " << map_size_3d_.transpose() << endl; |
| 195 | |
| 196 | path_node_pool_.resize(allocate_num_); |
| 197 | for (int i = 0; i < allocate_num_; i++) { |
| 198 | path_node_pool_[i] = new Node; |
| 199 | } |
| 200 | use_node_num_ = 0; |
| 201 | iter_num_ = 0; |
| 202 | } |
| 203 | |
| 204 | void Astar::setEnvironment(const EDTEnvironment::Ptr& env) { |
| 205 | this->edt_environment_ = env; |