void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
{
//project the new origin into the grid
int cell_ox, cell_oy;
cell_ox = int((new_origin_x - origin_x_) / resolution_);
cell_oy = int((new_origin_y - origin_y_) / resolution_);
if(cell_ox == 0 && cell_oy == 0){
return;
}
//compute the associated world coordinates for the origin cell
//beacuase we want to keep things grid-aligned
double new_grid_ox, new_grid_oy;
new_grid_ox = origin_x_ + cell_ox * resolution_;
new_grid_oy = origin_y_ + cell_oy * resolution_;
//To save casting from unsigned int to int a bunch of times
int size_x = size_x_;
int size_y = size_y_;
//we need to compute the overlap of the new and existing windows
int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
lower_left_x = std::min(std::max(cell_ox, 0), size_x);
lower_left_y = std::min(std::max(cell_oy, 0), size_y);
upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
unsigned int cell_size_x = upper_right_x - lower_left_x;
unsigned int cell_size_y = upper_right_y - lower_left_y;
//we need a map to store the obstacles in the window temporarily
unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
unsigned int* voxel_map = voxel_grid_.getData();
//copy the local window in the costmap to the local map
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
cell_size_y);
//we'll reset our maps to unknown space if appropriate
resetMaps();
//update the origin with the appropriate world coordinates
origin_x_ = new_grid_ox;
origin_y_ = new_grid_oy;
//compute the starting cell location for copying data back in
int start_x = lower_left_x - cell_ox;
int start_y = lower_left_y - cell_oy;
//now we want to copy the overlapping information back into the map, but in its new location
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
//make sure to clean up
delete[] local_map;
delete[] local_voxel_map;
}
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_);
}
void Costmap2D::resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y){
ROS_ASSERT_MSG(w_size_x >= 0 && w_size_y >= 0, "You cannot specify a negative size window");
double start_point_x = wx - w_size_x / 2;
double start_point_y = wy - w_size_y / 2;
double end_point_x = start_point_x + w_size_x;
double end_point_y = start_point_y + w_size_y;
//check start bounds
start_point_x = max(origin_x_, start_point_x);
start_point_y = max(origin_y_, start_point_y);
//check end bounds
end_point_x = min(origin_x_ + getSizeInMetersX(), end_point_x);
end_point_y = min(origin_y_ + getSizeInMetersY(), end_point_y);
unsigned int start_x, start_y, end_x, end_y;
//check for legality just in case
if(!worldToMap(start_point_x, start_point_y, start_x, start_y) || !worldToMap(end_point_x, end_point_y, end_x, end_y))
return;
ROS_ASSERT(end_x >= start_x && end_y >= start_y);
unsigned int cell_size_x = end_x - start_x;
unsigned int cell_size_y = end_y - start_y;
//we need a map to store the obstacles in the window temporarily
unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
//copy the local window in the costmap to the local map
copyMapRegion(costmap_, start_x, start_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
//now we'll reset the costmap to the static map
memcpy(costmap_, static_map_, size_x_ * size_y_ * sizeof(unsigned char));
//now we want to copy the local map back into the costmap
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
//clean up
delete[] local_map;
}
void Costmap2D::reshapeStaticMap(double win_origin_x, double win_origin_y,
unsigned int data_size_x, unsigned int data_size_y, const std::vector<unsigned char>& static_data){
int m_ox, m_oy;
worldToMapNoBounds(win_origin_x, win_origin_y, m_ox, m_oy);
//compute the bounds for the size of our new map
int bl_x = std::min(m_ox, 0);
int bl_y = std::min(m_oy, 0);
int ur_x = std::max(m_ox + data_size_x, size_x_);
int ur_y = std::max(m_oy + data_size_y, size_y_);
//create a temporary map to hold our static data and copy the old static map into it
unsigned char* static_map_copy = new unsigned char[size_x_ * size_y_];
memcpy(static_map_copy, static_map_, size_x_ * size_y_ * sizeof(unsigned char));
//delete our old maps... the user will lose any
//cost information not stored in the static map when reshaping a map
deleteMaps();
//update the origin and sizes, and cache data we'll need
double old_origin_x = origin_x_;
double old_origin_y = origin_y_;
unsigned int old_size_x = size_x_;
unsigned int old_size_y = size_y_;
size_x_ = ur_x - bl_x;
size_y_ = ur_y - bl_y;
origin_x_ = std::min(origin_x_, win_origin_x);
origin_y_ = std::min(origin_y_, win_origin_y);
//initialize our various maps
initMaps(size_x_, size_y_);
//reset our maps to be full of unknown space if appropriate
resetMaps();
//now, copy the old static map into the new costmap
unsigned int start_x, start_y;
worldToMap(old_origin_x, old_origin_y, start_x, start_y);
copyMapRegion(static_map_copy, 0, 0, old_size_x, costmap_, start_x, start_y, size_x_, old_size_x, old_size_y);
delete[] static_map_copy;
//now we want to update the map with the new static map data
replaceStaticMapWindow(win_origin_x, win_origin_y, data_size_x, data_size_y, static_data);
}
void Costmap2D::replaceStaticMapWindow(double win_origin_x, double win_origin_y,
unsigned int data_size_x, unsigned int data_size_y,
const std::vector<unsigned char>& static_data){
unsigned int start_x, start_y;
if(!worldToMap(win_origin_x, win_origin_y, start_x, start_y) || (start_x + data_size_x) > size_x_ || (start_y + data_size_y) > size_y_){
ROS_ERROR("You must call replaceStaticMapWindow with a window origin and size that is contained within the map");
return;
}
//we need to compute the region of the costmap that could change from inflation of new obstacles
unsigned int max_inflation_change = 2 * cell_inflation_radius_;
//make sure that we don't go out of map bounds
unsigned int copy_sx = std::min(std::max(0, (int)start_x - (int)max_inflation_change), (int)size_x_);
unsigned int copy_sy = std::min(std::max(0, (int)start_y - (int)max_inflation_change), (int)size_x_);
unsigned int copy_ex = std::max(std::min((int)size_x_, (int)start_x + (int)data_size_x + (int)max_inflation_change), 0);
unsigned int copy_ey = std::max(std::min((int)size_y_, (int)start_y + (int)data_size_y + (int)max_inflation_change), 0);
unsigned int copy_size_x = copy_ex - copy_sx;
unsigned int copy_size_y = copy_ey - copy_sy;
//now... we have to compute the window
double ll_x, ll_y, ur_x, ur_y;
mapToWorld(copy_sx, copy_sy, ll_x, ll_y);
mapToWorld(copy_ex, copy_ey, ur_x, ur_y);
double mid_x = (ll_x + ur_x) / 2;
double mid_y = (ll_y + ur_y) / 2;
double size_x = ur_x - ll_x;
double size_y = ur_y - ll_y;
//finally... we'll clear all non-lethal costs in the area that could be affected by the map update
//we'll reinflate after the map update is complete
clearNonLethal(mid_x, mid_y, size_x, size_y);
//copy static data into the costmap
unsigned int start_index = start_y * size_x_ + start_x;
unsigned char* costmap_index = costmap_ + start_index;
std::vector<unsigned char>::const_iterator static_data_index = static_data.begin();
for(unsigned int i = 0; i < data_size_y; ++i){
for(unsigned int j = 0; j < data_size_x; ++j){
//check if the static value is above the unknown or lethal thresholds
if(track_unknown_space_ && unknown_cost_value_ > 0 && *static_data_index == unknown_cost_value_)
*costmap_index = NO_INFORMATION;
else if(*static_data_index >= lethal_threshold_)
*costmap_index = LETHAL_OBSTACLE;
else
*costmap_index = FREE_SPACE;
++costmap_index;
++static_data_index;
}
costmap_index += size_x_ - data_size_x;
}
//now, we're ready to reinflate obstacles in the window that has been updated
//we won't clear all non-lethal obstacles first because the static map update
//may have included non-lethal costs
reinflateWindow(mid_x, mid_y, size_x, size_y, false);
//we also want to keep a copy of the current costmap as the static map... we'll only need to write the region that has changed
copyMapRegion(costmap_, copy_sx, copy_sy, size_x_, static_map_, copy_sx, copy_sy, size_x_, copy_size_x, copy_size_y);
}
