| 220 | } |
| 221 | |
| 222 | void Costmap2D::UpdateOrigin(double new_origin_x, double new_origin_y) { |
| 223 | int cell_ox, cell_oy; |
| 224 | cell_ox = int((new_origin_x - origin_x_) / resolution_); |
| 225 | cell_oy = int((new_origin_y - origin_y_) / resolution_); |
| 226 | double new_grid_ox, new_grid_oy; |
| 227 | new_grid_ox = origin_x_ + cell_ox * resolution_; |
| 228 | new_grid_oy = origin_y_ + cell_oy * resolution_; |
| 229 | int size_x = size_x_; |
| 230 | int size_y = size_y_; |
| 231 | int lower_left_x, lower_left_y, upper_right_x, upper_right_y; |
| 232 | lower_left_x = std::min(std::max(cell_ox, 0), size_x); |
| 233 | lower_left_y = std::min(std::max(cell_oy, 0), size_y); |
| 234 | upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x); |
| 235 | upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y); |
| 236 | unsigned int cell_size_x = upper_right_x - lower_left_x; |
| 237 | unsigned int cell_size_y = upper_right_y - lower_left_y; |
| 238 | unsigned char *local_map = new unsigned char[cell_size_x * cell_size_y]; |
| 239 | CopyMapRegion(costmap_, local_map, size_x_, cell_size_x, lower_left_x, lower_left_y, 0, 0, cell_size_x, cell_size_y); |
| 240 | ResetMaps(); |
| 241 | origin_x_ = new_grid_ox; |
| 242 | origin_y_ = new_grid_oy; |
| 243 | int start_x = lower_left_x - cell_ox; |
| 244 | int start_y = lower_left_y - cell_oy; |
| 245 | CopyMapRegion(local_map, costmap_, cell_size_x, size_x_, 0, 0, start_x, start_y, cell_size_x, cell_size_y); |
| 246 | delete[] local_map; |
| 247 | } |
| 248 | |
| 249 | bool Costmap2D::SetConvexRegionCost(const std::vector<geometry_msgs::Point> &polygon, unsigned char cost_value) { |
| 250 | std::vector<MapLocation> map_polygon; |