void Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x, double win_size_y){ //check for self windowing if(this == &map){ ROS_ERROR("Cannot convert this costmap into a window of itself"); return; } //clean up old data deleteMaps(); deleteKernels(); //compute the bounds of our new map unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y; if(!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y) || ! map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y)){ ROS_ERROR("Cannot window a map that the window bounds don't fit inside of"); return; } size_x_ = upper_right_x - lower_left_x; size_y_ = upper_right_y - lower_left_y; resolution_ = map.resolution_; origin_x_ = win_origin_x; origin_y_ = win_origin_y; ROS_DEBUG("ll(%d, %d), ur(%d, %d), size(%d, %d), origin(%.2f, %.2f)", lower_left_x, lower_left_y, upper_right_x, upper_right_y, size_x_, size_y_, origin_x_, origin_y_); //initialize our various maps and reset markers for inflation initMaps(size_x_, size_y_); //copy the window of the static map and the costmap that we're taking copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_); copyMapRegion(map.static_map_, lower_left_x, lower_left_y, map.size_x_, static_map_, 0, 0, size_x_, size_x_, size_y_); max_obstacle_range_ = map.max_obstacle_range_; max_obstacle_height_ = map.max_obstacle_height_; max_raytrace_range_ = map.max_raytrace_range_; inscribed_radius_ = map.inscribed_radius_; circumscribed_radius_ = map.circumscribed_radius_; inflation_radius_ = map.inflation_radius_; cell_inscribed_radius_ = map.cell_inscribed_radius_; cell_circumscribed_radius_ = map.cell_circumscribed_radius_; cell_inflation_radius_ = map.cell_inflation_radius_; //set the cost for the circumscribed radius of the robot circumscribed_cost_lb_ = map.circumscribed_cost_lb_; weight_ = map.weight_; //copy the cost and distance kernels copyKernels(map, cell_inflation_radius_); }
unsigned int worldToIndex(Costmap2D& map, double wx, double wy){ unsigned int mx, my; map.worldToMap(wx, wy, mx, my); return map.getIndex(mx, my); }