void VoxelGridModel::removePointsInScanBoundry(const PlanarLaserScan& laser_scan, double raytrace_range){
if(laser_scan.cloud.points.size() == 0)
return;
unsigned int sensor_x, sensor_y, sensor_z;
double ox = laser_scan.origin.x;
double oy = laser_scan.origin.y;
double oz = laser_scan.origin.z;
if(!worldToMap3D(ox, oy, oz, sensor_x, sensor_y, sensor_z))
return;
for(unsigned int i = 0; i < laser_scan.cloud.points.size(); ++i){
double wpx = laser_scan.cloud.points[i].x;
double wpy = laser_scan.cloud.points[i].y;
double wpz = laser_scan.cloud.points[i].z;
double distance = dist(ox, oy, oz, wpx, wpy, wpz);
double scaling_fact = raytrace_range / distance;
scaling_fact = scaling_fact > 1.0 ? 1.0 : scaling_fact;
wpx = scaling_fact * (wpx - ox) + ox;
wpy = scaling_fact * (wpy - oy) + oy;
wpz = scaling_fact * (wpz - oz) + oz;
//we can only raytrace to a maximum z height
if(wpz >= max_z_){
//we know we want the vector's z value to be max_z
double a = wpx - ox;
double b = wpy - oy;
double c = wpz - oz;
double t = (max_z_ - .01 - oz) / c;
wpx = ox + a * t;
wpy = oy + b * t;
wpz = oz + c * t;
}
//and we can only raytrace down to the floor
else if(wpz < 0.0){
//we know we want the vector's z value to be 0.0
double a = wpx - ox;
double b = wpy - oy;
double c = wpz - oz;
double t = (0.0 - oz) / c;
wpx = ox + a * t;
wpy = oy + b * t;
wpz = oz + c * t;
}
unsigned int point_x, point_y, point_z;
if(worldToMap3D(wpx, wpy, wpz, point_x, point_y, point_z)){
obstacle_grid_.clearVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
}
}
}
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);
}