update CPU-version Simulator

README.md

@@ -69,7 +69,7 @@ conda deactivate
 cd Controller
 catkin_make
 ```
-Build the environment and sensors simulator
+Build the environment and sensors simulator (if CUDA errors occur, please refer to [Simulator_Introduction](Simulator/src/readme.md))
 ```
 conda deactivate
 cd Simulator

Simulator/src/CMakeLists.txt

@@ -30,6 +30,9 @@ if("${ARCH_FLAGS}" STREQUAL "")
     "  set(ARCH_FLAGS \"-gencode arch=compute_86,code=sm_86\")"
   )
 endif()
+# If automatic detection fails, you can delete above (cuda_select_nvcc_arch_flags(ARCH_FLAGS) ... endif()) and manually set the architecture flags here
+# Examples (NVIDIA 5060 GPU):
+# set(ARCH_FLAGS "-gencode arch=compute_120,code=sm_120")
 message(WARNING "CUDA Architecture: ${ARCH_FLAGS}")
 set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} ${ARCH_FLAGS}")
 
@@ -53,7 +56,7 @@ include_directories(
         ${OpenCV_INCLUDE_DIRS}
 )
 
-add_executable(sensor_simulator src/test_simulator.cpp src/sensor_simulator.cpp)
+add_executable(sensor_simulator src/test_simulator.cpp src/sensor_simulator.cpp src/maps.cpp src/perlinnoise.cpp)
 target_link_libraries(sensor_simulator ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} OpenMP::OpenMP_CXX yaml-cpp)
 target_compile_definitions(sensor_simulator PRIVATE CONFIG_FILE_PATH="${CMAKE_SOURCE_DIR}/config/config.yaml")
 

Simulator/src/include/sensor_simulator.h

@@ -19,6 +19,7 @@
 #include <chrono>
 #include <omp.h>
 #include <yaml-cpp/yaml.h>
