Skip to content

J
jfx_node_ros
Commit d6f5d7a2 authored by oscar's avatar oscar
Browse files
Options

上传发送逻辑修改

parent a90424a5
Hide whitespace changes
Inline Side-by-side
Showing with 688 additions and 7 deletions
CMakeLists.txt
View file @ d6f5d7a2
......@@ -2,8 +2,8 @@ cmake_minimum_required(VERSION 2.6)
project(jfx_node_ros)
#set(CMAKE_BUILD_TYPE Debug)
SET(CMAKE_BUILD_TYPE "Release")
set(CMAKE_BUILD_TYPE Debug)
#SET(CMAKE_BUILD_TYPE "Release")
SET(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g -ggdb")
SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O2 -Wall")
......@@ -11,6 +11,10 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Ofast -pthread -Wfatal
set(JFX_COMMON_LIBS_PATH ${PROJECT_SOURCE_DIR}/../jfx_common_libs)
find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
find_package(catkin REQUIRED COMPONENTS
roscpp
......@@ -26,7 +30,13 @@ find_package(catkin REQUIRED COMPONENTS
)
catkin_package(
CATKIN_DEPENDS roscpp rospy std_msgs jfx_common_msgs
CATKIN_DEPENDS
roscpp
rospy
std_msgs
jfx_common_msgs
#image_transport
#cv_bridge
)
#set(CMAKE_CXX_STANDARD 14)
......@@ -61,12 +71,15 @@ include_directories(
#add_executable(yolov5 ${PROJECT_SOURCE_DIR}/yolov5.cpp)
add_executable(${PROJECT_NAME}
${PROJECT_SOURCE_DIR}/src/main.cpp
${PROJECT_SOURCE_DIR}/src/SendRosMsg.cpp
)
target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS})
#target_link_libraries(${PROJECT_NAME} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES})
target_link_libraries(${PROJECT_NAME} yaml-cpp)
add_dependencies(${PROJECT_NAME} jfx_common_msgs_gencpp)
install(TARGETS
${PROJECT_NAME}
......
launch/jfx_node_ros.launch
View file @ d6f5d7a2
<launch>
<arg name="parampath" default="$(find jfx_node_ros)/config/params.yaml"/>
<arg name="run_mode" default="1"/>
<arg name="originDataPath" default="/media/sf_originData/data"/>
<arg name="gps_load_file" default="/media/sf_originData/102022041610.nav/102022041610_traj.txt"/>
<arg name="outPutSaveDir" default="/media/sf_originData/detect_data/"/>
<arg name="playDetectTxt" default="/media/sf_originData/detect_data/detect_gps_7.txt"/>
<arg name="pcdFileFolder" default="/media/sf_pcd_image/mesh"/>
<arg name="image0FileFolder" default="/media/sf_pcd_image/102022041610/image0"/>
<arg name="image1FileFolder" default="/media/sf_pcd_image/102022041610/image1"/>
<arg name="send_pcd" default="1"/>
<arg name="send_image" default="1"/>
<node pkg="jfx_node_ros" type="jfx_node_ros" name="jfx_node_ros" output="screen" >
<param name="parampath" value="$(arg parampath)" />
<param name="run_mode" value="$(arg run_mode)" />
<param name="originDataPath" value="$(arg originDataPath)" />
<param name="gps_load_file" value="$(arg gps_load_file)" />
<param name="outPutSaveDir" value="$(arg outPutSaveDir)" />
<param name="playDetectTxt" value="$(arg playDetectTxt)" />
<param name="pcdFileFolder" value="$(arg pcdFileFolder)" />
<param name="image0FileFolder" value="$(arg image0FileFolder)" />
<param name="image1FileFolder" value="$(arg image1FileFolder)" />
<param name="send_pcd" value="$(arg send_pcd)" />
<param name="send_image" value="$(arg send_image)" />
</node>
</launch>
src/SendRosMsg.cpp 0 → 100644
View file @ d6f5d7a2
#include "SendRosMsg.hpp"
#include <algorithm>
#include <iostream>
#include <fstream>
#include <sstream>
#include <sys/time.h>
#include <unistd.h>
#include <opencv2/opencv.hpp>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <jsk_recognition_msgs/BoundingBoxArray.h>
#include <tf2/LinearMath/Quaternion.h>
uint64_t GetCurTime()
{
struct timeval time;
gettimeofday(&time, NULL);
uint64_t seconds = time.tv_sec;
uint64_t ttt = seconds * 1000 + time.tv_usec/1000;
return ttt;
}
void rotate_pointcloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out)
{
// 顺时针旋转90度
Eigen::Matrix3f rot_matrix;
rot_matrix << 0, 1, 0, -1, 0, 0, 0, 0, 1;
for (size_t i = 0; i < cloud_in->size(); ++i)
{
Eigen::Vector3f point(cloud_in->points[i].x, cloud_in->points[i].y, cloud_in->points[i].z);
Eigen::Vector3f rotated_point = rot_matrix * point;
pcl::PointXYZ point_out(rotated_point(0), rotated_point(1), rotated_point(2));
cloud_out->push_back(point_out);
}
}
int LoadFileSendRosMsg::Play(ros::NodeHandle& nh)
{
m_pubFusionRes = nh.advertise<jfx_common_msgs::det_tracking_array>("/fusion_res", 100);
m_pubLocalization = nh.advertise<jfx_common_msgs::localization>("/localization_res", 100);
m_pubCloud = nh.advertise<sensor_msgs::PointCloud2>("/tracking_cloud", 100);
m_pubBoundingBoxes = nh.advertise<jsk_recognition_msgs::BoundingBoxArray>("/detect_boxes", 100);
m_pubMarkerArrow = nh.advertise<visualization_msgs::MarkerArray>("/Arrow", 100);
// 创建ROS图像传输对象
image_transport::ImageTransport it0(nh);
image_transport::Publisher pubImage0 = it0.advertise("/image0", 1);
image_transport::ImageTransport it1(nh);
image_transport::Publisher pubImage1 = it1.advertise("/image1", 1);
std::string loadFile = "/media/sf_originData/detect_data/detect_gps_40.txt";
nh.param<std::string>("playDetectTxt", loadFile, "/media/sf_originData/detect_data/detect_gps_40.txt");
std::string detectFolder = "/media/sf_originData/data";
nh.param<std::string>("originDataPath", detectFolder, "/media/sf_originData/data");
std::string pcdFolder = "/media/sf_pcd_image/mesh";
nh.param<std::string>("pcdFileFolder", pcdFolder, "/media/sf_pcd_image/mesh");
std::string image0Folder = "/media/sf_pcd_image/102022041610/image0";
nh.param<std::string>("image0FileFolder", image0Folder, "/media/sf_pcd_image/102022041610/image0");
std::string image1Folder = "/media/sf_pcd_image/102022041610/image1";
nh.param<std::string>("image1FileFolder", image1Folder, "/media/sf_pcd_image/102022041610/image1");
int send_pcd = 0;
int send_image = 0;
nh.param<int>("send_pcd", send_pcd, 0);
nh.param<int>("send_image", send_image, 0);
std::ifstream gpsFile;
gpsFile.open(loadFile, std::ios::in);
uint64_t base_curTime = 0;
uint64_t base_recvTime = 0;
std::string line;
do
{
std::getline(gpsFile, line);
if(line.empty())
break;
std::vector<std::string> values;
std::stringstream linestream(line);
std::string cell;
values.clear();
while (std::getline(linestream, cell, ','))
{
values.push_back(cell);
}
uint64_t recv_timestamp = std::stoll(values[2]);
int loc_idx = 8;
jfx_common_msgs::localization loc = {};
loc.timestamp = recv_timestamp;
loc.week = std::stoi(values[loc_idx++]);
loc.second = std::stoi(values[loc_idx++]);
loc.lat = std::stod(values[loc_idx++]);
loc.lon = std::stod(values[loc_idx++]);
loc.alt = std::stod(values[loc_idx++]);
loc.NorthV = std::stod(values[loc_idx++]);
loc.EastV = std::stod(values[loc_idx++]);
loc.UpV = std::stod(values[loc_idx++]);
loc.Roll = std::stod(values[loc_idx++]);
loc.Pitch = std::stod(values[loc_idx++]);
loc.Yaw = std::stod(values[loc_idx++]);
m_pubLocalization.publish(loc);
if(base_curTime == 0)
{
base_curTime = GetCurTime();
base_recvTime = recv_timestamp - 100;
}
int delay = recv_timestamp - base_recvTime;
int time_delay = GetCurTime() - base_curTime;
int sleep_delay = delay - time_delay;
if(sleep_delay > 0)
{
usleep(sleep_delay*1000);
}
std::string detectFile = detectFolder + "/" + values[4];
std::ifstream detFile;
detFile.open(detectFile, std::ios::in);
std::string det_line;
jfx_common_msgs::det_tracking_array det_array = {};
det_array.cloud.header.stamp.sec = recv_timestamp/1000;
det_array.cloud.header.stamp.nsec = recv_timestamp%1000*1e6;
jsk_recognition_msgs::BoundingBoxArray sendBoxes = {};
sendBoxes.header.frame_id = "Pandar64";
visualization_msgs::MarkerArray sendArrows = {};
int idx = 0;
do
{
std::getline(detFile, det_line);
if(det_line.empty())
break;
std::vector<std::string> _values;
std::stringstream _linestream(det_line);
std::string _cell;
_values.clear();
while (std::getline(_linestream, _cell, ' '))
{
_values.push_back(_cell);
}
float score = std::stof(_values[7]);
if(score < 0.4)
continue;
float x = std::stof(_values[0]);
float y = std::stof(_values[1]);
float z = std::stof(_values[2]);
float rot = std::stof(_values[6]) ;
Eigen::Vector4d pos(x, y, z, 1); //xyz为跟踪系下坐标
Eigen::Matrix4d matrix_ni = Eigen::Matrix4d::Identity();
matrix_ni.matrix() << -9.36218528e-05, -9.99968230e-01, 7.98499399e-03, -1.34839479e-02,
9.99726853e-01, 9.36218528e-05, 2.33679968e-02, -3.94606248e-02,
-2.33679968e-02, 7.98499399e-03, 9.99695083e-01, -1.68814610e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00;
// [[-9.36218528e-05 -9.99968230e-01 7.98499399e-03 -1.34839479e-02]
// [ 9.99726853e-01 9.36218528e-05 2.33679968e-02 -3.94606248e-02]
// [-2.33679968e-02 7.98499399e-03 9.99695083e-01 -1.68814610e+00]
// [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
Eigen::Vector4d pos_ni = matrix_ni* pos;
double theta_body = rot;
// 计算新的水平面的朝向角
Eigen::Matrix3d R_world_body;
R_world_body << std::cos(theta_body), -std::sin(theta_body), 0.0,
std::sin(theta_body), std::cos(theta_body), 0.0,
0.0, 0.0, 1.0;
Eigen::Matrix3d R_world_car = matrix_ni.block<3, 3>(0, 0) * R_world_body;
double theta_world = std::atan2(R_world_car(1, 0), R_world_car(0, 0));
jfx_common_msgs::det_tracking det_info = {};
det_info.frame = recv_timestamp;
det_info.type = std::stoi(_values[8]);
det_info.dataSource = 1;
det_info.score = score;
det_info.x = x;
det_info.y = y;
det_info.z = z;
det_info.l = std::stof(_values[3]);
det_info.w = std::stof(_values[4]);
det_info.h = std::stof(_values[5]);
det_info.rot_y = rot;
det_array.array.emplace_back(det_info);
jsk_recognition_msgs::BoundingBox box = {};
box.label = idx;
box.value = 2;
box.header.frame_id = "Pandar64";
box.dimensions.x = det_info.l;
box.dimensions.y = det_info.w;
box.dimensions.z = det_info.h;
box.pose.position.x = pos_ni(0);
box.pose.position.y = pos_ni(1);
box.pose.position.z = pos_ni(2);
Eigen::AngleAxisd V3(theta_world, Eigen::Vector3d(0, 0, 1));
Eigen::Quaterniond R_quat(V3);
box.pose.orientation.x = R_quat.x();
box.pose.orientation.y = R_quat.y();
box.pose.orientation.z = R_quat.z();
box.pose.orientation.w = R_quat.w();
sendBoxes.boxes.emplace_back(box);
visualization_msgs::Marker marker = {};
marker.id = idx++;
marker.header.frame_id = "/Pandar64";
marker.action = visualization_msgs::Marker::ADD;
marker.type = visualization_msgs::Marker::ARROW;
marker.pose.position.x = pos_ni(0);
marker.pose.position.y = pos_ni(1);
marker.pose.position.z = pos_ni(2);
tf2::Quaternion myquater;
myquater.setRPY( 0, 0, theta_world );
//myquater.normalize();
marker.pose.orientation.x = myquater.x();
marker.pose.orientation.y = myquater.y();
marker.pose.orientation.z = myquater.z();
marker.pose.orientation.w = myquater.w();
marker.scale.x = 6;
marker.scale.y = 1;
marker.scale.z = 0.1;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0;
marker.color.b = 0;
marker.lifetime = ros::Duration(0.5);
sendArrows.markers.emplace_back(marker);
}while(1);
m_pubFusionRes.publish(det_array);
m_pubBoundingBoxes.publish(sendBoxes);
m_pubMarkerArrow.publish(sendArrows);
if(send_pcd == 1)
{
// 读取PCD点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
std::string pcdFileName = pcdFolder + "/" + values[5];
if(pcl::io::loadPCDFile<pcl::PointXYZ>(pcdFileName, *cloud) == -1)
{
ROS_ERROR("Failed to read PCD file");
continue;
}
// 顺时针旋转点云
// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
// rotate_pointcloud(cloud, cloud_out);
// 创建ROS消息
sensor_msgs::PointCloud2Ptr msg(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*cloud, *msg);
msg->header.frame_id = "/Pandar64";
// 发布ROS消息
m_pubCloud.publish(msg);
}
if(!values[6].empty() && send_image == 1 )
{
// 读取JPEG图片
std::string image0FileName = image0Folder + "/" + values[6];
cv::Mat image0 = cv::imread(image0FileName, CV_LOAD_IMAGE_COLOR);
// 检查图片是否成功读取
if(!image0.data)
{
ROS_ERROR("Failed to read image file");
continue;
}
// 创建ROS消息
sensor_msgs::ImagePtr msg0 = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image0).toImageMsg();
// 发布ROS消息
pubImage0.publish(msg0);
}
// 读取JPEG图片
if(!values[7].empty() && send_image == 1 )
{
std::string image1FileName = image1Folder + "/" + values[7];
cv::Mat image1 = cv::imread(image1FileName, CV_LOAD_IMAGE_COLOR);
// 检查图片是否成功读取
if(!image1.data)
{
ROS_ERROR("Failed to read image file");
continue;
}
// 创建ROS消息
sensor_msgs::ImagePtr msg1 = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image1).toImageMsg();
// 发布ROS消息
pubImage1.publish(msg1);
}
printf("pub one frame timestamp = %llu\n",recv_timestamp);
}while(1);
printf("pub finish\n");
return 0;
}
src/SendRosMsg.hpp 0 → 100644
View file @ d6f5d7a2
#ifndef _SEND_ROS_MSG_HPP_
#define _SEND_ROS_MSG_HPP_
#include "ros/ros.h"
#include <jsk_recognition_msgs/BoundingBoxArray.h>
#include <visualization_msgs/MarkerArray.h>
#include "jfx_common_msgs/det_tracking_array.h"
#include "jfx_common_msgs/localization.h"
#include <sensor_msgs/PointCloud2.h>
class LoadFileSendRosMsg
{
public:
LoadFileSendRosMsg() = default;
~LoadFileSendRosMsg() = default;
int Play(ros::NodeHandle& nh);
ros::Publisher m_pubFusionRes;//发送融合后的结果
ros::Publisher m_pubLocalization;//发送定位结果
ros::Publisher m_pubBoundingBoxes;//发送3D框信息
ros::Publisher m_pubMarkerArray;//发送marker框信息
ros::Publisher m_pubMarkerArrow;//发送marker框信息
ros::Publisher m_pubCloud;
};
#endif
\ No newline at end of file
src/main.cpp
View file @ d6f5d7a2
......@@ -6,15 +6,323 @@
// #include "Synchronizer.hpp"
#include "unistd.h"
#include <ros/ros.h>
#include <dirent.h>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <sstream>
#include "SendRosMsg.hpp"
struct FileInfo
{
uint64_t timestamp;
std::string fileName;
std::string filePath;
std::string pcdFile;
};
int LoadImageList(std::ifstream& file,std::string path,std::vector<FileInfo>& lists);
int FindImageInfoFromTimstamp(uint64_t timestamp,std::vector<FileInfo>& lists,std::string& outFile);
int SaveFile(std::ofstream& file,std::string path,FileInfo& fileInfo,std::vector<std::string>& values,std::vector<FileInfo>& image0List,std::vector<FileInfo>& image1List);
int main(int argc, char** argv) {
ros::init(argc, argv, "jfx_node_ros");
printf("this is empty node\n");
// ros::NodeHandle phpriv_nh("~");
// juefx_data_gather::Synchronizer jfx_inter(phpriv_nh);
ros::NodeHandle phpriv_nh("~");
int run_mode = 0;
phpriv_nh.param<int>("run_mode", run_mode, 1);
if(run_mode == 0)
{
std::string directory = "/media/sf_originData/data"; // 指定目录的路径
std::string save_dir = "/media/sf_originData/detect_data";
std::string gps_load_file = "/media/sf_originData/102022041610.nav/102022041610_traj.txt";
std::string image0_txt = "/media/sf_pcd_image/102022041610/image0_raw.txt";
std::string image1_txt = "/media/sf_pcd_image/102022041610/image1_raw.txt";
std::string image0_folder = "/media/sf_pcd_image/102022041610/image0";
std::string image1_folder = "/media/sf_pcd_image/102022041610/image1";
phpriv_nh.param<std::string>("originDataPath", directory, "/media/sf_originData/data");//读取点云检测结果的路径
phpriv_nh.param<std::string>("outPutSaveDir", save_dir, "/media/sf_originData/detect_data");//生成最后数据的路径
phpriv_nh.param<std::string>("gpsLoadFile", gps_load_file, "/media/sf_originData/102022041610.nav/102022041610_traj.txt");//生成的gps数据路径
DIR *dir;
struct dirent *ent;
std::vector<FileInfo> filenames; // 存储所有文件名的向量
if ((dir = opendir(directory.c_str())) != NULL) { // 打开目录
while ((ent = readdir(dir)) != NULL) { // 遍历目录
if (ent->d_type == DT_DIR && strcmp(ent->d_name, ".") != 0 && strcmp(ent->d_name, "..") != 0) {
// 如果是一个文件夹，并且不是"."和".."，则遍历文件夹中的文件
std::string subdirectory = directory + "/" + ent->d_name;
std::string subDir = ent->d_name;
DIR *subdir;
struct dirent *subent;
if ((subdir = opendir(subdirectory.c_str())) != NULL) {
while ((subent = readdir(subdir)) != NULL) { // 遍历文件夹中的文件
if (subent->d_type == DT_REG) // 如果是一个普通文件，则将其文件名存储到向量中
{
std::string filename = subdirectory + "/" + subent->d_name;
std::string subFile = subDir + "/" + subent->d_name;
FileInfo info = {};
info.filePath = subFile;
info.fileName = subent->d_name;
size_t found = info.fileName.find_last_of('.');
if(found != std::string::npos)
{
std::string ts = info.fileName.substr(0,found);
info.pcdFile = subDir + "/output/" + ts + ".pcd";
double timestamp = std::stod(ts);
info.timestamp = timestamp*1000;
filenames.push_back(info);
}
}
}
closedir(subdir);
}
}
}
closedir(dir);
} else {
perror("Error opening directory"); // 如果打开目录失败，则输出错误信息
return EXIT_FAILURE;
}
std::sort(filenames.begin(), filenames.end(),[](FileInfo a, FileInfo b) {
return a.timestamp < b.timestamp;
}); // 对所有文件名进行排序
int fileSize = filenames.size();
std::cout << "Sorted filenames number :" << fileSize << std::endl;
std::cout << "Sorted filenames:" << std::endl;
for (int i = 0; i < 10; i++)
{
// std::cout << filenames[i].fileName << "," << filenames[i].timestamp << std::endl; // 输出排序后的所有文件名
printf("%s, %llu\n",filenames[i].filePath.c_str(),filenames[i].timestamp);
}
std::vector<FileInfo> image0List;
std::ifstream imageOtxt;
imageOtxt.open(image0_txt,std::ios::in);
LoadImageList(imageOtxt,image0_folder,image0List);
std::vector<FileInfo> image1List;
std::ifstream image1txt;
image1txt.open(image1_txt,std::ios::in);
LoadImageList(image1txt,image1_folder,image1List);
std::ifstream gpsFile;
gpsFile.open(gps_load_file, std::ios::in);
std::ofstream detectFile;
int file_index = 0;
std::vector<std::string> fileValues;
int delay = 100;
int find = 0;
std::string line;
do
{
std::getline(gpsFile, line);
if(line.empty())
break;
std::vector<std::string> values;
std::stringstream linestream(line);
std::string cell;
values.clear();
while (std::getline(linestream, cell, ','))
{
// std::cout << "[" << cell << "]";
values.push_back(cell);
}
double week = std::stod(values[0]);
double second = std::stod(values[1]);
double timestamp_ts = double(315936000.0 - 18 + week * 604800) + second;
uint64_t timestamp = timestamp_ts*1000;
if(file_index == fileSize)
break;
while(filenames[file_index].timestamp < timestamp && find == 0)
{
file_index++;
if(file_index == fileSize)
break;
}
if(file_index == fileSize)
break;
if(filenames[file_index].timestamp > timestamp + delay && find == 0)
continue;
int64_t dis = filenames[file_index].timestamp - timestamp;
if(labs(dis) < delay)
{
if(find == 0)
{
find = 1;
delay = dis;
fileValues = values;
}
else
{
delay = dis;
if(delay < 0)//保存数据
{
//保存
SaveFile(detectFile,save_dir,filenames[file_index],values,image0List,image1List);
file_index++;
find = 0;
delay = 100;
}
else
{
find = 1;
fileValues = values;
}
}
}
else if(find == 1)
{
//保存
SaveFile(detectFile,save_dir,filenames[file_index],values,image0List,image1List);
file_index++;
find = 0;
delay = 100;
}
ros::spin();
}while(!line.empty());
printf("all finish \n");
gpsFile.close();
detectFile.close();
ros::spin();
}
else if(run_mode == 1)
{
LoadFileSendRosMsg sendMsg = {};
sendMsg.Play(phpriv_nh);
ros::spin();
}
return 0;
}
int SaveFile(std::ofstream& file,std::string path,FileInfo& fileInfo,std::vector<std::string>& values,std::vector<FileInfo>& image0List,std::vector<FileInfo>& image1List)
{
static int perid = 0;
static int frame = 0;
static uint64_t last_timestamp = 0;
static uint64_t perid_start = 0;
int needPrint = 0;
if(perid == 0)
{
perid = 1;
frame = 1;
last_timestamp = fileInfo.timestamp;
perid_start = fileInfo.timestamp;
std::string save_file = path + "/" + "detect_gps_" + std::to_string(perid) + ".txt";
file.open(save_file,std::ios::out);
printf("save perid = %d begin , file = %s\n",perid,save_file.c_str());
needPrint = 1;
}
else
{
if(fileInfo.timestamp - last_timestamp < 200)
{
frame++;
}
else
{
file.close();
printf("save perid = %d end frame = %d, perid time = %llu\n",perid, frame, last_timestamp - perid_start);
perid++;
frame = 1;
std::string save_file = path + "/" + "detect_gps_" + std::to_string(perid) + ".txt";
file.open(save_file,std::ios::out);
printf("save perid = %d begin , file = %s\n",perid,save_file.c_str());
perid_start = fileInfo.timestamp;
needPrint = 1;
}
last_timestamp = fileInfo.timestamp;
}
std::string image0_str = "";
std::string image1_str = "";
FindImageInfoFromTimstamp(fileInfo.timestamp,image0List,image0_str);
FindImageInfoFromTimstamp(fileInfo.timestamp,image1List,image1_str);
std::stringstream lineData;
lineData << std::fixed << std::setprecision(8);
lineData << perid << "," << frame << "," << fileInfo.timestamp << "," << fileInfo.fileName << "," << fileInfo.filePath << "," << fileInfo.pcdFile << "," << image0_str << "," << image1_str;
for(auto iter : values)
{
lineData << "," << iter;
}
lineData << std::endl;
std::string str = lineData.str();
if(needPrint == 1)
printf("input timestamp = %llu, str = %s",fileInfo.timestamp,str.c_str());
file << str;
return 0;
}
int LoadImageList(std::ifstream& file,std::string path,std::vector<FileInfo>& lists)
{
do
{
std::string line;
std::getline(file, line);
if(line.empty())
break;
std::vector<std::string> values;
std::stringstream linestream(line);
std::string cell;
values.clear();
while (std::getline(linestream, cell, ','))
{
// std::cout << "[" << cell << "]";
values.push_back(cell);
}
if(values.size() != 3)
break;
double week = std::stod(values[0]);
double second = std::stod(values[1]);
double timestamp_ts = double(315936000.0 - 18 + week * 604800) + second;
uint64_t timestamp = timestamp_ts*1000;
if(values[2].at(values[2].size() -1) == '\r')
values[2].erase(values[2].end()-1);
FileInfo info = {};
info.timestamp = timestamp;
info.fileName = values[2];
info.filePath = path + "/" + values[2];
lists.emplace_back(info);
}while(1);
std::sort(lists.begin(), lists.end(),[](FileInfo a, FileInfo b) {
return a.timestamp < b.timestamp;
}); // 对所有文件名进行排序
return 0;
}
int FindImageInfoFromTimstamp(uint64_t timestamp,std::vector<FileInfo>& lists,std::string& outFile)
{
int find = 0;
uint64_t distance = 100;
for(int i = 0; i < lists.size(); i++)
{
int64_t dis = timestamp - lists[i].timestamp;
if(dis < -100)
break;
else
{
if(llabs(dis) <= distance)
{
find = 1;
outFile = lists[i].fileName;
distance = llabs(dis);
}
}
}
if(find == 1)
return 0;
else
return -1;
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment