void Costmap2D::updateWorld(double robot_x, double robot_y,
const vector<Observation>& observations, const vector<Observation>& clearing_observations){
//reset the markers for inflation
memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
//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");
//raytrace freespace
raytraceFreespace(clearing_observations);
//if we raytrace X meters out... we must re-inflate obstacles within the containing square of that circle
double inflation_window_size = 2 * (max_raytrace_range_ + inflation_radius_);
//clear all non-lethal obstacles in preparation for re-inflation
clearNonLethal(robot_x, robot_y, inflation_window_size, inflation_window_size);
//reset the inflation window
resetInflationWindow(robot_x, robot_y, inflation_window_size + 2 * inflation_radius_, inflation_window_size + 2 * inflation_radius_, inflation_queue_, false);
//now we also want to add the new obstacles we've received to the cost map
updateObstacles(observations, inflation_queue_);
inflateObstacles(inflation_queue_);
}
void Costmap2D::reinflateWindow(double wx, double wy, double w_size_x, double w_size_y, bool clear){
//reset the markers for inflation
memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
//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");
//reset the inflation window.. adds all lethal costs to the queue for re-propagation
resetInflationWindow(wx, wy, w_size_x, w_size_y, inflation_queue_, clear);
//inflate the obstacles
inflateObstacles(inflation_queue_);
}
void Costmap2D::replaceFullMap(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){
//delete our old maps
deleteMaps();
//update the origin and size of the new map
size_x_ = data_size_x;
size_y_ = data_size_y;
origin_x_ = win_origin_x;
origin_y_ = win_origin_y;
//initialize our various maps
initMaps(size_x_, size_y_);
//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();
//copy static data into the costmap
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));
}
void BaseStrategy::slamMapCb(const nav_msgs::OccupancyGrid& slam_map)
{
if (finished_)
return;
ros::WallTime startTime = ros::WallTime::now();
ROS_INFO_STREAM("Received new map. Processing data... (safety radius = " << safety_radius_ << ")");
GridMap* map = new GridMap(slam_map);
const int Rinflate = std::ceil(safety_radius_ / map->resolution);
inflateObstacles(*map, Rinflate);
const int Rblacklist = std::ceil(blacklist_radius_ / map->resolution);
inflateBlacklist(*map, blacklist_, Rblacklist);
ros::WallDuration elapsedTime = ros::WallTime::now() - startTime;
ROS_INFO_STREAM("Map replaced. Tweaked bits for " << elapsedTime.toSec() << "s.");
map_.reset(map);
map_updated_ = 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();
}
}
