From bac11f3e25e1de8370c2f57b5258ca5ec301fec4 Mon Sep 17 00:00:00 2001
From: Jiandong Chen <jiandong.chen@rwth-aachen.de>
Date: Mon, 23 Oct 2023 15:44:50 +0200
Subject: [PATCH] add gps
---
dataset_converter/pohang.py | 25 +++++++++++++++++++++++--
1 file changed, 23 insertions(+), 2 deletions(-)
diff --git a/dataset_converter/pohang.py b/dataset_converter/pohang.py
index e1df480..ab11c98 100644
--- a/dataset_converter/pohang.py
+++ b/dataset_converter/pohang.py
@@ -5,7 +5,7 @@ import cv_bridge
import numpy as np
import dataset_converter.util as util
from pathlib import Path
-from sensor_msgs.msg import Imu, PointCloud2, PointField
+from sensor_msgs.msg import Imu, PointCloud2, PointField, NavSatFix
from geometry_msgs.msg import PoseStamped
# used by process func
@@ -13,6 +13,7 @@ from geometry_msgs.msg import PoseStamped
NAVIGATION_PROCESS = {
"ahrs.txt": ("gx5_link", "/gx5/imu/data"),
"baseline.txt": ("gx5_link", "/gx5/baseline"),
+ "gps.txt": ("gps_link", "/gps_nav"),
}
LIDAR_PROCESS = {
"lidar_front/points": (
@@ -196,7 +197,6 @@ def process_navigation(data_folder: Path, rosbag_path: Path):
# AHRS Part
data = txt_preprocess(data_path.joinpath("ahrs.txt"))
-
with rosbag.Bag(rosbag_path, check_bag_mode(rosbag_path)) as bag:
for idx, l in enumerate(data):
l = list(map(float, l))
@@ -245,6 +245,27 @@ def process_navigation(data_folder: Path, rosbag_path: Path):
print("Baseline Done!")
+ # GPS Part
+ data = txt_preprocess(data_path.joinpath("gps.txt"))
+ with rosbag.Bag(rosbag_path, check_bag_mode(rosbag_path)) as bag:
+ for idx, l in enumerate(data):
+ l.remove("N")
+ l.remove("E")
+ l = list(map(float, l))
+ t = rospy.Time.from_sec(l[0])
+
+ gps = NavSatFix()
+ gps.header.seq = idx
+ gps.header.stamp = t
+ gps.header.frame_id = NAVIGATION_PROCESS["gps.txt"][0]
+ gps.latitude = l[2]
+ gps.longitude = l[3]
+ gps.altitude = l[-1]
+
+ bag.write(NAVIGATION_PROCESS["gps.txt"][1], gps, t)
+
+ print("GPS Done!")
+
def process_stereo(data_folder: Path, rosbag_path: Path):
data_path = data_folder.joinpath("stereo")
--
GitLab