diff --git a/CMakeLists.txt b/CMakeLists.txt index 94945af..d182342 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(file_player) -add_definitions(-std=c++11) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) if (NOT CMAKE_BUILD_TYPE) set (CMAKE_BUILD_TYPE release) diff --git a/src/ROSThread.cpp b/src/ROSThread.cpp index 3081d8e..5a66106 100644 --- a/src/ROSThread.cpp +++ b/src/ROSThread.cpp @@ -1,4 +1,5 @@ #include +#include #include "ROSThread.h" @@ -7,7 +8,7 @@ using namespace std; struct PointXYZIRT { PCL_ADD_POINT4D; float intensity; - uint32_t t; + std::uint32_t t; int ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -15,7 +16,7 @@ struct PointXYZIRT { POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRT, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) - (uint32_t, t, t) (int, ring, ring) + (std::uint32_t, t, t) (int, ring, ring) ) @@ -593,7 +594,7 @@ ROSThread::RadarpolarThread() string current_radarpolar_name = data_folder_path_ + "/sensor_data/radar/polar" + "/" + to_string(data) + ".png"; cv::Mat radarpolar_image; - radarpolar_image = imread(current_radarpolar_name, CV_LOAD_IMAGE_GRAYSCALE); + radarpolar_image = imread(current_radarpolar_name, cv::IMREAD_GRAYSCALE); if(!radarpolar_image.empty()) { @@ -615,7 +616,7 @@ ROSThread::RadarpolarThread() string next_radarpolar_name = data_folder_path_ + "/radar/polar" +"/"+ radarpolar_file_list_[current_img_index+1]; cv::Mat radarpolar_image; - radarpolar_image = imread(next_radarpolar_name, CV_LOAD_IMAGE_COLOR); + radarpolar_image = imread(next_radarpolar_name, cv::IMREAD_COLOR); if(!radarpolar_image.empty()) { @@ -779,6 +780,39 @@ void ROSThread::SaveRosbag() //cout<<"Written: "< ouster_cloud; + while (!ouster_bin.eof()) { + PointXYZIRT point; + ouster_bin.read(reinterpret_cast(&point.x), sizeof(float)); + ouster_bin.read(reinterpret_cast(&point.y), sizeof(float)); + ouster_bin.read(reinterpret_cast(&point.z), sizeof(float)); + ouster_bin.read(reinterpret_cast(&point.intensity), sizeof(float)); + point.ring = (num_points % 64) + 1; + num_points++; + ouster_cloud.points.emplace_back(point); + } + ouster_bin.close(); + + sensor_msgs::PointCloud2 ouster_cloud2; + pcl::toROSMsg(ouster_cloud, ouster_cloud2); + const std::int64_t ouster_stamp = std::stoull(ouster_file.path().stem()); + ouster_cloud2.header.stamp.fromNSec(ouster_stamp); + ouster_cloud2.header.frame_id = "ouster"; + bag.write("/os1_points", ouster_cloud2.header.stamp, ouster_cloud2); + std::cout << "processing ouster: " << num_processed_ouster++ << "/" << num_ouster_bins << std::endl; + } + cout<<"rosbag stored at: "< #include #include -#include -#include #include #include #include