diff --git a/README.md b/README.md index 78e8007..d3d16aa 100644 --- a/README.md +++ b/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 diff --git a/Simulator/src/CMakeLists.txt b/Simulator/src/CMakeLists.txt index 64fd324..9a57d2a 100644 --- a/Simulator/src/CMakeLists.txt +++ b/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") diff --git a/Simulator/src/include/sensor_simulator.h b/Simulator/src/include/sensor_simulator.h index a4f3515..0c1696e 100644 --- a/Simulator/src/include/sensor_simulator.h +++ b/Simulator/src/include/sensor_simulator.h @@ -19,6 +19,7 @@ #include #include #include +#include "maps.hpp" class SensorSimulator { public: @@ -52,32 +53,56 @@ public: std::string depth_topic = config["depth_topic"].as(); std::string lidar_topic = config["lidar_topic"].as(); + // 读取地图参数 float resolution = config["resolution"].as(); int expand_y_times = config["expand_y_times"].as(); int expand_x_times = config["expand_x_times"].as(); - - pcl::PointCloud::Ptr orig_cloud(new pcl::PointCloud()); - 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(); + int seed = config["seed"].as(); + int sizeX = config["x_length"].as(); + int sizeY = config["y_length"].as(); + int sizeZ = config["z_length"].as(); + int type = config["maze_type"].as(); + double scale = 1 / resolution; + sizeX = sizeX * scale; + sizeY = sizeY * scale; + sizeZ = sizeZ * scale; cloud = pcl::PointCloud::Ptr(new pcl::PointCloud()); - *cloud = *orig_cloud; - for (int i = 0; i < expand_x_times; ++i) { - expand_cloud(cloud, 0); + // 生成随机地图 + if (use_random_map) { + 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) { - expand_cloud(cloud, 1); + else { + pcl::PointCloud::Ptr orig_cloud(new pcl::PointCloud()); + 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::Ptr( diff --git a/Simulator/src/readme.md b/Simulator/src/readme.md index 13a179a..6e642ac 100644 --- a/Simulator/src/readme.md +++ b/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 diff --git a/Simulator/src/src/sensor_simulator.cpp b/Simulator/src/src/sensor_simulator.cpp index d6d9ebd..4c48e30 100644 --- a/Simulator/src/src/sensor_simulator.cpp +++ b/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); }