diff --git a/dataset_converter/pohang.py b/dataset_converter/pohang.py index e1df480222a43635779fe58a066fa54772d315ac..ab11c988e09c1cba092abf546635c099a1a405cf 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")