From 78504c1b9a166f3005ed08272c3068c205a186ff Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Wed, 10 Mar 2021 18:05:00 +0800 Subject: [PATCH] add example that does not need ROS support --- README.md | 21 +++++++++-- common/config/yaml_config.cc | 13 +++++++ common/config/yaml_config.h | 5 +-- common/log/log.cc | 2 +- common/macro/macros.h | 2 +- examples/CMakeLists.txt | 16 +++++++++ examples/main_noros.cc | 67 ++++++++++++++++++++++++++++++++++++ examples/main_rosbag.cc | 9 +++-- 8 files changed, 124 insertions(+), 11 deletions(-) create mode 100644 common/config/yaml_config.cc create mode 100644 examples/main_noros.cc diff --git a/README.md b/README.md index 1338d5a..08a86de 100644 --- a/README.md +++ b/README.md @@ -27,17 +27,32 @@ Although **Oh-My-LOAM** is ROS-free, running it with ROS bag as input is the sim We'll take *nsh_indoor_outdoor.bag* as example. You can download this bag from [google drive](https://drive.google.com/file/d/1s05tBQOLNEDDurlg48KiUWxCp-YqYyGH/view) or [baidupan](https://pan.baidu.com/s/1TbfMQ3Rvmmn1hCjFhXSPcQ) (提取码:9nf7). -Launch: +Launch **Oh-My-LOAM**: ``` ./devel/lib/oh_my_loam/main_rosbag ../configs/config_nsh_indoor_outdoor.yaml ``` -Play bag: +Play ROS bag (in a new terminal): ``` ros play nsh_indoor_outdoor.bag ``` ## Run without ROS support -You can write by yourself. +Launch **Oh-My-LOAM**: +``` +./devel/lib/oh_my_loam/main_noros ../configs/config_nsh_indoor_outdoor.yaml xxxxxx +``` +Please replace `xxxxxx` with the directory that contains the input point cloud files with tree structure like following: +``` +xxxxxx +├── frame00000.pcd +├── frame00001.pcd +├── frame00002.pcd +├── frame00003.pcd +├── frame00004.pcd +├── ... +``` +Currently only `.pcd` format is supported. +You can modify `examples/main_noros.cc` to add support for other point cloud formats. # Dependences diff --git a/common/config/yaml_config.cc b/common/config/yaml_config.cc new file mode 100644 index 0000000..a8216f2 --- /dev/null +++ b/common/config/yaml_config.cc @@ -0,0 +1,13 @@ + +#include "common/config/yaml_config.h" + +namespace common { + +YAMLConfig::YAMLConfig() {} + +void YAMLConfig::Init(const std::string &file) { + config_.reset(new YAML::Node); + *config_ = YAML::LoadFile(file); +} + +} // namespace common \ No newline at end of file diff --git a/common/config/yaml_config.h b/common/config/yaml_config.h index 2735c71..b3aa214 100644 --- a/common/config/yaml_config.h +++ b/common/config/yaml_config.h @@ -11,10 +11,7 @@ namespace common { class YAMLConfig { public: - void Init(const std::string &file) { - config_.reset(new YAML::Node); - *config_ = YAML::LoadFile(file); - } + void Init(const std::string &file); template const T Get(const std::string &key) const { diff --git a/common/log/log.cc b/common/log/log.cc index 47c8b8c..ae18c4a 100644 --- a/common/log/log.cc +++ b/common/log/log.cc @@ -68,7 +68,7 @@ std::string CustomSink::GetColorCode(const LEVELS &level) const { return "\033[1m\033[34m"; // bold blue } if (g3::internal::wasFatal(level)) { - return "\033[1m\033[31m"; // red + return "\033[1m\033[31m"; // bold red } return "\033[97m"; // white } diff --git a/common/macro/macros.h b/common/macro/macros.h index 96a2e1f..fc18766 100644 --- a/common/macro/macros.h +++ b/common/macro/macros.h @@ -20,5 +20,5 @@ } \ \ private: \ - classname() = default; \ + classname(); \ DISALLOW_COPY_AND_ASSIGN(classname) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 668356d..0710a4d 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -32,3 +32,19 @@ target_link_libraries(main_rosbag visualizer base ) + +add_executable(main_noros main_noros.cc) +target_link_libraries(main_noros + ${PCL_LIBRARIES} + ${G3LOG_LIBRARIES} + ${YAML_CPP_LIBRARIES} + common + oh_my_loam + extractor + odometer + mapper + solver + ${CERES_LIBRARIES} + visualizer + base +) \ No newline at end of file diff --git a/examples/main_noros.cc b/examples/main_noros.cc new file mode 100644 index 0000000..bdd9cb9 --- /dev/null +++ b/examples/main_noros.cc @@ -0,0 +1,67 @@ +#include + +#include +#include +#include +#include + +#include "common/common.h" +#include "oh_my_loam/oh_my_loam.h" + +using namespace common; +using namespace oh_my_loam; +namespace fs = std::filesystem; + +void PointCloudHandler(const PointCloudConstPtr &cloud, OhMyLoam *const slam); + +int main(int argc, char *argv[]) { + if (argc != 3) { + std::cerr << "\033[1m\033[31mConfiguration file and input path should be " + "specified!\033[m" + << std::endl; + return -1; + } + // config + YAMLConfig::Instance()->Init(argv[1]); + bool is_log_to_file = YAMLConfig::Instance()->Get("log_to_file"); + std::string log_path = YAMLConfig::Instance()->Get("log_path"); + std::string lidar = YAMLConfig::Instance()->Get("lidar"); + // logging + InitG3Logging(is_log_to_file, "oh_my_loam_" + lidar, log_path); + AUSER << "LOAM start..., lidar = " << lidar; + // SLAM system + OhMyLoam slam; + if (!slam.Init()) { + AFATAL << "Failed to initilize slam system."; + } + // get input point cloud file names + ACHECK(fs::exists(argv[2])) << "Directory not exist: " << argv[2]; + std::vector cloud_paths; + for (auto &it : fs::directory_iterator(argv[2])) { + if (fs::is_regular_file(it.path())) cloud_paths.push_back(it.path()); + } + AWARN_IF(cloud_paths.empty()) + << "No point cloud file in directory: " << argv[2]; + AINFO_IF(!cloud_paths.empty()) + << "There are " << cloud_paths.size() << " point clouds in total"; + std::sort(cloud_paths.begin(), cloud_paths.end()); + // load point cloud and process + for (auto &path : cloud_paths) { + PointCloudPtr cloud(new PointCloud); + pcl::io::loadPCDFile(path.string(), *cloud); + PointCloudHandler(cloud, &slam); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + return 0; +} + +void PointCloudHandler(const PointCloudConstPtr &cloud, OhMyLoam *const slam) { + auto millisecs = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + double timestamp = millisecs.count() / 1000.0; + static size_t frame_id = 0; + AINFO << "Ohmyloam: frame_id = " << ++frame_id + << ", timestamp = " << FMT_TIMESTAMP(timestamp) + << ", point_number = " << cloud->size(); + slam->Run(timestamp, cloud); +} \ No newline at end of file diff --git a/examples/main_rosbag.cc b/examples/main_rosbag.cc index 98961e6..a140c3e 100644 --- a/examples/main_rosbag.cc +++ b/examples/main_rosbag.cc @@ -16,13 +16,18 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr &msg, OhMyLoam *const slam); int main(int argc, char *argv[]) { + if (argc != 2) { + std::cerr << "\033[1m\033[31mConfiguration file should be specified!\033[m" + << std::endl; + return -1; + } // config YAMLConfig::Instance()->Init(argv[1]); - bool log_to_file = YAMLConfig::Instance()->Get("log_to_file"); + bool is_log_to_file = YAMLConfig::Instance()->Get("log_to_file"); std::string log_path = YAMLConfig::Instance()->Get("log_path"); std::string lidar = YAMLConfig::Instance()->Get("lidar"); // logging - InitG3Logging(log_to_file, "oh_my_loam_" + lidar, log_path); + InitG3Logging(is_log_to_file, "oh_my_loam_" + lidar, log_path); AUSER << "LOAM start..., lidar = " << lidar; // SLAM system OhMyLoam slam;