00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <costmap_2d/voxel_layer.h>
00039 #include <pluginlib/class_list_macros.h>
00040 #include <pcl_conversions/pcl_conversions.h>
00041
00042 #define VOXEL_BITS 16
00043 PLUGINLIB_EXPORT_CLASS(costmap_2d::VoxelLayer, costmap_2d::Layer)
00044
00045 using costmap_2d::NO_INFORMATION;
00046 using costmap_2d::LETHAL_OBSTACLE;
00047 using costmap_2d::FREE_SPACE;
00048
00049 using costmap_2d::ObservationBuffer;
00050 using costmap_2d::Observation;
00051
00052 namespace costmap_2d
00053 {
00054
00055 void VoxelLayer::onInitialize()
00056 {
00057 ObstacleLayer::onInitialize();
00058 ros::NodeHandle private_nh("~/" + name_);
00059
00060 private_nh.param("publish_voxel_map", publish_voxel_, false);
00061 if (publish_voxel_)
00062 voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1);
00063
00064 clearing_endpoints_pub_ = private_nh.advertise<sensor_msgs::PointCloud>("clearing_endpoints", 1);
00065 }
00066
00067 void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
00068 {
00069 voxel_dsrv_ = new dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>(nh);
00070 dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>::CallbackType cb = boost::bind(
00071 &VoxelLayer::reconfigureCB, this, _1, _2);
00072 voxel_dsrv_->setCallback(cb);
00073 }
00074
00075 VoxelLayer::~VoxelLayer()
00076 {
00077 if (voxel_dsrv_)
00078 delete voxel_dsrv_;
00079 }
00080
00081 void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
00082 {
00083 enabled_ = config.enabled;
00084 footprint_clearing_enabled_ = config.footprint_clearing_enabled;
00085 max_obstacle_height_ = config.max_obstacle_height;
00086 size_z_ = config.z_voxels;
00087 origin_z_ = config.origin_z;
00088 z_resolution_ = config.z_resolution;
00089 unknown_threshold_ = config.unknown_threshold + (VOXEL_BITS - size_z_);
00090 mark_threshold_ = config.mark_threshold;
00091 combination_method_ = config.combination_method;
00092 matchSize();
00093 }
00094
00095 void VoxelLayer::matchSize()
00096 {
00097 ObstacleLayer::matchSize();
00098 voxel_grid_.resize(size_x_, size_y_, size_z_);
00099 ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
00100 }
00101
00102 void VoxelLayer::reset()
00103 {
00104 deactivate();
00105 resetMaps();
00106 voxel_grid_.reset();
00107 activate();
00108 }
00109
00110 void VoxelLayer::resetMaps()
00111 {
00112 Costmap2D::resetMaps();
00113 voxel_grid_.reset();
00114 }
00115
00116 void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
00117 double* min_y, double* max_x, double* max_y)
00118 {
00119 if (rolling_window_)
00120 updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
00121 if (!enabled_)
00122 return;
00123 useExtraBounds(min_x, min_y, max_x, max_y);
00124
00125 bool current = true;
00126 std::vector<Observation> observations, clearing_observations;
00127
00128
00129 current = getMarkingObservations(observations) && current;
00130
00131
00132 current = getClearingObservations(clearing_observations) && current;
00133
00134
00135 current_ = current;
00136
00137
00138 for (unsigned int i = 0; i < clearing_observations.size(); ++i)
00139 {
00140 raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
00141 }
00142
00143
00144 for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
00145 {
00146 const Observation& obs = *it;
00147
00148 const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);
00149
00150 double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
00151
00152 for (unsigned int i = 0; i < cloud.points.size(); ++i)
00153 {
00154
00155 if (cloud.points[i].z > max_obstacle_height_)
00156 continue;
00157
00158
00159 double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x)
00160 + (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y)
00161 + (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z);
00162
00163
00164 if (sq_dist >= sq_obstacle_range)
00165 continue;
00166
00167
00168 unsigned int mx, my, mz;
00169 if (cloud.points[i].z < origin_z_)
00170 {
00171 if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, origin_z_, mx, my, mz))
00172 continue;
00173 }
00174 else if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z, mx, my, mz))
00175 {
00176 continue;
00177 }
00178
00179
00180 if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
00181 {
00182 unsigned int index = getIndex(mx, my);
00183
00184 costmap_[index] = LETHAL_OBSTACLE;
00185 touch((double)cloud.points[i].x, (double)cloud.points[i].y, min_x, min_y, max_x, max_y);
00186 }
00187 }
00188 }
00189
00190 if (publish_voxel_)
00191 {
00192 costmap_2d::VoxelGrid grid_msg;
00193 unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
00194 grid_msg.size_x = voxel_grid_.sizeX();
00195 grid_msg.size_y = voxel_grid_.sizeY();
00196 grid_msg.size_z = voxel_grid_.sizeZ();
00197 grid_msg.data.resize(size);
00198 memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
00199
00200 grid_msg.origin.x = origin_x_;
00201 grid_msg.origin.y = origin_y_;
00202 grid_msg.origin.z = origin_z_;
00203
00204 grid_msg.resolutions.x = resolution_;
00205 grid_msg.resolutions.y = resolution_;
00206 grid_msg.resolutions.z = z_resolution_;
00207 grid_msg.header.frame_id = global_frame_;
00208 grid_msg.header.stamp = ros::Time::now();
00209 voxel_pub_.publish(grid_msg);
00210 }
00211
00212 updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
00213 }
00214
00215 void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
00216 {
00217
00218 unsigned int mx, my;
00219 if (!worldToMap(wx, wy, mx, my))
00220 return;
00221
00222
00223 double start_x = wx - w_size_x / 2;
00224 double start_y = wy - w_size_y / 2;
00225 double end_x = start_x + w_size_x;
00226 double end_y = start_y + w_size_y;
00227
00228
00229 start_x = std::max(origin_x_, start_x);
00230 start_y = std::max(origin_y_, start_y);
00231
00232 end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
00233 end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);
00234
00235
00236 unsigned int map_sx, map_sy, map_ex, map_ey;
00237
00238
00239 if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
00240 return;
00241
00242
00243 unsigned int index = getIndex(map_sx, map_sy);
00244 unsigned char* current = &costmap_[index];
00245 for (unsigned int j = map_sy; j <= map_ey; ++j)
00246 {
00247 for (unsigned int i = map_sx; i <= map_ex; ++i)
00248 {
00249
00250 if (*current != LETHAL_OBSTACLE)
00251 {
00252 if (clear_no_info || *current != NO_INFORMATION)
00253 {
00254 *current = FREE_SPACE;
00255 voxel_grid_.clearVoxelColumn(index);
00256 }
00257 }
00258 current++;
00259 index++;
00260 }
00261 current += size_x_ - (map_ex - map_sx) - 1;
00262 index += size_x_ - (map_ex - map_sx) - 1;
00263 }
00264 }
00265
00266 void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
00267 double* max_x, double* max_y)
00268 {
00269 if (clearing_observation.cloud_->points.size() == 0)
00270 return;
00271
00272 double sensor_x, sensor_y, sensor_z;
00273 double ox = clearing_observation.origin_.x;
00274 double oy = clearing_observation.origin_.y;
00275 double oz = clearing_observation.origin_.z;
00276
00277 if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
00278 {
00279 ROS_WARN_THROTTLE(
00280 1.0,
00281 "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
00282 ox, oy, oz);
00283 return;
00284 }
00285
00286 bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
00287 if (publish_clearing_points)
00288 {
00289 clearing_endpoints_.points.clear();
00290 clearing_endpoints_.points.reserve(clearing_observation.cloud_->points.size());
00291 }
00292
00293
00294 double map_end_x = origin_x_ + getSizeInMetersX();
00295 double map_end_y = origin_y_ + getSizeInMetersY();
00296
00297 for (unsigned int i = 0; i < clearing_observation.cloud_->points.size(); ++i)
00298 {
00299 double wpx = clearing_observation.cloud_->points[i].x;
00300 double wpy = clearing_observation.cloud_->points[i].y;
00301 double wpz = clearing_observation.cloud_->points[i].z;
00302
00303 double distance = dist(ox, oy, oz, wpx, wpy, wpz);
00304 double scaling_fact = 1.0;
00305 scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
00306 wpx = scaling_fact * (wpx - ox) + ox;
00307 wpy = scaling_fact * (wpy - oy) + oy;
00308 wpz = scaling_fact * (wpz - oz) + oz;
00309
00310 double a = wpx - ox;
00311 double b = wpy - oy;
00312 double c = wpz - oz;
00313 double t = 1.0;
00314
00315
00316 if (wpz > max_obstacle_height_)
00317 {
00318
00319 t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
00320 }
00321
00322 else if (wpz < origin_z_)
00323 {
00324
00325 t = std::min(t, (origin_z_ - oz) / c);
00326 }
00327
00328
00329 if (wpx < origin_x_)
00330 {
00331 t = std::min(t, (origin_x_ - ox) / a);
00332 }
00333 if (wpy < origin_y_)
00334 {
00335 t = std::min(t, (origin_y_ - oy) / b);
00336 }
00337
00338
00339 if (wpx > map_end_x)
00340 {
00341 t = std::min(t, (map_end_x - ox) / a);
00342 }
00343 if (wpy > map_end_y)
00344 {
00345 t = std::min(t, (map_end_y - oy) / b);
00346 }
00347
00348 wpx = ox + a * t;
00349 wpy = oy + b * t;
00350 wpz = oz + c * t;
00351
00352 double point_x, point_y, point_z;
00353 if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z))
00354 {
00355 unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
00356
00357
00358 voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
00359 unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
00360 cell_raytrace_range);
00361
00362 updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
00363
00364 if (publish_clearing_points)
00365 {
00366 geometry_msgs::Point32 point;
00367 point.x = wpx;
00368 point.y = wpy;
00369 point.z = wpz;
00370 clearing_endpoints_.points.push_back(point);
00371 }
00372 }
00373 }
00374
00375 if (publish_clearing_points)
00376 {
00377 clearing_endpoints_.header.frame_id = global_frame_;
00378 clearing_endpoints_.header.stamp = pcl_conversions::fromPCL(clearing_observation.cloud_->header).stamp;
00379 clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
00380
00381 clearing_endpoints_pub_.publish(clearing_endpoints_);
00382 }
00383 }
00384
00385 void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
00386 {
00387
00388 int cell_ox, cell_oy;
00389 cell_ox = int((new_origin_x - origin_x_) / resolution_);
00390 cell_oy = int((new_origin_y - origin_y_) / resolution_);
00391
00392
00393
00394 double new_grid_ox, new_grid_oy;
00395 new_grid_ox = origin_x_ + cell_ox * resolution_;
00396 new_grid_oy = origin_y_ + cell_oy * resolution_;
00397
00398
00399 int size_x = size_x_;
00400 int size_y = size_y_;
00401
00402
00403 int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
00404 lower_left_x = std::min(std::max(cell_ox, 0), size_x);
00405 lower_left_y = std::min(std::max(cell_oy, 0), size_y);
00406 upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
00407 upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
00408
00409 unsigned int cell_size_x = upper_right_x - lower_left_x;
00410 unsigned int cell_size_y = upper_right_y - lower_left_y;
00411
00412
00413 unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
00414 unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
00415 unsigned int* voxel_map = voxel_grid_.getData();
00416
00417
00418 copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
00419 copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
00420 cell_size_y);
00421
00422
00423 resetMaps();
00424
00425
00426 origin_x_ = new_grid_ox;
00427 origin_y_ = new_grid_oy;
00428
00429
00430 int start_x = lower_left_x - cell_ox;
00431 int start_y = lower_left_y - cell_oy;
00432
00433
00434 copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
00435 copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
00436
00437
00438 delete[] local_map;
00439 delete[] local_voxel_map;
00440 }
00441
00442 }