diff --git a/doc/“森警” 系统实践汇报 .pptx b/doc/“森警” 系统实践汇报_last.pptx similarity index 74% rename from doc/“森警” 系统实践汇报 .pptx rename to doc/“森警” 系统实践汇报_last.pptx index 141e4f9..a25c293 100644 Binary files a/doc/“森警” 系统实践汇报 .pptx and b/doc/“森警” 系统实践汇报_last.pptx differ diff --git a/src/.vs/VSWorkspaceState.json b/src/.vs/VSWorkspaceState.json index a5de989..c293885 100644 --- a/src/.vs/VSWorkspaceState.json +++ b/src/.vs/VSWorkspaceState.json @@ -1,7 +1,8 @@ { "ExpandedNodes": [ - "" + "", + "\\避障" ], - "SelectedNode": "\\避障", + "SelectedNode": "\\避障\\occupy_map.cpp", "PreviewInSolutionExplorer": false } \ No newline at end of file diff --git a/src/.vs/slnx.sqlite b/src/.vs/slnx.sqlite index cc3728a..177a2ac 100644 Binary files a/src/.vs/slnx.sqlite and b/src/.vs/slnx.sqlite differ diff --git a/src/.vs/src/FileContentIndex/9d086590-a967-4ccf-a26a-de498501da01.vsidx b/src/.vs/src/FileContentIndex/9d086590-a967-4ccf-a26a-de498501da01.vsidx new file mode 100644 index 0000000..1346048 Binary files /dev/null and b/src/.vs/src/FileContentIndex/9d086590-a967-4ccf-a26a-de498501da01.vsidx differ diff --git a/src/.vs/src/FileContentIndex/ba891a57-ebfa-4f08-9052-ed15be3b34aa.vsidx b/src/.vs/src/FileContentIndex/ba891a57-ebfa-4f08-9052-ed15be3b34aa.vsidx deleted file mode 100644 index 49b3bbc..0000000 Binary files a/src/.vs/src/FileContentIndex/ba891a57-ebfa-4f08-9052-ed15be3b34aa.vsidx and /dev/null differ diff --git a/src/.vs/src/FileContentIndex/be467675-4dfa-446d-85e5-c379a3101be7.vsidx b/src/.vs/src/FileContentIndex/be467675-4dfa-446d-85e5-c379a3101be7.vsidx deleted file mode 100644 index 23f8c77..0000000 Binary files a/src/.vs/src/FileContentIndex/be467675-4dfa-446d-85e5-c379a3101be7.vsidx and /dev/null differ diff --git a/src/.vs/src/FileContentIndex/cec68d3e-9204-4317-8e1b-2bd5cb448612.vsidx b/src/.vs/src/FileContentIndex/cec68d3e-9204-4317-8e1b-2bd5cb448612.vsidx deleted file mode 100644 index e9020a0..0000000 Binary files a/src/.vs/src/FileContentIndex/cec68d3e-9204-4317-8e1b-2bd5cb448612.vsidx and /dev/null differ diff --git a/src/.vs/src/FileContentIndex/e2988654-3565-47c3-bea3-2237bc501021.vsidx b/src/.vs/src/FileContentIndex/e2988654-3565-47c3-bea3-2237bc501021.vsidx new file mode 100644 index 0000000..53e6f6f Binary files /dev/null and b/src/.vs/src/FileContentIndex/e2988654-3565-47c3-bea3-2237bc501021.vsidx differ diff --git a/src/.vs/src/FileContentIndex/16cf715e-57d1-474c-8545-ea9501051399.vsidx b/src/.vs/src/FileContentIndex/e58f3f5a-637f-4aa9-bebf-f7235b1d907a.vsidx similarity index 61% rename from src/.vs/src/FileContentIndex/16cf715e-57d1-474c-8545-ea9501051399.vsidx rename to src/.vs/src/FileContentIndex/e58f3f5a-637f-4aa9-bebf-f7235b1d907a.vsidx index 1bf79ca..3987485 100644 Binary files a/src/.vs/src/FileContentIndex/16cf715e-57d1-474c-8545-ea9501051399.vsidx and b/src/.vs/src/FileContentIndex/e58f3f5a-637f-4aa9-bebf-f7235b1d907a.vsidx differ diff --git a/src/.vs/src/FileContentIndex/f309b6b6-a700-4392-8488-dbf0dc325f27.vsidx b/src/.vs/src/FileContentIndex/f309b6b6-a700-4392-8488-dbf0dc325f27.vsidx new file mode 100644 index 0000000..e875a83 Binary files /dev/null and b/src/.vs/src/FileContentIndex/f309b6b6-a700-4392-8488-dbf0dc325f27.vsidx differ diff --git a/src/.vs/src/v17/.suo b/src/.vs/src/v17/.suo index 52f1a4e..102c300 100644 Binary files a/src/.vs/src/v17/.suo and b/src/.vs/src/v17/.suo differ diff --git a/src/.vs/src/v17/Browse.VC.db b/src/.vs/src/v17/Browse.VC.db index 11b145c..ac2f073 100644 Binary files a/src/.vs/src/v17/Browse.VC.db and b/src/.vs/src/v17/Browse.VC.db differ diff --git a/src/避障/occupy_map.cpp b/src/避障/occupy_map.cpp index fc184df..74583b0 100644 --- a/src/避障/occupy_map.cpp +++ b/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 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]点会被判断为不在地图里 // 同时 对于2D情况,超出飞行高度的数据也会认为不在地图内部 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; }