+#include "maps.hpp"
 
 class SensorSimulator {
 public:
@@ -52,32 +53,56 @@ public:
         std::string depth_topic = config["depth_topic"].as<std::string>();
         std::string lidar_topic = config["lidar_topic"].as<std::string>();
 
+        // 读取地图参数
         float resolution = config["resolution"].as<float>();
         int expand_y_times = config["expand_y_times"].as<int>();
         int expand_x_times = config["expand_x_times"].as<int>();
-
-        pcl::PointCloud<pcl::PointXYZ>::Ptr orig_cloud(new pcl::PointCloud<pcl::PointXYZ>());
-        printf("1.Reading Point Cloud... \n");
-
-        // if (pcl::io::loadPCDFile("/home/lu/用完删除/test/map.pcd", *cloud) == -1) {
-        //     PCL_ERROR("Couldn't read PLY file \n");
-        //     return;
-        // }
-
-        if (pcl::io::loadPLYFile(ply_file, *orig_cloud) == -1) {
-            PCL_ERROR("Couldn't read PLY file \n");
-            return;
-        }
         
-        printf("2.Processing... \n");
+        bool use_random_map = config["random_map"].as<bool>();
+        int seed = config["seed"].as<int>();
+        int sizeX = config["x_length"].as<int>();
+        int sizeY = config["y_length"].as<int>();
+        int sizeZ = config["z_length"].as<int>();
+        int type = config["maze_type"].as<int>();
+        double scale = 1 / resolution;
+        sizeX = sizeX * scale;
+        sizeY = sizeY * scale;
+        sizeZ = sizeZ * scale;
 
         cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
-        *cloud = *orig_cloud;
+        // 生成随机地图
-        for (int i = 0; i < expand_x_times; ++i) {
+        if (use_random_map) {
-            expand_cloud(cloud, 0);
+            printf("1.Generate Random Map... \n");
+            mocka::Maps::BasicInfo info;
+            info.sizeX      = sizeX;
+            info.sizeY      = sizeY;
+            info.sizeZ      = sizeZ;
+            info.seed       = seed;
+            info.scale      = scale;
+            info.cloud      = cloud;
+
+            mocka::Maps map;
+            map.setParam(config);
+            map.setInfo(info);
+            map.generate(type);
+            printf("2.Mapping... \n");
         }
-        for (int i = 0; i < expand_y_times; ++i) {
+        else {
-            expand_cloud(cloud, 1);
+            pcl::PointCloud<pcl::PointXYZ>::Ptr orig_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+            printf("1.Reading Point Cloud... \n");
+            if (pcl::io::loadPLYFile(ply_file, *orig_cloud) == -1) {
+                PCL_ERROR("Couldn't read PLY file \n");
+                return;
+            }
+            
+            printf("2.Processing... \n");
+            *cloud = *orig_cloud;
+            for (int i = 0; i < expand_x_times; ++i) {
+                expand_cloud(cloud, 0);
+            }
+            for (int i = 0; i < expand_y_times; ++i) {
+                expand_cloud(cloud, 1);
+            }
         }
 
         octree = pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::Ptr(

Simulator/src/readme.md

@@ -11,14 +11,25 @@ sudo apt-get install libyaml-cpp-dev
 ```angular2html
 catkin build
 ```
+注：在CmakeList中采用`cuda_select_nvcc_arch_flags`自动检测，如果遇到编译错误，你需要手动设置CUDA架构，请将CmakeList 25行起：
+```
+cuda_select_nvcc_arch_flags(ARCH_FLAGS)
+...
+endif()
+```
+替换为(5060 GPU是120, 需要根据自己设备设置)：
+```
+set(ARCH_FLAGS "-gencode arch=compute_120,code=sm_120")
+```
 
 ### 3 运行
 ```angular2html
 source devel/setup.bash
-# CPU版本 (已弃用)
-rosrun sensor_simulator sensor_simulator
 # GPU版本 (推荐, RTX 3060 深度输出 > 1000fps)
 rosrun sensor_simulator sensor_simulator_cuda
+
+# CPU版本 (资源占用高，仅供GPU编译失败无法解决时测试)
+rosrun sensor_simulator sensor_simulator
 ```
 
 传感器参数以及点云环境修改见[config](config/config.yaml)，重要参数说明:
@@ -29,14 +40,10 @@ depth_topic: "/depth_image"
 lidar_topic: "/lidar_points"
 # 使用预先构建的点云地图还是随机地图
 random_map: true
-# 点云地图文件
+# 随机地图配置
-ply_file:
+maze_type: 5   # 1: 溶洞 2: 柱子 3:迷宫 5:森林 6:房间
-# 随机地图配置  
-maze_type: 5   # 1: 溶洞 2: 柱子 3:迷宫 5:森林(也需设置树的点云文件) 6:房间
 ```
 
-如果使用预先构建的点云地图，可下载我们收集的一个树林的示例: [谷歌云盘](https://drive.google.com/file/d/1WT3vh0m7Gjn0mt4ri-D35mVDgRCT0mNc/view?usp=sharing)
-
 ### 4 仿真位置发布与简单可视化（可选）
 ```angular2html
 cd src/sensor_simulator

Simulator/src/src/sensor_simulator.cpp

@@ -47,10 +47,6 @@ cv::Mat SensorSimulator::renderDepthImage(){
         }
     }
 
-    // 再做一遍插值更接近真实
-    // cv::Mat resized_depth_image;
-    // cv::resize(depth_image, resized_depth_image, cv::Size(48, 27));
-
     return depth_image;
 }
 
@@ -157,7 +153,7 @@ void SensorSimulator::timerLidarCallback(const ros::TimerEvent&) {
     sensor_msgs::PointCloud2 output;
     pcl::toROSMsg(lidar_points, output);
     output.header.stamp = ros::Time::now();
-    output.header.frame_id = "world";
+    output.header.frame_id = "odom";
 
     point_cloud_pub_.publish(output);
 }