| 351 | } |
| 352 | |
| 353 | void MapROS::publishMapLocal() { |
| 354 | pcl::PointXYZ pt; |
| 355 | pcl::PointCloud<pcl::PointXYZ> cloud; |
| 356 | pcl::PointCloud<pcl::PointXYZ> cloud2; |
| 357 | Eigen::Vector3i min_cut = map_->md_->local_bound_min_; |
| 358 | Eigen::Vector3i max_cut = map_->md_->local_bound_max_; |
| 359 | map_->boundIndex(min_cut); |
| 360 | map_->boundIndex(max_cut); |
| 361 | |
| 362 | // for (int z = min_cut(2); z <= max_cut(2); ++z) |
| 363 | for (int x = min_cut(0); x <= max_cut(0); ++x) |
| 364 | for (int y = min_cut(1); y <= max_cut(1); ++y) |
| 365 | for (int z = map_->mp_->box_min_(2); z < map_->mp_->box_max_(2); ++z) { |
| 366 | if (map_->md_->occupancy_buffer_[map_->toAddress(x, y, z)] > |
| 367 | map_->mp_->min_occupancy_log_) { |
| 368 | // Occupied cells |
| 369 | Eigen::Vector3d pos; |
| 370 | map_->indexToPos(Eigen::Vector3i(x, y, z), pos); |
| 371 | if (pos(2) > visualization_truncate_height_) continue; |
| 372 | if (pos(2) < visualization_truncate_low_) continue; |
| 373 | |
| 374 | pt.x = pos(0); |
| 375 | pt.y = pos(1); |
| 376 | pt.z = pos(2); |
| 377 | cloud.push_back(pt); |
| 378 | } |
| 379 | // else if (map_->md_->occupancy_buffer_inflate_[map_->toAddress(x, y, z)] == 1) |
| 380 | // { |
| 381 | // // Inflated occupied cells |
| 382 | // Eigen::Vector3d pos; |
| 383 | // map_->indexToPos(Eigen::Vector3i(x, y, z), pos); |
| 384 | // if (pos(2) > visualization_truncate_height_) |
| 385 | // continue; |
| 386 | // if (pos(2) < visualization_truncate_low_) |
| 387 | // continue; |
| 388 | |
| 389 | // pt.x = pos(0); |
| 390 | // pt.y = pos(1); |
| 391 | // pt.z = pos(2); |
| 392 | // cloud2.push_back(pt); |
| 393 | // } |
| 394 | } |
| 395 | |
| 396 | cloud.width = cloud.points.size(); |
| 397 | cloud.height = 1; |
| 398 | cloud.is_dense = true; |
| 399 | cloud.header.frame_id = frame_id_; |
| 400 | cloud2.width = cloud2.points.size(); |
| 401 | cloud2.height = 1; |
| 402 | cloud2.is_dense = true; |
| 403 | cloud2.header.frame_id = frame_id_; |
| 404 | sensor_msgs::PointCloud2 cloud_msg; |
| 405 | |
| 406 | pcl::toROSMsg(cloud, cloud_msg); |
| 407 | map_local_pub_.publish(cloud_msg); |
| 408 | pcl::toROSMsg(cloud2, cloud_msg); |
| 409 | map_local_inflate_pub_.publish(cloud_msg); |
| 410 | } |
nothing calls this directly
no test coverage detected