diff --git a/.gitignore b/.gitignore index 74d6dfe..bbd868a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,3 @@ /.vscode /.log -/lib /build \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 4105e05..71efcad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,8 +6,9 @@ set(CMAKE_CXX_FLAGS "-std=c++17 -lpthread") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") find_package(Ceres REQUIRED) -find_package(PCL REQUIRED) +find_package(PCL QUIET) find_package(g3log REQUIRED) +find_package(yaml-cpp REQUIRED) find_package(catkin REQUIRED COMPONENTS geometry_msgs @@ -22,11 +23,14 @@ find_package(catkin REQUIRED COMPONENTS tf ) -include_directories( +include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} ${G3LOG_INCLUDE_DIRS} +) + +include_directories( src common ) @@ -37,7 +41,6 @@ catkin_package( INCLUDE_DIRS src common ) - add_subdirectory(common) add_subdirectory(src) @@ -46,7 +49,8 @@ target_link_libraries(oh_my_loam ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} - ${G3LOG_INCLUDE_DIRS} + ${G3LOG_LIBRARIES} + ${YAML_CPP_LIBRARIES} g3log OhMyLoam ) diff --git a/config/config.yaml b/config/config.yaml new file mode 100644 index 0000000..8081d36 --- /dev/null +++ b/config/config.yaml @@ -0,0 +1,6 @@ +%YAML:1.0 + +lidar: VPL16 + +feature_points_extractor_config: + min_point_num: 10 \ No newline at end of file diff --git a/main.cc b/main.cc index ccacc7e..6f3e99c 100644 --- a/main.cc +++ b/main.cc @@ -1,6 +1,7 @@ #include #include #include +#include #include @@ -11,11 +12,17 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg, oh_my_loam::OhMyLoam* const slam); int main(int argc, char* argv[]) { - g3::InitG3Logging("oh_my_loam", ".log"); + // logging + g3::InitG3Logging("oh_my_loam", ".log"); + // configurations + YAML::Node config = YAML::LoadFile("./config/config.yaml"); + AWARN << config["lidar"].as(); + // SLAM system oh_my_loam::OhMyLoam slam; - slam.Init(); + slam.Init(config); + // ros ros::init(argc, argv, "oh_my_loam"); ros::NodeHandle nh; ros::Subscriber sub_point_cloud = nh.subscribe( diff --git a/src/oh_my_loam.cc b/src/oh_my_loam.cc index 4beec4d..4569347 100644 --- a/src/oh_my_loam.cc +++ b/src/oh_my_loam.cc @@ -2,8 +2,9 @@ namespace oh_my_loam { -bool OhMyLoam::Init() { +bool OhMyLoam::Init(const YAML::Node& config) { is_vis_ = true; + config_ = config; feature_extractor_.reset(new FeatureExtractorVLP16); if (is_vis_) { visualizer_.reset(new FeaturePointsVisualizer); diff --git a/src/oh_my_loam.h b/src/oh_my_loam.h index b9027e9..e31af52 100644 --- a/src/oh_my_loam.h +++ b/src/oh_my_loam.h @@ -1,5 +1,7 @@ #pragma once +#include + #include "common.h" #include "feature_extractor_VLP16.h" #include "visualizer_feature_points.h" @@ -13,7 +15,7 @@ class OhMyLoam { OhMyLoam(const OhMyLoam&) = delete; OhMyLoam& operator=(const OhMyLoam&) = delete; - bool Init(); + bool Init(const YAML::Node& config); void Run(const PointCloud& cloud, double timestamp); @@ -25,6 +27,7 @@ class OhMyLoam { bool is_vis_ = false; std::unique_ptr feature_extractor_ = nullptr; std::unique_ptr visualizer_ = nullptr; + YAML::Node config_; }; } // namespace oh_my_loam \ No newline at end of file