diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..a79df19 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 2.8.3) +project(map_conversion) + +SET(CMAKE_BUILD_TYPE "Release") +SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall") +add_compile_options(-std=c++11) +add_definitions(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + pcl_ros + geometry_msgs + tf + eigen_conversions +) + +set(ALL_TARGET_LIBRARIES "") + +# find_package(gflags REQUIRED) + +include(cmake/glog.cmake) +include(cmake/PCL.cmake) +include(cmake/eigen.cmake) +include(cmake/yaml.cmake) + +include_directories( include +${catkin_INCLUDE_DIRS} +) + +include(cmake/global_defination.cmake) +catkin_package() + +file(GLOB_RECURSE ALL_SRCS "*.cpp") +file(GLOB_RECURSE NODE_SRCS "src/*_node.cpp") +list(REMOVE_ITEM ALL_SRCS ${NODE_SRCS}) + +add_executable(local_environment_node src/app/local_environment_node.cpp ${ALL_SRCS}) +add_dependencies(local_environment_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(local_environment_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES} gflags yaml-cpp) + +add_executable(global_submap_node src/app/global_submap_node.cpp ${ALL_SRCS}) +add_dependencies(global_submap_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(global_submap_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES} gflags yaml-cpp) + +install(TARGETS + local_environment_node + global_submap_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY + include/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other directories for installation: +install(DIRECTORY + launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + FILES_MATCHING PATTERN "*.launch" +) +install(DIRECTORY + config/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config + FILES_MATCHING PATTERN "*.yaml" +) +install(DIRECTORY + rviz/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz + FILES_MATCHING PATTERN "*.rviz" +) + diff --git a/cmake/PCL.cmake b/cmake/PCL.cmake new file mode 100644 index 0000000..41468a9 --- /dev/null +++ b/cmake/PCL.cmake @@ -0,0 +1,5 @@ +find_package(PCL 1.7 REQUIRED) +list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") + +include_directories(${PCL_INCLUDE_DIRS}) +list(APPEND ALL_TARGET_LIBRARIES ${PCL_LIBRARIES}) \ No newline at end of file diff --git a/cmake/eigen.cmake b/cmake/eigen.cmake new file mode 100644 index 0000000..11534c1 --- /dev/null +++ b/cmake/eigen.cmake @@ -0,0 +1,2 @@ +set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) +include_directories( ${Eigen3_INCLUDE_DIRS} ) \ No newline at end of file diff --git a/cmake/global_defination.cmake b/cmake/global_defination.cmake new file mode 100644 index 0000000..d6851d6 --- /dev/null +++ b/cmake/global_defination.cmake @@ -0,0 +1,5 @@ +set(WORK_SPACE_PATH ${PROJECT_SOURCE_DIR}) +configure_file ( + ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/global_defination/global_defination.h.in + ${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/global_defination/global_defination.h) +include_directories(${PROJECT_BINARY_DIR}/include) \ No newline at end of file diff --git a/cmake/glog.cmake b/cmake/glog.cmake new file mode 100644 index 0000000..1aba4db --- /dev/null +++ b/cmake/glog.cmake @@ -0,0 +1,40 @@ +include(FindPackageHandleStandardArgs) + +set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") + +if(WIN32) + find_path(GLOG_INCLUDE_DIR glog/logging.h + PATHS ${GLOG_ROOT_DIR}/src/windows) +else() + find_path(GLOG_INCLUDE_DIR glog/logging.h + PATHS ${GLOG_ROOT_DIR}) +endif() + +if(MSVC) + find_library(GLOG_LIBRARY_RELEASE libglog_static + PATHS ${GLOG_ROOT_DIR} + PATH_SUFFIXES Release) + + find_library(GLOG_LIBRARY_DEBUG libglog_static + PATHS ${GLOG_ROOT_DIR} + PATH_SUFFIXES Debug) + + set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) +else() + find_library(GLOG_LIBRARY glog + PATHS ${GLOG_ROOT_DIR} + PATH_SUFFIXES lib lib64) +endif() + +find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIR GLOG_LIBRARY) + +if(GLOG_FOUND) + set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) + set(GLOG_LIBRARIES ${GLOG_LIBRARY}) + message(STATUS "Found glog (include: ${GLOG_INCLUDE_DIR}, library: ${GLOG_LIBRARY})") + mark_as_advanced(GLOG_ROOT_DIR GLOG_LIBRARY_RELEASE GLOG_LIBRARY_DEBUG + GLOG_LIBRARY GLOG_INCLUDE_DIR) +endif() + +include_directories(${GLOG_INCLUDE_DIRS}) +list(APPEND ALL_TARGET_LIBRARIES ${GLOG_LIBRARIES}) \ No newline at end of file diff --git a/cmake/yaml.cmake b/cmake/yaml.cmake new file mode 100644 index 0000000..0cc81de --- /dev/null +++ b/cmake/yaml.cmake @@ -0,0 +1,2 @@ +find_package(yaml-cpp REQUIRED) +include_directories( ${YAML_CPP_INCLUDE_DIR}) \ No newline at end of file diff --git a/config/params.yaml b/config/params.yaml new file mode 100644 index 0000000..8215a0d --- /dev/null +++ b/config/params.yaml @@ -0,0 +1,8 @@ +%YAML:1.0 + +Global_file_directory: "/home/qjs/cloudGlobal.pcd" #全局地图文件位置 +local_cloud_frame: "/velodyne_cloud_registered" #局部雷达原始帧话题 (/laser_cloud_map) +global_z: "0" #这两个参数是调整栅格地图的Z轴直通滤波的范围 +local_z: "0.27" + + diff --git a/config/rviz_test.rviz b/config/rviz_test.rviz new file mode 100644 index 0000000..15f7ff7 --- /dev/null +++ b/config/rviz_test.rviz @@ -0,0 +1,880 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /odometry1/odomPath1 + - /odometry1/PointCloud21 + - /mapping1/allMapCloud1 + - /PointCloud22 + - /PointCloud23 + Splitter Ratio: 0.5 + Tree Height: 811 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 160 + Reference Frame: + Value: false + - Class: rviz/Axes + Enabled: false + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Value: false + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 170; 0 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: gtPathlc + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /path_gt + Unreliable: false + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 170; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: gtPathLidar + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /path_gt_lidar + Unreliable: false + Value: true + Enabled: false + Name: GT + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.10000000149011612 + Name: odomPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /laser_odom_path + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /velodyne_cloud_2 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: false + Name: odometry + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: mappingPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /aft_mapped_path + Unreliable: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: allMapCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /laser_cloud_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 1 + Size (m): 0.05000000074505806 + Style: Squares + Topic: /laser_cloud_surround + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /velodyne_cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 255; 0; 0 + Name: corner + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: /mapping_corner + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: surf + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: /mapping_surf + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: used_corner + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.5 + Style: Flat Squares + Topic: /mapping_used_corner + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 0; 255; 0 + Min Color: 0; 0; 0 + Name: used_surf + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.5 + Style: Flat Squares + Topic: /mapping_used_surf + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: map_corner + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /mapping_map_corner + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: map_surf + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /mapping_map_surf + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: leftcameraPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /lc_path + Unreliable: false + Value: false + Enabled: false + Name: mapping + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: sharp + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /laser_cloud_sharp + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: flat + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /laser_cloud_flat + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: raw_data + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /velodyne_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: scan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: /laser_scanid_63 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: removPoints + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: /laser_remove_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: false + Name: scan + - Class: rviz/Image + Enabled: true + Image Topic: /hikrobot_camera/rgb + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + aft_mapped: + Value: true + camera_init: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + camera_init: + aft_mapped: + {} + Update Interval: 0 + Value: true + - Class: octomap_rviz_plugin/OccupancyGrid + Enabled: false + Max. Height Display: 3.4028234663852886e+38 + Max. Octree Depth: 16 + Min. Height Display: -3.4028234663852886e+38 + Name: OccupancyGrid + Octomap Topic: /octomap_full + Queue Size: 5 + Value: false + Voxel Alpha: 1 + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + - Alpha: 0.699999988079071 + Class: octomap_rviz_plugin/OccupancyMap + Color Scheme: map + Draw Behind: false + Enabled: false + Max. Octree Depth: 16 + Name: OccupancyMap + Octomap Binary Topic: /octomap_binary + Unreliable: false + Use Timestamp: false + Value: false + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Map + Topic: /projected_map + Unreliable: false + Use Timestamp: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 13.387751579284668 + Min Value: 1.7561100721359253 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /pcl_output + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /grid_map_realtime + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 6.525000095367432 + Min Value: 1.5750000476837158 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 2 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Boxes + Topic: /inflation_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 1 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /global_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /grid_map_global + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 4.125 + Min Value: 0.07500000298023224 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Boxes + Topic: /global_inflation_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: camera_init + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 131.0474853515625 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -25.12978744506836 + Y: 34.508941650390625 + Z: -2.3098056316375732 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: body + Value: XYOrbit (rviz) + Yaw: 3.254099130630493 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1385 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004cbfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003b6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000003f90000010f0000001600ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000082d0000003efc0100000002fb0000000800540069006d006501000000000000082d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006bd000004cb00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 2093 + X: 67 + Y: 27 diff --git a/include/map_conversion/global_defination/global_defination.h.in b/include/map_conversion/global_defination/global_defination.h.in new file mode 100644 index 0000000..750e8a4 --- /dev/null +++ b/include/map_conversion/global_defination/global_defination.h.in @@ -0,0 +1,14 @@ +/* + * @Description: + * @Author: Ren Qian + * @Date: 2020-02-05 02:27:30 + */ +#ifndef MAP_CONVERSION_GLOBAL_DEFINATION_H_IN_ +#define MAP_CONVERSION_GLOBAL_DEFINATION_H_IN_ + +#include + +namespace map_conversion { +const std::string WORK_SPACE_PATH = "@WORK_SPACE_PATH@"; +} +#endif \ No newline at end of file diff --git a/include/map_conversion/pointcloud_process/costmap_calculator.hpp b/include/map_conversion/pointcloud_process/costmap_calculator.hpp new file mode 100644 index 0000000..077ddf6 --- /dev/null +++ b/include/map_conversion/pointcloud_process/costmap_calculator.hpp @@ -0,0 +1,23 @@ +/*该类的功能主要是代价地图的计算 + edited by goldqiu -2022-04-9 +*/ +#ifndef MAP_CONVERSION_COSTMAP_CALCULATOR_HPP_ +#define MAP_CONVERSION_COSTMAP_CALCULATOR_HPP_ +#include "map_conversion/ros_topic_interface/cloud_data.hpp" +#include +#include "map_conversion/utility.hpp" + +namespace map_conversion { +class CostmapCalculator { + public: + CostmapCalculator(YAML::Node& config_node); + CostmapCalculator() = default; + + private: + + private: + +}; +} + +#endif \ No newline at end of file diff --git a/include/map_conversion/pointcloud_process/pointcloud_2d_process.hpp b/include/map_conversion/pointcloud_process/pointcloud_2d_process.hpp new file mode 100644 index 0000000..5d05091 --- /dev/null +++ b/include/map_conversion/pointcloud_process/pointcloud_2d_process.hpp @@ -0,0 +1,48 @@ +/*该类的功能主要是点云的二维栅格处理 + edited by goldqiu -2022-04-9 +*/ +#ifndef MAP_CONVERSION_POINTCLOUD_2D_PROCESS_HPP_ +#define MAP_CONVERSION_POINTCLOUD_2D_PROCESS_HPP_ +#include "map_conversion/ros_topic_interface/cloud_data.hpp" +#include +#include "map_conversion/utility.hpp" + +namespace map_conversion { +class Pointcloud2dProcess { + public: + Pointcloud2dProcess(YAML::Node& config_node); + Pointcloud2dProcess() = default; + + void find_Z_value(CloudData& cloud_data); //计算点云中Z轴的最大和最小值 + int global_map_init(void); //初始化全局地图 + //直通滤波器 + void PassThroughFilter(CloudData& pcd_cloud,CloudData& cloud_after_PassThrough, const bool &flag_in); + //将三维点云转换为三维栅格 + void Pointcloud_to_2d_grid(const CloudData& pcd_cloud, nav_msgs::OccupancyGrid& msg); + + double max_z; + double min_z; + std::string Global_map_file; + std::string local_cloud_topic; + + CloudData global_map_data; + CloudData global_map_after_filter; + + + double grid_x = 0.1; + double grid_y = 0.1; + double grid_z = 0.1; + + double map_resolution = 0.05; + double thre_radius = 0.5; + double global_z_adjust; + double local_z_adjust; + + private: + + private: + +}; +} + +#endif \ No newline at end of file diff --git a/include/map_conversion/pointcloud_process/pointcloud_3d_process.hpp b/include/map_conversion/pointcloud_process/pointcloud_3d_process.hpp new file mode 100644 index 0000000..67cc703 --- /dev/null +++ b/include/map_conversion/pointcloud_process/pointcloud_3d_process.hpp @@ -0,0 +1,48 @@ +/*该类的功能主要是点云的三维栅格处理 + edited by goldqiu -2022-04-9 +*/ +#ifndef MAP_CONVERSION_POINTCLOUD_3D_PROCESS_HPP_ +#define MAP_CONVERSION_POINTCLOUD_3D_PROCESS_HPP_ +#include "map_conversion/ros_topic_interface/cloud_data.hpp" +#include +#include "map_conversion/utility.hpp" + +namespace map_conversion { +class Pointcloud3dProcess { + public: + Pointcloud3dProcess(YAML::Node& config_node); + Pointcloud3dProcess() = default; + + //以下是三维栅格化的成员方法 + Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d pt); + bool setObs(const double coord_x, const double coord_y, const double coord_z); + Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i index); + void initGridMap(double _resolution, Eigen::Vector3d global_xyz_l, Eigen::Vector3d global_xyz_u); + void pointcloud_to_3dGridMap(CloudData& cloud_data,CloudData& cloud_inf); + + double _resolution = 0.15 ; + double _cloud_margin = 0.3; + double _inv_resolution; + double _x_size = 120.0; //要生成的地图大小 20*20*3 单位为m + double _y_size = 120.0; + double _z_size = 7.0 ; + + double gl_xl, gl_yl, gl_zl; + double gl_xu, gl_yu, gl_zu; + int GLX_SIZE; + int GLY_SIZE; + int GLZ_SIZE; + int GLYZ_SIZE; + int GLXYZ_SIZE; + int tmp_id_x, tmp_id_y, tmp_id_z; + uint8_t * data; + Eigen::Vector3d _map_lower,_map_upper; + + private: + + private: + +}; +} + +#endif \ No newline at end of file diff --git a/include/map_conversion/ros_topic_interface/cloud_data.hpp b/include/map_conversion/ros_topic_interface/cloud_data.hpp new file mode 100644 index 0000000..c035fd6 --- /dev/null +++ b/include/map_conversion/ros_topic_interface/cloud_data.hpp @@ -0,0 +1,26 @@ + +#ifndef MAP_CONVERSION_SENSOR_DATA_CLOUD_DATA_HPP_ +#define MAP_CONVERSION_SENSOR_DATA_CLOUD_DATA_HPP_ + +#include +#include + +namespace map_conversion { +class CloudData { + public: + using POINT = pcl::PointXYZ; + using CLOUD = pcl::PointCloud; + using CLOUD_PTR = CLOUD::Ptr; + + public: + CloudData() + :cloud_ptr(new CLOUD()) { + } + + public: + double time = 0.0; + CLOUD_PTR cloud_ptr; +}; +} + +#endif \ No newline at end of file diff --git a/include/map_conversion/ros_topic_interface/cloud_publisher.hpp b/include/map_conversion/ros_topic_interface/cloud_publisher.hpp new file mode 100644 index 0000000..d4fa244 --- /dev/null +++ b/include/map_conversion/ros_topic_interface/cloud_publisher.hpp @@ -0,0 +1,33 @@ +#ifndef MAP_CONVERSION_CLOUD_PUBLISHER_HPP_ +#define MAP_CONVERSION_CLOUD_PUBLISHER_HPP_ + +#include +#include +#include +#include +#include "map_conversion/ros_topic_interface/cloud_data.hpp" + +namespace map_conversion { +class CloudPublisher { + public: + CloudPublisher(ros::NodeHandle& nh, + std::string topic_name, + std::string frame_id, + size_t buff_size); + CloudPublisher() = default; + + void Publish(CloudData::CLOUD_PTR& cloud_ptr_input, double time); + void Publish(CloudData::CLOUD_PTR& cloud_ptr_input); + + bool HasSubscribers(); + + private: + void PublishData(CloudData::CLOUD_PTR& cloud_ptr_input, ros::Time time); + + private: + ros::NodeHandle nh_; + ros::Publisher publisher_; + std::string frame_id_; +}; +} +#endif \ No newline at end of file diff --git a/include/map_conversion/ros_topic_interface/cloud_subscriber.hpp b/include/map_conversion/ros_topic_interface/cloud_subscriber.hpp new file mode 100644 index 0000000..9ef4b02 --- /dev/null +++ b/include/map_conversion/ros_topic_interface/cloud_subscriber.hpp @@ -0,0 +1,35 @@ +#ifndef MAP_CONVERSION_CLOUD_SUBSCRIBER_HPP_ +#define MAP_CONVERSION_CLOUD_SUBSCRIBER_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "map_conversion/ros_topic_interface/cloud_data.hpp" + +namespace map_conversion { +class CloudSubscriber { + public: + CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); + CloudSubscriber() = default; + void ParseData(std::deque& deque_cloud_data); + + private: + void msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr); + + private: + ros::NodeHandle nh_; + ros::Subscriber subscriber_; + std::deque new_cloud_data_; + + std::mutex buff_mutex_; +}; +} + +#endif \ No newline at end of file diff --git a/include/map_conversion/tic_toc.h b/include/map_conversion/tic_toc.h new file mode 100644 index 0000000..8885273 --- /dev/null +++ b/include/map_conversion/tic_toc.h @@ -0,0 +1,29 @@ +#pragma once + +#include +#include +#include + +class TicToc +{ + public: + TicToc() + { + tic(); + } + + void tic() + { + start = std::chrono::system_clock::now(); + } + + double toc() + { + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + return elapsed_seconds.count() * 1000; + } + + private: + std::chrono::time_point start, end; +}; diff --git a/include/map_conversion/utility.hpp b/include/map_conversion/utility.hpp new file mode 100644 index 0000000..cb1ccf4 --- /dev/null +++ b/include/map_conversion/utility.hpp @@ -0,0 +1,74 @@ +// +// Created by qjs on 21-5-11. +// + +#ifndef MAP_CONVERSION_UTILITY_H +#define MAP_CONVERSION_UTILITY_H + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include //直通滤波器头文件 +#include //体素滤波器头文件 +#include //统计滤波器头文件 +#include //条件滤波器头文件 +#include //半径滤波器头文件 +#include +#include +#include +#include + +#include +#include "string.h" +#include +#include +#include +#include +#include +#include +#include "glog/logging.h" +#include "map_conversion/global_defination/global_defination.h" +#include "map_conversion/ros_topic_interface/cloud_subscriber.hpp" +#include "map_conversion/pointcloud_process/pointcloud_2d_process.hpp" +#include "map_conversion/pointcloud_process/pointcloud_3d_process.hpp" +#include "map_conversion/ros_topic_interface/cloud_publisher.hpp" + + +using namespace map_conversion; +using namespace Eigen; +using namespace std; + +struct GridNode +{ + int id; // 1--> open set, -1 --> closed set + Eigen::Vector3d coord; + Eigen::Vector3i index; + + uint8_t * occupancy; + + GridNode(Eigen::Vector3i _index) + { + id = 0; + index = _index; + } + + GridNode(Eigen::Vector3i _index, Eigen::Vector3d _coord) + { + id = 0; + index = _index; + coord = _coord; + } + + GridNode(){}; + + ~GridNode(){}; +}; + +#endif //OFFLINE_FUSION_UTILITY_H diff --git a/launch/slam_to_planning.launch b/launch/slam_to_planning.launch new file mode 100644 index 0000000..1fb40f2 --- /dev/null +++ b/launch/slam_to_planning.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..241d16b --- /dev/null +++ b/package.xml @@ -0,0 +1,67 @@ + + + map_conversion + 0.0.0 + The map_conversion package + + + + + qjs + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + roscpp + roscpp + std_msgs + std_msgs + std_msgs + nav_msgs + nav_msgs + nav_msgs + + + + + + + diff --git a/src/app/global_submap_node.cpp b/src/app/global_submap_node.cpp new file mode 100644 index 0000000..24f5cc6 --- /dev/null +++ b/src/app/global_submap_node.cpp @@ -0,0 +1,51 @@ +/*该类的功能主要是导入全局地图，进行1hz的全局地图三种格式的发布，包括三维点云、 + 三维栅格、二维栅格。 + edited by goldqiu -2022-04-9 +*/ + +#include "map_conversion/utility.hpp" +#include "map_conversion/tic_toc.h" + +int main(int argc, char** argv) +{ + google::InitGoogleLogging(argv[0]); + FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; + FLAGS_alsologtostderr = 1; + + ros::init(argc, argv, "global_submap_node"); + ros::NodeHandle nh; //创建ros句柄 + //创建读取参数的对象 + YAML::Node config = YAML::LoadFile(WORK_SPACE_PATH+"/config/params.yaml"); + + //创建点云处理对象，传入参数为前面创建的读取参数的对象 + Pointcloud2dProcess *pointcloud_2d_process_ptr = new Pointcloud2dProcess(config); + Pointcloud3dProcess *pointcloud_3d_process_ptr = new Pointcloud3dProcess(config); + + //创建ros发布和接受话题对象 + std::shared_ptr global_cloud_pub_ptr = std::make_shared(nh, "global_cloud", "/camera_init", 100); + std::shared_ptr global_3d_grid_pub_ptr = std::make_shared(nh, "global_inflation_map", "/camera_init", 100); + ros::Publisher global_grid_map_pub = nh.advertise("/grid_map_global", 100); + + nav_msgs::OccupancyGrid global_grid_map_msg; + TicToc t_map; //创建计时对象 + + //导入全局地图并初始化，其中转换为2d和3d栅格格式 + pointcloud_2d_process_ptr->global_map_init(); + pointcloud_2d_process_ptr->Pointcloud_to_2d_grid(pointcloud_2d_process_ptr->global_map_after_filter,global_grid_map_msg); + global_grid_map_msg.header.frame_id = "camera_init"; + CloudData grid_cloud_data; + pointcloud_3d_process_ptr->pointcloud_to_3dGridMap(pointcloud_2d_process_ptr->global_map_after_filter,grid_cloud_data); + printf("global map init time %f ms \n", t_map.toc()); //打印初始化需要的时间 + + ros::Rate loop_rate(1.0); //全局地图或者全局子地图的更新频率为1s + while(ros::ok()) + { + ros::spinOnce(); + //这里发布三个话题，分别为原始全局地图、3d栅格、2d栅格 + global_cloud_pub_ptr->Publish(pointcloud_2d_process_ptr->global_map_after_filter.cloud_ptr); + global_grid_map_pub.publish(global_grid_map_msg); + global_3d_grid_pub_ptr->Publish(grid_cloud_data.cloud_ptr); + loop_rate.sleep(); + } + return 0; +} diff --git a/src/app/local_environment_node.cpp b/src/app/local_environment_node.cpp new file mode 100644 index 0000000..5ea955b --- /dev/null +++ b/src/app/local_environment_node.cpp @@ -0,0 +1,66 @@ +/*该类的功能主要是接受实时三维点云帧，进行100hz的三种格式的发布，包括三维点云、 + 三维栅格、二维栅格。 + edited by goldqiu -2022-04-9 +*/ + +#include "map_conversion/utility.hpp" + +int main(int argc, char** argv) +{ + google::InitGoogleLogging(argv[0]); + FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; + FLAGS_alsologtostderr = 1; + + ros::init(argc, argv, "test_frame_node"); + ros::NodeHandle nh; //创建ros句柄 + //创建读取参数的对象 + YAML::Node config = YAML::LoadFile(WORK_SPACE_PATH+"/config/params.yaml"); + + //创建点云处理对象，传入参数为前面创建的读取参数的对象 + Pointcloud2dProcess *pointcloud_2d_process_ptr = new Pointcloud2dProcess(config); + Pointcloud3dProcess *pointcloud_3d_process_ptr = new Pointcloud3dProcess(config); + + //创建ros发布和接受话题对象 + std::shared_ptr cloud_sub_ptr = std::make_shared(nh, pointcloud_2d_process_ptr->local_cloud_topic, 100000); + std::shared_ptr local_cloud_pub_ptr = std::make_shared(nh, "pcl_output", "/camera_init", 100); + std::shared_ptr local_3d_grid_pub_ptr = std::make_shared(nh, "inflation_map", "/camera_init", 100); + + //这里创建发布二维占据栅格地图对象并没有封装为类 + ros::Publisher local_grid_map_pub = nh.advertise("/grid_map_realtime", 100); + + nav_msgs::OccupancyGrid local_grid_map_msg; + std::deque cloud_data_buff; //实时点云buffer + + ros::Rate loop_rate(100.0);//局部地图的更新频率为100hz + while(ros::ok()) + { + ros::spinOnce(); + cloud_sub_ptr->ParseData(cloud_data_buff);//从封装好的点云话题接受类内取数据 + if(cloud_data_buff.size() > 0) + { + + CloudData cloud_data = cloud_data_buff.front(); //存到buffer + + //发布实时三维点云 + pointcloud_2d_process_ptr->find_Z_value(cloud_data); + pointcloud_2d_process_ptr->PassThroughFilter(cloud_data,cloud_data,false); + local_cloud_pub_ptr->Publish(cloud_data.cloud_ptr); + + //发布实时二维栅格 + pointcloud_2d_process_ptr->Pointcloud_to_2d_grid(cloud_data,local_grid_map_msg); + local_grid_map_msg.header.frame_id = "camera_init"; + local_grid_map_pub.publish(local_grid_map_msg); + + //发布实时三维栅格 + CloudData grid_cloud_data; + pointcloud_3d_process_ptr->pointcloud_to_3dGridMap(cloud_data,grid_cloud_data); + local_3d_grid_pub_ptr->Publish(grid_cloud_data.cloud_ptr); + + + cloud_data_buff.pop_front(); + } + loop_rate.sleep(); + } + return 0; +} + diff --git a/src/pointcloud_process/costmap_calculator.cpp b/src/pointcloud_process/costmap_calculator.cpp new file mode 100644 index 0000000..e5a63b4 --- /dev/null +++ b/src/pointcloud_process/costmap_calculator.cpp @@ -0,0 +1,12 @@ +/*该类的功能主要是代价地图的计算 + edited by goldqiu -2022-04-9 +*/ +#include "map_conversion/pointcloud_process/costmap_calculator.hpp" +#include "glog/logging.h" + +namespace map_conversion { +CostmapCalculator::CostmapCalculator(YAML::Node& config_node) { +} + + +} \ No newline at end of file diff --git a/src/pointcloud_process/pointcloud_2d_process.cpp b/src/pointcloud_process/pointcloud_2d_process.cpp new file mode 100644 index 0000000..220f2d5 --- /dev/null +++ b/src/pointcloud_process/pointcloud_2d_process.cpp @@ -0,0 +1,132 @@ +/*该类的功能主要是点云的二维栅格处理 + edited by goldqiu -2022-04-9 +*/ +#include "map_conversion/pointcloud_process/pointcloud_2d_process.hpp" +#include "glog/logging.h" + +namespace map_conversion { +Pointcloud2dProcess::Pointcloud2dProcess(YAML::Node& config_node) { + + Global_map_file = config_node["Global_file_directory"].as(); + local_cloud_topic = config_node["local_cloud_frame"].as(); + global_z_adjust = atof((config_node["global_z"].as()).c_str()); + local_z_adjust = atof((config_node["local_z"].as()).c_str()); + cout << "max_angle:" << local_z_adjust << endl; +} + +void Pointcloud2dProcess::find_Z_value(CloudData& cloud_data){ + std::cout << "初始点云数据点数：" << cloud_data.cloud_ptr->points.size() << std::endl; + for(int i = 0; i < cloud_data.cloud_ptr->points.size() - 1; i++){ + cloud_data.cloud_ptr->points[i].z = cloud_data.cloud_ptr->points[i].z +0.7; + } + for(int i = 0; i < cloud_data.cloud_ptr->points.size() - 1; i++){ + if(cloud_data.cloud_ptr->points[i].z>max_z){ + max_z=cloud_data.cloud_ptr->points[i].z; + } + if(cloud_data.cloud_ptr->points[i].zpoints[i].z; + } + } + std::cout<<"orig max_z="< passthrough; + passthrough.setInputCloud(pcd_cloud.cloud_ptr);//输入点云 + passthrough.setFilterFieldName("z");//对z轴进行操作 + passthrough.setFilterLimits(min_z+local_z_adjust+global_z_adjust, max_z);//设置直通滤波器操作范围 + passthrough.setFilterLimitsNegative(flag_in);//true表示保留范围外，false表示保留范围内 + passthrough.filter(*cloud_after_PassThrough.cloud_ptr);//执行滤波，过滤结果保存在 cloud_after_PassThrough + std::cout << "直通滤波后点云数据点数：" << cloud_after_PassThrough.cloud_ptr->points.size() << std::endl; + for(int i = 0; i < cloud_after_PassThrough.cloud_ptr->points.size() - 1; i++){ + if(cloud_after_PassThrough.cloud_ptr->points[i].z>max_z){ + max_z=cloud_after_PassThrough.cloud_ptr->points[i].z; + } + if(cloud_after_PassThrough.cloud_ptr->points[i].zpoints[i].z; + } + } + std::cout<<"aft pass through filter: max_z="<points.empty()) + { + ROS_WARN("pcd is empty!\n"); + return; + } + + for(int i = 0; i < pcd_cloud.cloud_ptr->points.size() - 1; i++) + { + if(i == 0) + { + x_min = x_max = pcd_cloud.cloud_ptr->points[i].x; + y_min = y_max = pcd_cloud.cloud_ptr->points[i].y; + } + + double x = pcd_cloud.cloud_ptr->points[i].x; + double y = pcd_cloud.cloud_ptr->points[i].y; + + if(x < x_min) x_min = x; + if(x > x_max) x_max = x; + + if(y < y_min) y_min = y; + if(y > y_max) y_max = y; + } + + msg.info.origin.position.x = x_min; + msg.info.origin.position.y = y_min; + msg.info.origin.position.z = 0.0; + msg.info.origin.orientation.x = 0.0; + msg.info.origin.orientation.y = 0.0; + msg.info.origin.orientation.z = 0.0; + msg.info.origin.orientation.w = 1.0; + + msg.info.width = int((x_max - x_min) / map_resolution); + msg.info.height = int((y_max - y_min) / map_resolution); + + msg.data.resize(msg.info.width * msg.info.height); + msg.data.assign(msg.info.width * msg.info.height, 0); + + ROS_INFO("data size = %d\n", msg.data.size()); + + for(int iter = 0; iter < pcd_cloud.cloud_ptr->points.size(); iter++) + { + int i = int((pcd_cloud.cloud_ptr->points[iter].x - x_min) / map_resolution); + if(i < 0 || i >= msg.info.width) continue; + + int j = int((pcd_cloud.cloud_ptr->points[iter].y - y_min) / map_resolution); + if(j < 0 || j >= msg.info.height - 1) continue; + + msg.data[i + j * msg.info.width] = 100; +// msg.data[i + j * msg.info.width] = int(255 * (pcd_cloud.cloud_ptr->points[iter].z * k_line + b_line)) % 255; + } +} +} // namespace data_input \ No newline at end of file diff --git a/src/pointcloud_process/pointcloud_3d_process.cpp b/src/pointcloud_process/pointcloud_3d_process.cpp new file mode 100644 index 0000000..38ac95e --- /dev/null +++ b/src/pointcloud_process/pointcloud_3d_process.cpp @@ -0,0 +1,147 @@ +/*该类的功能主要是点云的三维栅格处理 + edited by goldqiu -2022-04-9 +*/ +#include "map_conversion/pointcloud_process/pointcloud_3d_process.hpp" +#include "glog/logging.h" + +typedef GridNode* GridNodePtr; +GridNodePtr *** GridNodeMap; + +namespace map_conversion { +Pointcloud3dProcess::Pointcloud3dProcess(YAML::Node& config_node) { + //三维栅格初始化 + _map_lower << -_y_size/2.0, -_y_size/2.0, 0.0; + _map_upper << +_x_size/2.0, +_y_size/2.0, _z_size; + + _inv_resolution = 1.0 / _resolution; + int _max_x_id = (int)(_x_size * _inv_resolution); + int _max_y_id = (int)(_y_size * _inv_resolution); + int _max_z_id = (int)(_z_size * _inv_resolution); + + GLX_SIZE = _max_x_id; + GLY_SIZE = _max_y_id; + GLZ_SIZE = _max_z_id; + GLYZ_SIZE = GLY_SIZE * GLZ_SIZE; + GLXYZ_SIZE = GLX_SIZE * GLYZ_SIZE; + initGridMap(_resolution, _map_lower, _map_upper);//要初始化地图 +} +Eigen::Vector3i Pointcloud3dProcess::coord2gridIndex(const Eigen::Vector3d pt) +{ + Eigen::Vector3i idx; + idx << std::min( std::max( int( (pt(0) - gl_xl) * _inv_resolution), 0), GLX_SIZE - 1), + std::min( std::max( int( (pt(1) - gl_yl) * _inv_resolution), 0), GLY_SIZE - 1), + std::min( std::max( int( (pt(2) - gl_zl) * _inv_resolution), 0), GLZ_SIZE - 1); + + return idx; +}; + +bool Pointcloud3dProcess::setObs(const double coord_x, const double coord_y, const double coord_z) +{ + if( coord_x < gl_xl || coord_y < gl_yl || coord_z < gl_zl || + coord_x >= gl_xu || coord_y >= gl_yu || coord_z >= gl_zu ) + return false; + + tmp_id_x = static_cast( (coord_x - gl_xl) * _inv_resolution); + tmp_id_y = static_cast( (coord_y - gl_yl) * _inv_resolution); + tmp_id_z = static_cast( (coord_z - gl_zl) * _inv_resolution); + + if(data[tmp_id_x * GLYZ_SIZE + tmp_id_y * GLZ_SIZE + tmp_id_z] == 0){ + data[tmp_id_x * GLYZ_SIZE + tmp_id_y * GLZ_SIZE + tmp_id_z] = 1; + return true; + } + else{ + return false; + } +} + +Eigen::Vector3d Pointcloud3dProcess::gridIndex2coord(const Eigen::Vector3i index) +{ + Eigen::Vector3d pt; + + pt(0) = ((double)index(0) + 0.5) * _resolution + gl_xl; + pt(1) = ((double)index(1) + 0.5) * _resolution + gl_yl; + pt(2) = ((double)index(2) + 0.5) * _resolution + gl_zl; + + return pt; +}; + +void Pointcloud3dProcess::initGridMap(double _resolution, Eigen::Vector3d global_xyz_l, Eigen::Vector3d global_xyz_u) +{ + gl_xl = global_xyz_l(0); + gl_yl = global_xyz_l(1); + gl_zl = global_xyz_l(2); + + gl_xu = global_xyz_u(0); + gl_yu = global_xyz_u(1); + gl_zu = global_xyz_u(2); + + double resolution = _resolution; + double inv_resolution = 1.0 / _resolution; + + data = new uint8_t[GLXYZ_SIZE]; + + memset(data, 0, GLXYZ_SIZE * sizeof(uint8_t)); + + GridNodeMap = new GridNodePtr ** [GLX_SIZE]; + for(int i = 0; i < GLX_SIZE; i++) + { + GridNodeMap[i] = new GridNodePtr * [GLY_SIZE]; + for(int j = 0; j < GLY_SIZE; j++) + { + GridNodeMap[i][j] = new GridNodePtr [GLZ_SIZE]; + for( int k = 0; k < GLZ_SIZE;k++) + { + Vector3i tmpIdx(i,j,k); + Vector3d pos = gridIndex2coord(tmpIdx); + GridNodeMap[i][j][k] = new GridNode(tmpIdx, pos); + GridNodeMap[i][j][k]->occupancy = & data[i * GLYZ_SIZE + j * GLZ_SIZE + k]; + } + } + } +} +void Pointcloud3dProcess::pointcloud_to_3dGridMap(CloudData& cloud_data,CloudData& cloud_inf) +{ + pcl::PointXYZ pt, pt_inf; + int inf_step = round(_cloud_margin * _inv_resolution); + //这里涉及膨胀地图的大小，resolution 为一个栅格大小， + //单位为m,设置为[0.15 0.30]之间，越大地图越稀疏，_inv_resolution,为resolution的倒数 + int inf_step_z = max(1, inf_step / 2); + for (int idx = 0; idx < (int)cloud_data.cloud_ptr->points.size(); idx++) + { + pt = cloud_data.cloud_ptr->points[idx]; + for(int x = -inf_step ; x <= inf_step; x ++ ) + { + for(int y = -inf_step ; y <= inf_step; y ++ ) + { + for(int z = -inf_step_z; z <= inf_step_z; z ++ ) + { + double inf_x = pt.x + x * _resolution; + double inf_y = pt.y + y * _resolution; + double inf_z = pt.z + z * _resolution; + + if(isnan(inf_x) || isnan(inf_y) || isnan(inf_z)) continue; + + Vector3d vec_inf(inf_x, inf_y, inf_z); + Vector3i idx_inf = coord2gridIndex(vec_inf); + + // set in obstacle points + bool flag_newly_occupied = setObs(inf_x, inf_y, inf_z); + + // rounding for visualizing the grid map + if(flag_newly_occupied) + { + Vector3d coor_round = gridIndex2coord(idx_inf); + pt_inf.x = coor_round(0); + pt_inf.y = coor_round(1); + pt_inf.z = coor_round(2); + cloud_inf.cloud_ptr->points.push_back(pt_inf); + } + } + } + } + } + cloud_inf.cloud_ptr->width = cloud_inf.cloud_ptr->points.size(); + cloud_inf.cloud_ptr->height = 1; + cloud_inf.cloud_ptr->is_dense = true; +} +} // namespace data_input \ No newline at end of file diff --git a/src/ros_topic_interface/cloud_publisher.cpp b/src/ros_topic_interface/cloud_publisher.cpp new file mode 100644 index 0000000..fa3ab09 --- /dev/null +++ b/src/ros_topic_interface/cloud_publisher.cpp @@ -0,0 +1,35 @@ +#include "map_conversion/ros_topic_interface/cloud_publisher.hpp" +#include "glog/logging.h" + +namespace map_conversion { +CloudPublisher::CloudPublisher(ros::NodeHandle& nh, + std::string topic_name, + std::string frame_id, + size_t buff_size) + :nh_(nh), frame_id_(frame_id) { + publisher_ = nh_.advertise(topic_name, buff_size); +} + +void CloudPublisher::Publish(CloudData::CLOUD_PTR& cloud_ptr_input, double time) { + ros::Time ros_time((float)time); + PublishData(cloud_ptr_input, ros_time); +} + +void CloudPublisher::Publish(CloudData::CLOUD_PTR& cloud_ptr_input) { + ros::Time time = ros::Time::now(); + PublishData(cloud_ptr_input, time); +} + +void CloudPublisher::PublishData(CloudData::CLOUD_PTR& cloud_ptr_input, ros::Time time) { + sensor_msgs::PointCloud2Ptr cloud_ptr_output(new sensor_msgs::PointCloud2()); + pcl::toROSMsg(*cloud_ptr_input, *cloud_ptr_output); + + cloud_ptr_output->header.stamp = time; + cloud_ptr_output->header.frame_id = frame_id_; + publisher_.publish(*cloud_ptr_output); +} + +bool CloudPublisher::HasSubscribers() { + return publisher_.getNumSubscribers() != 0; +} +} // namespace lidar_localization \ No newline at end of file diff --git a/src/ros_topic_interface/cloud_subscriber.cpp b/src/ros_topic_interface/cloud_subscriber.cpp new file mode 100644 index 0000000..2dea487 --- /dev/null +++ b/src/ros_topic_interface/cloud_subscriber.cpp @@ -0,0 +1,34 @@ +#include "map_conversion/ros_topic_interface/cloud_subscriber.hpp" +#include "glog/logging.h" + +namespace map_conversion { +CloudSubscriber::CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) + :nh_(nh) { + subscriber_ = nh_.subscribe(topic_name, buff_size, &CloudSubscriber::msg_callback, this); +} + +void CloudSubscriber::msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr) { + buff_mutex_.lock(); + + // convert ROS PointCloud2 to pcl::PointCloud: + CloudData cloud_data; + cloud_data.time = cloud_msg_ptr->header.stamp.toSec(); + pcl::fromROSMsg(*cloud_msg_ptr, *(cloud_data.cloud_ptr)); + // add new message to buffer: + new_cloud_data_.push_back(cloud_data); + + buff_mutex_.unlock(); +} + +void CloudSubscriber::ParseData(std::deque& cloud_data_buff) { + buff_mutex_.lock(); + + // pipe all available measurements to output buffer: + if (new_cloud_data_.size() > 0) { + cloud_data_buff.insert(cloud_data_buff.end(), new_cloud_data_.begin(), new_cloud_data_.end()); + new_cloud_data_.clear(); + } + + buff_mutex_.unlock(); +} +} // namespace data_input \ No newline at end of file