| 8 | namespace fast_planner { |
| 9 | |
| 10 | HGrid::HGrid(const shared_ptr<EDTEnvironment>& edt, ros::NodeHandle& nh) { |
| 11 | |
| 12 | this->edt_ = edt; |
| 13 | nh.param("partitioning/consistent_cost", consistent_cost_, 3.5); |
| 14 | nh.param("partitioning/consistent_cost2", consistent_cost2_, 3.5); |
| 15 | nh.param("partitioning/use_swarm_tf", use_swarm_tf_, false); |
| 16 | nh.param("partitioning/w_first", w_first_, 1.0); |
| 17 | |
| 18 | path_finder_.reset(new Astar); |
| 19 | path_finder_->init(nh, edt); |
| 20 | |
| 21 | grid1_.reset(new UniformGrid(edt, nh, 1)); |
| 22 | grid2_.reset(new UniformGrid(edt, nh, 2)); |
| 23 | |
| 24 | // Swarm tf |
| 25 | grid1_->use_swarm_tf_ = grid2_->use_swarm_tf_ = use_swarm_tf_; |
| 26 | double yaw = 0.0; |
| 27 | rot_sw_ << cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1; |
| 28 | trans_sw_ << 0.0, 0.0, 0; |
| 29 | grid1_->rot_sw_ = grid2_->rot_sw_ = rot_sw_; |
| 30 | grid1_->trans_sw_ = grid2_->trans_sw_ = trans_sw_; |
| 31 | |
| 32 | // Wait for swarm basecoor transform and initialize grid |
| 33 | // while (!updateBaseCoor()) { |
| 34 | // ROS_WARN("Wait for basecoor."); |
| 35 | // ros::Duration(0.5).sleep(); |
| 36 | // ros::spinOnce(); |
| 37 | // } |
| 38 | grid1_->initGridData(); |
| 39 | grid2_->initGridData(); |
| 40 | updateBaseCoor(); |
| 41 | } |
| 42 | |
| 43 | HGrid::~HGrid() { |
| 44 | } |
nothing calls this directly
no test coverage detected