| 187 | } |
| 188 | |
| 189 | void StaticLayer::UpdateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { |
| 190 | if(!map_received_) { |
| 191 | return; |
| 192 | } |
| 193 | if(!layered_costmap_->IsRollingWindow()) { |
| 194 | if(!use_maximum_) { |
| 195 | UpdateOverwriteByAll(master_grid, min_i, min_j, max_i, max_j); |
| 196 | } else { |
| 197 | UpdateOverwriteByMax(master_grid, min_i, min_j, max_i, max_j); |
| 198 | } |
| 199 | } else { |
| 200 | unsigned int mx, my; |
| 201 | double wx, wy; |
| 202 | tf::StampedTransform temp_transform; |
| 203 | try { |
| 204 | tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), temp_transform); |
| 205 | } |
| 206 | catch (tf::TransformException ex) { |
| 207 | ROS_ERROR("%s", ex.what()); |
| 208 | return; |
| 209 | } |
| 210 | for(auto i = min_i; i < max_i; ++i) { |
| 211 | for(auto j = min_j; j < max_j; ++j) { |
| 212 | layered_costmap_->GetCostMap()->Map2World(i, j, wx, wy); |
| 213 | tf::Point p(wx, wy, 0); |
| 214 | p = temp_transform(p); |
| 215 | if(World2Map(p.x(), p.y(), mx, my)){ |
| 216 | if(!use_maximum_) { |
| 217 | master_grid.SetCost(i, j, GetCost(mx, my)); |
| 218 | } |
| 219 | else { |
| 220 | master_grid.SetCost(i, j, std::max(master_grid.GetCost(i, j), GetCost(i, j))); |
| 221 | } |
| 222 | } |
| 223 | } |
| 224 | } |
| 225 | } |
| 226 | } |
| 227 | |
| 228 | } //namespace roborts_costmap |
| 229 |
no test coverage detected