| 14 | } |
| 15 | |
| 16 | void Astar::init(ros::NodeHandle& nh, const EDTEnvironment::Ptr& env) { |
| 17 | nh.param("astar/resolution_astar", resolution_, -1.0); |
| 18 | nh.param("astar/lambda_heu", lambda_heu_, -1.0); |
| 19 | nh.param("astar/max_search_time", max_search_time_, -1.0); |
| 20 | nh.param("astar/allocate_num", allocate_num_, -1); |
| 21 | |
| 22 | tie_breaker_ = 1.0 + 1.0 / 1000; |
| 23 | |
| 24 | this->edt_env_ = env; |
| 25 | |
| 26 | /* ---------- map params ---------- */ |
| 27 | this->inv_resolution_ = 1.0 / resolution_; |
| 28 | edt_env_->sdf_map_->getRegion(origin_, map_size_3d_); |
| 29 | cout << "origin_: " << origin_.transpose() << endl; |
| 30 | cout << "map size: " << map_size_3d_.transpose() << endl; |
| 31 | |
| 32 | path_node_pool_.resize(allocate_num_); |
| 33 | for (int i = 0; i < allocate_num_; i++) { |
| 34 | path_node_pool_[i] = new Node; |
| 35 | } |
| 36 | use_node_num_ = 0; |
| 37 | iter_num_ = 0; |
| 38 | early_terminate_cost_ = 0.0; |
| 39 | } |
| 40 | |
| 41 | void Astar::setResolution(const double& res) { |
| 42 | resolution_ = res; |