void setKmersByPos(const std::vector<T> & reads, bool kmersByPosition) {
resetMaps();
for (const auto & read : reads) {
kmersByPos_->increaseCounts(getSeqBase(read).seq_, getSeqBase(read).cnt_);
}
setKmers(true);
}
void setKmersAnywhere(const std::vector<T> & reads) {
resetMaps();
for (const auto & read : reads) {
kmersNoPos_->increaseCounts(getSeqBase(read).seq_, getSeqBase(read).cnt_);
}
setKmers(false);
}
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 setAllKmers(const std::vector<T> & reads, bool kmersByPosition,
uint32_t kmerExpandSize) {
resetMaps();
for (const auto & read : reads) {
kmersNoPos_->increaseCounts(getSeqBase(read).seq_, getSeqBase(read).cnt_);
kmersByPos_->increaseCountsExp(getSeqBase(read).seq_, kmerExpandSize,
getSeqBase(read).cnt_);
}
setKmers(kmersByPosition);
}
void VoxelLayer::reset()
{
deactivate();
resetMaps();
voxel_grid_.reset();
if(clear_old_){
locations_utime.reset();
}
activate();
}
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);
}
bool Campaign::reset (bool unlockFirstMap)
{
const bool alreadyUnlocked = isUnlocked();
if (!alreadyUnlocked) {
Log::error(LOG_CAMPAIGN, "could not reset campaign '%s' - not yet unlocked", getId().c_str());
return false;
}
if (!_persister->resetCampaign(this)) {
Log::error(LOG_CAMPAIGN, "failed to reset the campaign");
return false;
}
resetMaps();
if (unlockFirstMap)
unlock();
return true;
}
Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
double resolution, double origin_x, double origin_y, double inscribed_radius,
double circumscribed_radius, double inflation_radius, double max_obstacle_range,
double max_obstacle_height, double max_raytrace_range, double weight,
const std::vector<unsigned char>& static_data, unsigned char lethal_threshold, bool track_unknown_space, unsigned char unknown_cost_value) : size_x_(cells_size_x),
size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), origin_y_(origin_y), static_map_(NULL),
costmap_(NULL), markers_(NULL), max_obstacle_range_(max_obstacle_range),
max_obstacle_height_(max_obstacle_height), max_raytrace_range_(max_raytrace_range), cached_costs_(NULL), cached_distances_(NULL),
inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius), inflation_radius_(inflation_radius),
weight_(weight), lethal_threshold_(lethal_threshold), track_unknown_space_(track_unknown_space), unknown_cost_value_(unknown_cost_value), inflation_queue_(){
//creat the costmap, static_map, and markers
costmap_ = new unsigned char[size_x_ * size_y_];
static_map_ = new unsigned char[size_x_ * size_y_];
markers_ = new unsigned char[size_x_ * size_y_];
memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
//convert our inflations from world to cell distance
cell_inscribed_radius_ = cellDistance(inscribed_radius);
cell_circumscribed_radius_ = cellDistance(circumscribed_radius);
cell_inflation_radius_ = cellDistance(inflation_radius);
//set the cost for the circumscribed radius of the robot
circumscribed_cost_lb_ = computeCost(cell_circumscribed_radius_);
//based on the inflation radius... compute distance and cost caches
cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];
cached_distances_ = new double*[cell_inflation_radius_ + 2];
for(unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i){
cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
cached_distances_[i] = new double[cell_inflation_radius_ + 2];
for(unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j){
cached_distances_[i][j] = sqrt(i*i + j*j);
cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
}
}
if(!static_data.empty()){
ROS_ASSERT_MSG(size_x_ * size_y_ == static_data.size(), "If you want to initialize a costmap with static data, their sizes must match.");
//make sure the inflation queue is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
unsigned int index = 0;
unsigned char* costmap_index = costmap_;
std::vector<unsigned char>::const_iterator static_data_index = static_data.begin();
//initialize the costmap with static data
for(unsigned int i = 0; i < size_y_; ++i){
for(unsigned int j = 0; j < 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;
if(*costmap_index == LETHAL_OBSTACLE){
unsigned int mx, my;
indexToCells(index, mx, my);
enqueue(index, mx, my, mx, my, inflation_queue_);
}
++costmap_index;
++static_data_index;
++index;
}
}
//now... let's inflate the obstacles
inflateObstacles(inflation_queue_);
//we also want to keep a copy of the current costmap as the static map
memcpy(static_map_, costmap_, size_x_ * size_y_ * sizeof(unsigned char));
}
else{
//everything is unknown initially if we don't have a static map unless we aren't tracking unkown space in which case it is free
resetMaps();
}
}
