void BallPickerLayer::updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x, double* min_y, double* max_x, double* max_y)
{
if (!enabled_)
return;
if (layered_costmap_->isRolling())
updateOrigin(origin_x - getSizeInMetersX() / 2, origin_y - getSizeInMetersY() / 2);
double mark_x, mark_y;
unsigned int mx, my;
unsigned int index;
if (clear_flag)
{
*min_x = *min_x - getSizeInMetersX()/2;
*min_y = *min_y - getSizeInMetersY()/2;
*max_x = *max_x + getSizeInMetersX()/2;
*max_y = *max_y + getSizeInMetersY()/2;
}
for (std::vector<ball_picker::PointOfInterest>::iterator iter = obstacle_buffer.begin() ; iter != obstacle_buffer.end(); iter++)
{
mark_x = iter->x;
mark_y = iter->y;
if(worldToMap(mark_x, mark_y, mx, my))
{
index = getIndex(mx, my);
costmap_[index] = LETHAL_OBSTACLE;
}
*min_x = std::min(*min_x, mark_x);
*min_y = std::min(*min_y, mark_y);
*max_x = std::max(*max_x, mark_x);
*max_y = std::max(*max_y, mark_y);
}
}
void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
{
//get the cell coordinates of the center point of the window
unsigned int mx, my;
if (!worldToMap(wx, wy, mx, my))
return;
//compute the bounds of the window
double start_x = wx - w_size_x / 2;
double start_y = wy - w_size_y / 2;
double end_x = start_x + w_size_x;
double end_y = start_y + w_size_y;
//scale the window based on the bounds of the costmap
start_x = std::max(origin_x_, start_x);
start_y = std::max(origin_y_, start_y);
end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);
//get the map coordinates of the bounds of the window
unsigned int map_sx, map_sy, map_ex, map_ey;
//check for legality just in case
if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
return;
//we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
unsigned int index = getIndex(map_sx, map_sy);
unsigned char* current = &costmap_[index];
for (unsigned int j = map_sy; j <= map_ey; ++j)
{
for (unsigned int i = map_sx; i <= map_ex; ++i)
{
//if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
if (*current != LETHAL_OBSTACLE)
{
if (clear_no_info || *current != NO_INFORMATION)
{
*current = FREE_SPACE;
voxel_grid_.clearVoxelColumn(index);
}
}
current++;
index++;
}
current += size_x_ - (map_ex - map_sx) - 1;
index += size_x_ - (map_ex - map_sx) - 1;
}
}
void Costmap2D::resetInflationWindow(double wx, double wy, double w_size_x, double w_size_y,
priority_queue<CellData>& inflation_queue, bool clear){
//get the cell coordinates of the center point of the window
unsigned int mx, my;
if(!worldToMap(wx, wy, mx, my))
return;
//compute the bounds of the window
double start_x = wx - w_size_x / 2;
double start_y = wy - w_size_y / 2;
double end_x = start_x + w_size_x;
double end_y = start_y + w_size_y;
//scale the window based on the bounds of the costmap
start_x = max(origin_x_, start_x);
start_y = max(origin_y_, start_y);
end_x = min(origin_x_ + getSizeInMetersX(), end_x);
end_y = min(origin_y_ + getSizeInMetersY(), end_y);
//get the map coordinates of the bounds of the window
unsigned int map_sx, map_sy, map_ex, map_ey;
//check for legality just in case
if(!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey)){
ROS_ERROR("Bounds not legal for reset window. Doing nothing.");
return;
}
//we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
unsigned int index = getIndex(map_sx, map_sy);
unsigned char* current = &costmap_[index];
for(unsigned int j = map_sy; j <= map_ey; ++j){
for(unsigned int i = map_sx; i <= map_ex; ++i){
//if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
if(*current == LETHAL_OBSTACLE)
enqueue(index, i, j, i, j, inflation_queue);
else if(clear && *current != NO_INFORMATION)
*current = FREE_SPACE;
current++;
index++;
}
current += size_x_ - (map_ex - map_sx) - 1;
index += size_x_ - (map_ex - map_sx) - 1;
}
}
void Costmap2D::resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y){
ROS_ASSERT_MSG(w_size_x >= 0 && w_size_y >= 0, "You cannot specify a negative size window");
double start_point_x = wx - w_size_x / 2;
double start_point_y = wy - w_size_y / 2;
double end_point_x = start_point_x + w_size_x;
double end_point_y = start_point_y + w_size_y;
//check start bounds
start_point_x = max(origin_x_, start_point_x);
start_point_y = max(origin_y_, start_point_y);
//check end bounds
end_point_x = min(origin_x_ + getSizeInMetersX(), end_point_x);
end_point_y = min(origin_y_ + getSizeInMetersY(), end_point_y);
unsigned int start_x, start_y, end_x, end_y;
//check for legality just in case
if(!worldToMap(start_point_x, start_point_y, start_x, start_y) || !worldToMap(end_point_x, end_point_y, end_x, end_y))
return;
ROS_ASSERT(end_x >= start_x && end_y >= start_y);
unsigned int cell_size_x = end_x - start_x;
unsigned int cell_size_y = end_y - start_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];
//copy the local window in the costmap to the local map
copyMapRegion(costmap_, start_x, start_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
//now we'll reset the costmap to the static map
memcpy(costmap_, static_map_, size_x_ * size_y_ * sizeof(unsigned char));
//now we want to copy the local map back into the costmap
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
//clean up
delete[] local_map;
}
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);
}
}
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_ );
}
}
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);
}
