update

src/.vs/VSWorkspaceState.json

@@ -1,7 +1,8 @@
 {
   "ExpandedNodes": [
-    ""
+    "",
+    "\\避障"
   ],
-  "SelectedNode": "\\避障",
+  "SelectedNode": "\\避障\\occupy_map.cpp",
   "PreviewInSolutionExplorer": false
 }

src/避障/occupy_map.cpp

@@ -81,7 +81,7 @@ void Occupy_map::inflate_point_cloud(void)
     //开始时间
     ros::Time time_start = ros::Time::now();
 
-    // 转化为PCL的格式进行处理
+    // 转化为PCL处理
     pcl::PointCloud<pcl::PointXYZ> latest_global_cloud_;
     pcl::fromROSMsg(*global_env_, latest_global_cloud_);
     //printf("time 1 take %f [s].\n",   (ros::Time::now()-time_start).toSec());
@@ -171,8 +171,7 @@ void Occupy_map::setOccupancy(Eigen::Vector3d pos, int occ)
     posToIndex(pos, id);
 
     // 设置为占据/不占据 索引是如何索引的？ [三维地图 变 二维数组]
-    // 假设10*10*10米，分辨率1米，buffer大小为 1000 （即每一个占格都对应一个buffer索引）
-    // [0.1,0.1,0.1] 对应索引为[0,0,0]， buffer索引为 0  
+    // 假设10*10*10米，分辨率1米，buffer大小为 1000 （即每一个占格都对应一个buffer索引） 
     // [9.9,9.9,9.9] 对应索引为[9,9,9]， buffer索引为 900+90+9 = 999
     occupancy_buffer_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)] = occ;
 }
@@ -180,7 +179,6 @@ void Occupy_map::setOccupancy(Eigen::Vector3d pos, int occ)
 bool Occupy_map::isInMap(Eigen::Vector3d pos) 
 {
     // min_range就是原点，max_range就是原点+地图尺寸
-    // 比如设置0,0,0为原点，[0,0,0]点会被判断为不在地图里
     //　同时　对于２Ｄ情况，超出飞行高度的数据也会认为不在地图内部
     if (pos(0) < min_range_(0) + 1e-4 || pos(1) < min_range_(1) + 1e-4 || pos(2) < min_range_(2) + 1e-4) 
     {
@@ -219,7 +217,6 @@ bool Occupy_map::check_safety(Eigen::Vector3d& pos, double check_distance)
                 id_occ(2) = id(2)+iz;
                 indexToPos(id_occ, pos_occ);
                 if(!isInMap(pos_occ)){
-                    // printf("[check_safety]: current odom is near the boundary of the map\n");
                     // pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "[check_safety]: current odom is near the boundary of the map\n");
                     return 0;
                 }