diff --git a/CMakeLists.txt b/CMakeLists.txt index 51916bb42e2b8f478c1000c0183b8f84cc0275f4..63dc69994da6cc03a072e8a4b645f7afa8e31476 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,8 +11,10 @@ find_package(OpenCV REQUIRED) find_package(catkin REQUIRED COMPONENTS roscpp rospy - sensor_msgs std_msgs + sensor_msgs + geometry_msgs + nav_msgs cv_bridge ) diff --git a/dataset_converter/pohang.py b/dataset_converter/pohang.py index 09f00d9002a89a306c26dcd7af8d6c68725d6395..6c14ec8e8bd584be2fe325d407edbbaf752d27f1 100644 --- a/dataset_converter/pohang.py +++ b/dataset_converter/pohang.py @@ -4,9 +4,11 @@ import cv2 import cv_bridge import dataset_converter.util as util from pathlib import Path +from sensor_msgs.msg import Imu # used by process func -# map "folder_name" : ("LINK_NAME", "TOPIC_NAME") +# map "folder_name(file_name)" : ("LINK_NAME", "TOPIC_NAME") +NAVIGATION_PROCESS = {"ahrs.txt": ("gx5_link", "/gx5/imu/data")} STEREO_PROCESS = { "left_images": ("stereo_left_link", "/stereo_cam/left_img/compressed"), "right_images": ("stereo_right_link", "/stereo_cam/right_img/compressed"), @@ -34,6 +36,7 @@ DATA_FOLDER_GENERAL = { "radar", "stereo", } +NAVIGATION_FOLDER = {"ahrs.txt", "baseline.txt", "gps.txt"} STEREO_FOLDER = {"left_images", "right_images", "timestamp.txt"} INFRARED_FOLDER = {"images", "timestamp.txt"} OMNI_FOLDER = {"cam_0", "cam_1", "cam_2", "cam_3", "cam_4", "cam_5", "timestamp.txt"} @@ -47,6 +50,10 @@ def is_datafolder_valid(folder: Path): res &= util.is_folder_contain_folders(folder, DATA_FOLDER_GENERAL) if res: res &= util.is_folder_contain_folders(folder.joinpath("stereo"), STEREO_FOLDER) + if res: + res &= util.is_folder_contain_folders( + folder.joinpath("navigation"), NAVIGATION_FOLDER + ) if res: res &= util.is_folder_contain_folders( folder.joinpath("infrared"), INFRARED_FOLDER @@ -84,10 +91,8 @@ def process_images( # store all images into bag bridge = cv_bridge.CvBridge() - if rosbag_path.exists(): - bagmode = "a" - else: - bagmode = "w" + + bagmode = check_bag_mode(rosbag_path) with rosbag.Bag(rosbag_path, bagmode) as bag: for img_path, time_list in zip(images, timestamps): @@ -114,6 +119,40 @@ def process_images( bag.write(topic, msg, t) +def process_navigation(data_folder: Path, rosbag_path: Path): + data_path = data_folder.joinpath("navigation") + + ahrs = data_path.joinpath("ahrs.txt") + with open(ahrs, "r") as f: + data = f.readlines() + data = [l.strip().split("\t") for l in data] + + bagmode = check_bag_mode(rosbag_path) + with rosbag.Bag(rosbag_path, bagmode) as bag: + for idx, l in enumerate(data): + l = list(map(float, l)) + t = rospy.Time.from_sec(l[0]) + + imu = Imu() + imu.header.seq = idx + imu.header.stamp = t + imu.header.frame_id = NAVIGATION_PROCESS["ahrs.txt"][0] + imu.orientation.x = l[1] + imu.orientation.y = l[2] + imu.orientation.z = l[3] + imu.orientation.w = l[4] + imu.angular_velocity.x = l[5] + imu.angular_velocity.y = l[6] + imu.angular_velocity.z = l[7] + imu.linear_acceleration.x = l[8] + imu.linear_acceleration.y = l[9] + imu.linear_acceleration.z = l[10] + + bag.write(NAVIGATION_PROCESS["ahrs.txt"][1], imu, t) + + print("AHRS Done!") + + def process_stereo(data_folder: Path, rosbag_path: Path): data_path = data_folder.joinpath("stereo") for k, v in STEREO_PROCESS.items(): @@ -141,3 +180,12 @@ def process_omni(data_folder: Path, rosbag_path: Path): process_images(data_path / k, rosbag_path, v[1], v[0]) print("Omni Done!") + + +def check_bag_mode(rosbag_path): + if rosbag_path.exists(): + bagmode = "a" + else: + bagmode = "w" + + return bagmode diff --git a/package.xml b/package.xml index 334a823e6d7a33b8087aeddc2f84063a19ec00c3..c0a727d1c1603dc2fe985067eb005ca91d5802b2 100644 --- a/package.xml +++ b/package.xml @@ -51,20 +51,26 @@ <buildtool_depend>catkin</buildtool_depend> <build_depend>roscpp</build_depend> <build_depend>rospy</build_depend> - <build_depend>sensor_msgs</build_depend> <build_depend>std_msgs</build_depend> + <build_depend>sensor_msgs</build_depend> + <build_depend>geometry_msgs</build_depend> + <build_depend>nav_msgs</build_depend> <build_depend>cv_bridge</build_depend> <build_depend>OpenCV</build_depend> <build_export_depend>roscpp</build_export_depend> <build_export_depend>rospy</build_export_depend> - <build_export_depend>sensor_msgs</build_export_depend> <build_export_depend>std_msgs</build_export_depend> + <build_export_depend>sensor_msgs</build_export_depend> + <build_export_depend>geometry_msgs</build_export_depend> + <build_export_depend>nav_msgs</build_export_depend> <build_export_depend>cv_bridge</build_export_depend> <build_export_depend>OpenCV</build_export_depend> <exec_depend>roscpp</exec_depend> <exec_depend>rospy</exec_depend> - <exec_depend>sensor_msgs</exec_depend> <exec_depend>std_msgs</exec_depend> + <exec_depend>sensor_msgs</exec_depend> + <exec_depend>geometry_msgs</exec_depend> + <exec_depend>nav_msgs</exec_depend> <exec_depend>cv_bridge</exec_depend> <exec_depend>OpenCV</exec_depend> diff --git a/scripts/pohang2rosbag.py b/scripts/pohang2rosbag.py index 2daf77844781e3184f3c105d9a73e9fc2e290a8c..802faccbb7b5ae698051259ba1997360c30a57dd 100755 --- a/scripts/pohang2rosbag.py +++ b/scripts/pohang2rosbag.py @@ -22,6 +22,7 @@ def main(): continue rosbag_file_path = rosbag_file_dir.joinpath(data_folder.name + ".bag") + pohang.process_navigation(data_folder, rosbag_file_path) pohang.process_stereo(data_folder, rosbag_file_path) pohang.process_infrared(data_folder, rosbag_file_path) pohang.process_omni(data_folder, rosbag_file_path)