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