DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add function computeCost file layer.h

Browse Source
This commit is contained in:
duongtd
2026-07-10 11:27:20 +07:00
parent 2fcd211ccf
commit a2a021c114
10 changed files with 1355 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
plugins/voxel_layer.cpp
View File

@@ -331,8 +331,8 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
// bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
// if (publish_clearing_points)
// {
clearing_endpoints_.points.clear();
clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
// clearing_endpoints_.points.clear();
// clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
// }
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
@@ -412,20 +412,20 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
// if (publish_clearing_points)
// {
robot_geometry_msgs::Point32 point;
point.x = wpx;
point.y = wpy;
point.z = wpz;
clearing_endpoints_.points.push_back(point);
// robot_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 = clearing_observation.cloud_->header.stamp;
clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
// clearing_endpoints_.header.frame_id = global_frame_;
// clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
// clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
// clearing_endpoints_pub_.publish(clearing_endpoints_);
// }