55 void VoxelLayer::onInitialize()
57 ObstacleLayer::onInitialize();
60 private_nh.
param(
"publish_voxel_map", publish_voxel_,
false);
62 voxel_pub_ = private_nh.
advertise < costmap_2d::VoxelGrid > (
"voxel_grid", 1);
64 clearing_endpoints_pub_ = private_nh.
advertise<sensor_msgs::PointCloud>(
"clearing_endpoints", 1);
69 voxel_dsrv_ =
new dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>(nh);
70 dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>::CallbackType cb = boost::bind(
71 &VoxelLayer::reconfigureCB,
this, _1, _2);
72 voxel_dsrv_->setCallback(cb);
75 VoxelLayer::~VoxelLayer()
81 void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
83 enabled_ = config.enabled;
84 footprint_clearing_enabled_ = config.footprint_clearing_enabled;
85 max_obstacle_height_ = config.max_obstacle_height;
86 size_z_ = config.z_voxels;
87 origin_z_ = config.origin_z;
88 z_resolution_ = config.z_resolution;
89 unknown_threshold_ = config.unknown_threshold + (
VOXEL_BITS - size_z_);
90 mark_threshold_ = config.mark_threshold;
91 combination_method_ = config.combination_method;
95 void VoxelLayer::matchSize()
97 ObstacleLayer::matchSize();
98 voxel_grid_.resize(size_x_, size_y_, size_z_);
99 ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
102 void VoxelLayer::reset()
110 void VoxelLayer::resetMaps()
112 Costmap2D::resetMaps();
116 void VoxelLayer::updateBounds(
double robot_x,
double robot_y,
double robot_yaw,
double* min_x,
117 double* min_y,
double* max_x,
double* max_y)
120 updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
123 useExtraBounds(min_x, min_y, max_x, max_y);
126 std::vector<Observation> observations, clearing_observations;
129 current = getMarkingObservations(observations) && current;
132 current = getClearingObservations(clearing_observations) && current;
138 for (
unsigned int i = 0; i < clearing_observations.size(); ++i)
140 raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
144 for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
146 const Observation& obs = *it;
148 const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.
cloud_);
152 for (
unsigned int i = 0; i < cloud.points.size(); ++i)
155 if (cloud.points[i].z > max_obstacle_height_)
159 double sq_dist = (cloud.points[i].x - obs.
origin_.x) * (cloud.points[i].x - obs.
origin_.x)
160 + (cloud.points[i].y - obs.
origin_.y) * (cloud.points[i].y - obs.
origin_.y)
161 + (cloud.points[i].z - obs.
origin_.z) * (cloud.points[i].z - obs.
origin_.z);
164 if (sq_dist >= sq_obstacle_range)
168 unsigned int mx, my, mz;
169 if (cloud.points[i].z < origin_z_)
171 if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, origin_z_, mx, my, mz))
174 else if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z, mx, my, mz))
180 if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
182 unsigned int index = getIndex(mx, my);
185 touch((
double)cloud.points[i].x, (
double)cloud.points[i].y, min_x, min_y, max_x, max_y);
192 costmap_2d::VoxelGrid grid_msg;
193 unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
194 grid_msg.size_x = voxel_grid_.sizeX();
195 grid_msg.size_y = voxel_grid_.sizeY();
196 grid_msg.size_z = voxel_grid_.sizeZ();
197 grid_msg.data.resize(size);
198 memcpy(&grid_msg.data[0], voxel_grid_.getData(), size *
sizeof(
unsigned int));
200 grid_msg.origin.x = origin_x_;
201 grid_msg.origin.y = origin_y_;
202 grid_msg.origin.z = origin_z_;
204 grid_msg.resolutions.x = resolution_;
205 grid_msg.resolutions.y = resolution_;
206 grid_msg.resolutions.z = z_resolution_;
207 grid_msg.header.frame_id = global_frame_;
209 voxel_pub_.publish(grid_msg);
212 updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
215 void VoxelLayer::clearNonLethal(
double wx,
double wy,
double w_size_x,
double w_size_y,
bool clear_no_info)
219 if (!worldToMap(wx, wy, mx, my))
223 double start_x = wx - w_size_x / 2;
224 double start_y = wy - w_size_y / 2;
225 double end_x = start_x + w_size_x;
226 double end_y = start_y + w_size_y;
229 start_x = std::max(origin_x_, start_x);
230 start_y = std::max(origin_y_, start_y);
232 end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
233 end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);
236 unsigned int map_sx, map_sy, map_ex, map_ey;
239 if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
243 unsigned int index = getIndex(map_sx, map_sy);
244 unsigned char* current = &costmap_[index];
245 for (
unsigned int j = map_sy; j <= map_ey; ++j)
247 for (
unsigned int i = map_sx; i <= map_ex; ++i)
250 if (*current != LETHAL_OBSTACLE)
252 if (clear_no_info || *current != NO_INFORMATION)
255 voxel_grid_.clearVoxelColumn(index);
261 current += size_x_ - (map_ex - map_sx) - 1;
262 index += size_x_ - (map_ex - map_sx) - 1;
266 void VoxelLayer::raytraceFreespace(
const Observation& clearing_observation,
double* min_x,
double* min_y,
267 double* max_x,
double* max_y)
269 if (clearing_observation.
cloud_->points.size() == 0)
272 double sensor_x, sensor_y, sensor_z;
273 double ox = clearing_observation.
origin_.x;
274 double oy = clearing_observation.
origin_.y;
275 double oz = clearing_observation.
origin_.z;
277 if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
281 "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
286 bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
287 if (publish_clearing_points)
289 clearing_endpoints_.points.clear();
290 clearing_endpoints_.points.reserve(clearing_observation.
cloud_->points.size());
294 double map_end_x = origin_x_ + getSizeInMetersX();
295 double map_end_y = origin_y_ + getSizeInMetersY();
297 for (
unsigned int i = 0; i < clearing_observation.
cloud_->points.size(); ++i)
299 double wpx = clearing_observation.
cloud_->points[i].x;
300 double wpy = clearing_observation.
cloud_->points[i].y;
301 double wpz = clearing_observation.
cloud_->points[i].z;
303 double distance = dist(ox, oy, oz, wpx, wpy, wpz);
304 double scaling_fact = 1.0;
305 scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
306 wpx = scaling_fact * (wpx - ox) + ox;
307 wpy = scaling_fact * (wpy - oy) + oy;
308 wpz = scaling_fact * (wpz - oz) + oz;
316 if (wpz > max_obstacle_height_)
319 t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
322 else if (wpz < origin_z_)
325 t = std::min(t, (origin_z_ - oz) / c);
331 t = std::min(t, (origin_x_ - ox) / a);
335 t = std::min(t, (origin_y_ - oy) / b);
341 t = std::min(t, (map_end_x - ox) / a);
345 t = std::min(t, (map_end_y - oy) / b);
352 double point_x, point_y, point_z;
353 if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z))
355 unsigned int cell_raytrace_range = cellDistance(clearing_observation.
raytrace_range_);
358 voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
359 unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
360 cell_raytrace_range);
362 updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.
raytrace_range_, min_x, min_y, max_x, max_y);
364 if (publish_clearing_points)
366 geometry_msgs::Point32 point;
370 clearing_endpoints_.points.push_back(point);
375 if (publish_clearing_points)
377 clearing_endpoints_.header.frame_id = global_frame_;
379 clearing_endpoints_.header.seq = clearing_observation.
cloud_->header.seq;
381 clearing_endpoints_pub_.publish(clearing_endpoints_);
385 void VoxelLayer::updateOrigin(
double new_origin_x,
double new_origin_y)
388 int cell_ox, cell_oy;
389 cell_ox = int((new_origin_x - origin_x_) / resolution_);
390 cell_oy = int((new_origin_y - origin_y_) / resolution_);
394 double new_grid_ox, new_grid_oy;
395 new_grid_ox = origin_x_ + cell_ox * resolution_;
396 new_grid_oy = origin_y_ + cell_oy * resolution_;
399 int size_x = size_x_;
400 int size_y = size_y_;
403 int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
404 lower_left_x = std::min(std::max(cell_ox, 0), size_x);
405 lower_left_y = std::min(std::max(cell_oy, 0), size_y);
406 upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
407 upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
409 unsigned int cell_size_x = upper_right_x - lower_left_x;
410 unsigned int cell_size_y = upper_right_y - lower_left_y;
413 unsigned char* local_map =
new unsigned char[cell_size_x * cell_size_y];
414 unsigned int* local_voxel_map =
new unsigned int[cell_size_x * cell_size_y];
415 unsigned int* voxel_map = voxel_grid_.getData();
418 copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
419 copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
426 origin_x_ = new_grid_ox;
427 origin_y_ = new_grid_oy;
430 int start_x = lower_left_x - cell_ox;
431 int start_y = lower_left_y - cell_oy;
434 copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
435 copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
439 delete[] local_voxel_map;
geometry_msgs::Point origin_
#define ROS_WARN_THROTTLE(rate,...)
pcl::PointCloud< pcl::PointXYZ > * cloud_
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
Stores an observation in terms of a point cloud and the origin of the source.
static const unsigned char FREE_SPACE
geometry_msgs::TransformStamped t
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
double distance(double x0, double y0, double x1, double y1)
Takes in point clouds from sensors, transforms them to the desired frame, and stores them...
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static const unsigned char LETHAL_OBSTACLE
static const unsigned char NO_INFORMATION