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::raytraceFreespace(const std::vector<Observation>& clearing_observations){
for(unsigned int i = 0; i < clearing_observations.size(); ++i){
raytraceFreespace(clearing_observations[i]);
}
}
void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
if (rolling_window_)
updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
if (!enabled_)
return;
useExtraBounds(min_x, min_y, max_x, max_y);
bool current = true;
std::vector<ObservationSet> observations;
std::vector<Observation> clearing_observations;
if(clear_old_){
resetOldCosts(min_x, min_y, max_x, max_y);
}
//get the marking observations
current = current && getMarkingObservations(observations);
//get the clearing observations
current = current && getClearingObservations(clearing_observations);
//update the global current status
current_ = current;
//raytrace freespace
for (unsigned int i = 0; i < clearing_observations.size(); ++i)
{
raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
}
//place the new obstacles into a priority queue... each with a priority of zero to begin with
for (std::vector<ObservationSet>::const_iterator it = observations.begin(); it != observations.end(); ++it)
{
const ObservationSet& obs_set = *it;
//check if this is in the max_timeout list
bool max_timeout = false;
if(observation_timeout.find(obs_set.topic)!= observation_timeout.end()){
max_timeout = true;
ROS_DEBUG("Observation set for topic %s - clearing\n", obs_set.topic.c_str());
}
for(std::vector<Observation>::const_iterator it_o = obs_set.marking_observations.begin();
it_o != obs_set.marking_observations.end(); it_o++){
const Observation& obs = *it_o;
//we should throw out stale observations also
CostMapList cm_list;
cm_list.topic = obs_set.topic;
cm_list.obs_timestamp = obs.cloud_->header.stamp;
double obs_ts = cm_list.obs_timestamp / 1.0e6;
const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
int count = 0;
for (unsigned int i = 0; i < cloud.points.size(); ++i)
{
//if the obstacle is too high or too far away from the robot we won't add it
if (cloud.points[i].z > max_obstacle_height_)
continue;
//compute the squared distance from the hitpoint to the pointcloud's origin
double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x)
+ (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y)
+ (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z);
//if the point is far enough away... we won't consider it
if (sq_dist >= sq_obstacle_range)
continue;
//now we need to compute the map coordinates for the observation
unsigned int mx, my, mz;
if (cloud.points[i].z < origin_z_)
{
if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, origin_z_, mx, my, mz))
continue;
}
else if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z, mx, my, mz))
{
continue;
}
//mark the cell in the voxel grid and check if we should also mark it in the costmap
if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
{
unsigned int index = getIndex(mx, my);
//these are the ones set as occupied
costmap_[index] = LETHAL_OBSTACLE;
touch((double)cloud.points[i].x, (double)cloud.points[i].y, min_x, min_y, max_x, max_y);
//keep track of which indices we updated
if(max_timeout){
cm_list.indices.push_back(ObstaclePoint(index, (double)cloud.points[i].x, (double)cloud.points[i].y));
count++;
}
}
}
if(max_timeout && count > 0){
locations_utime.updateObstacleTime(cm_list);
new_obs_list.push_back(cm_list);
}
}
}
if(clear_old_){
//inflate the bounds - otherwise the inflation layer wont reset the inflated cost
*min_x -= inflation_radius_;
*min_y -= inflation_radius_;
*max_x += inflation_radius_;
*max_y += inflation_radius_;
}
if (publish_voxel_)
{
costmap_2d::VoxelGrid grid_msg;
unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
grid_msg.size_x = voxel_grid_.sizeX();
grid_msg.size_y = voxel_grid_.sizeY();
grid_msg.size_z = voxel_grid_.sizeZ();
grid_msg.data.resize(size);
memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
grid_msg.origin.x = origin_x_;
grid_msg.origin.y = origin_y_;
grid_msg.origin.z = origin_z_;
grid_msg.resolutions.x = resolution_;
grid_msg.resolutions.y = resolution_;
grid_msg.resolutions.z = z_resolution_;
grid_msg.header.frame_id = global_frame_;
grid_msg.header.stamp = ros::Time::now();
voxel_pub_.publish(grid_msg);
}
footprint_layer_.updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}
