void InflationLayer::onFootprintChanged() { inscribed_radius_ = layered_costmap_->getInscribedRadius(); cell_inflation_radius_ = cellDistance( inflation_radius_ ); computeCaches(); need_reinflation_ = true; ROS_DEBUG( "InflationLayer::onFootprintChanged(): num footprint points: %lu, inscribed_radius_ = %.3f, inflation_radius_ = %.3f", layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_ ); }
void InflationLayer::matchSize() { costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap(); resolution_ = costmap->getResolution(); cell_inflation_radius_ = cellDistance(inflation_radius_); computeCaches(); unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY(); if (seen_) delete seen_; seen_ = new bool[size_x * size_y]; }
void InflationLayer::matchSize() { boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap(); resolution_ = costmap->getResolution(); cell_inflation_radius_ = cellDistance(inflation_radius_); computeCaches(); unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY(); if (seen_) delete[] seen_; seen_size_ = size_x * size_y; seen_ = new bool[seen_size_]; }
void InflationLayer::setInflationParameters(double inflation_radius, double cost_scaling_factor) { if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius) { // Lock here so that reconfiguring the inflation radius doesn't cause segfaults // when accessing the cached arrays boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); inflation_radius_ = inflation_radius; cell_inflation_radius_ = cellDistance(inflation_radius_); weight_ = cost_scaling_factor; need_reinflation_ = true; computeCaches(); } }
void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level) { if (weight_ != config.cost_scaling_factor || inflation_radius_ != config.inflation_radius) { inflation_radius_ = config.inflation_radius; cell_inflation_radius_ = cellDistance(inflation_radius_); weight_ = config.cost_scaling_factor; need_reinflation_ = true; computeCaches(); } if (enabled_ != config.enabled) { enabled_ = config.enabled; need_reinflation_ = true; } }
void Costmap2D::raytraceFreespace(const Observation& clearing_observation){ //create the functor that we'll use to clear cells from the costmap ClearCell clearer(costmap_); double ox = clearing_observation.origin_.x; double oy = clearing_observation.origin_.y; sensor_msgs::PointCloud cloud = clearing_observation.cloud_; //get the map coordinates of the origin of the sensor unsigned int x0, y0; if(!worldToMap(ox, oy, x0, y0)){ ROS_WARN("The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.", ox, oy); return; } //we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later double map_end_x = origin_x_ + getSizeInMetersX(); double map_end_y = origin_y_ + getSizeInMetersY(); //for each point in the cloud, we want to trace a line from the origin and clear obstacles along it for(unsigned int i = 0; i < cloud.points.size(); ++i){ double wx = cloud.points[i].x; double wy = cloud.points[i].y; //now we also need to make sure that the enpoint we're raytracing //to isn't off the costmap and scale if necessary double a = wx - ox; double b = wy - oy; //the minimum value to raytrace from is the origin if(wx < origin_x_){ double t = (origin_x_ - ox) / a; wx = origin_x_; wy = oy + b * t; } if(wy < origin_y_){ double t = (origin_y_ - oy) / b; wx = ox + a * t; wy = origin_y_; } //the maximum value to raytrace to is the end of the map if(wx > map_end_x){ double t = (map_end_x - ox) / a; wx = map_end_x; wy = oy + b * t; } if(wy > map_end_y){ double t = (map_end_y - oy) / b; wx = ox + a * t; wy = map_end_y; } //now that the vector is scaled correctly... we'll get the map coordinates of its endpoint unsigned int x1, y1; //check for legality just in case if(!worldToMap(wx, wy, x1, y1)) continue; unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_); //and finally... we can execute our trace to clear obstacles along that line raytraceLine(clearer, x0, y0, x1, y1, cell_raytrace_range); } }
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(); } }
void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y, double* max_x, double* max_y) { if (clearing_observation.cloud_->points.size() == 0) return; double sensor_x, sensor_y, sensor_z; double ox = clearing_observation.origin_.x; double oy = clearing_observation.origin_.y; double oz = clearing_observation.origin_.z; if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) { ROS_WARN_THROTTLE( 1.0, "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.", ox, oy, oz); return; } bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0); if( publish_clearing_points ) { clearing_endpoints_.points.clear(); clearing_endpoints_.points.reserve( clearing_observation.cloud_->points.size() ); } //we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later double map_end_x = origin_x_ + getSizeInMetersX(); double map_end_y = origin_y_ + getSizeInMetersY(); for (unsigned int i = 0; i < clearing_observation.cloud_->points.size(); ++i) { double wpx = clearing_observation.cloud_->points[i].x; double wpy = clearing_observation.cloud_->points[i].y; double wpz = clearing_observation.cloud_->points[i].z; double distance = dist(ox, oy, oz, wpx, wpy, wpz); double scaling_fact = 1.0; scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0); wpx = scaling_fact * (wpx - ox) + ox; wpy = scaling_fact * (wpy - oy) + oy; wpz = scaling_fact * (wpz - oz) + oz; double a = wpx - ox; double b = wpy - oy; double c = wpz - oz; double t = 1.0; //we can only raytrace to a maximum z height if (wpz > max_obstacle_height_) { //we know we want the vector's z value to be max_z t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c)); } //and we can only raytrace down to the floor else if (wpz < origin_z_) { //we know we want the vector's z value to be 0.0 t = std::min(t, (origin_z_ - oz) / c); } //the minimum value to raytrace from is the origin if (wpx < origin_x_) { t = std::min(t, (origin_x_ - ox) / a); } if (wpy < origin_y_) { t = std::min(t, (origin_y_ - oy) / b); } //the maximum value to raytrace to is the end of the map if (wpx > map_end_x) { t = std::min(t, (map_end_x - ox) / a); } if (wpy > map_end_y) { t = std::min(t, (map_end_y - oy) / b); } wpx = ox + a * t; wpy = oy + b * t; wpz = oz + c * t; double point_x, point_y, point_z; if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z)) { unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_); //voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z); voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_, unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION, cell_raytrace_range); updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y); if( publish_clearing_points ) { geometry_msgs::Point32 point; point.x = wpx; point.y = wpy; point.z = wpz; clearing_endpoints_.points.push_back( point ); } } } if( publish_clearing_points ) { clearing_endpoints_.header.frame_id = global_frame_; clearing_endpoints_.header.stamp = pcl_conversions::fromPCL(clearing_observation.cloud_->header).stamp; clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq; clearing_endpoints_pub_.publish( clearing_endpoints_ ); } }