add gps raw

CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   geometry_msgs
   nav_msgs
+  message_generation
   cv_bridge
 )
 
@@ -52,11 +53,10 @@ catkin_python_setup()
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+  FILES
+  PohangGPS.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -73,10 +73,11 @@ catkin_python_setup()
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   sensor_msgs#   std_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  sensor_msgs
+  std_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##

dataset_converter/pohang.py

@@ -5,6 +5,7 @@ import cv_bridge
 import numpy as np
 import dataset_converter.util as util
 from pathlib import Path
+from dataset_converter.msg import PohangGPS
 from sensor_msgs.msg import Imu, PointCloud2, PointField, NavSatFix
 from geometry_msgs.msg import PoseStamped
 
@@ -13,7 +14,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"),
+    "gps.txt": ("gps_link", "/gps/gps_nav"),
 }
 LIDAR_PROCESS = {
     "lidar_front/points": (
@@ -254,6 +255,7 @@ def process_navigation(data_folder: Path, rosbag_path: Path):
             l = list(map(float, l))
             t = rospy.Time.from_sec(l[0])
 
+            # NavSatFix Part
             gps = NavSatFix()
             gps.header.seq = idx
             gps.header.stamp = t
@@ -264,6 +266,22 @@ def process_navigation(data_folder: Path, rosbag_path: Path):
 
             bag.write(NAVIGATION_PROCESS["gps.txt"][1], gps, t)
 
+            # PohangGPS Part
+            gps = PohangGPS()
+            gps.header.seq = idx
+            gps.header.stamp = t
+            gps.header.frame_id = NAVIGATION_PROCESS["gps.txt"][0]
+            gps.gps_time = l[1]
+            gps.latitude = l[2]
+            gps.longitude = l[3]
+            gps.heading = l[4]
+            gps.gps_quality_indicator = int(l[5])
+            gps.num_satellites = int(l[6])
+            gps.horizontal_dilution_of_precision = l[7]
+            gps.geoid = l[8]
+
+            bag.write(NAVIGATION_PROCESS["gps.txt"][1] + "_raw", gps, t)
+
     print("GPS Done!")
 
 

msg/PohangGPS.msg

+
+std_msgs/Header header
+
+# in sec
+float64 gps_time
+
+# in deg
+float64 latitude
+float64 longitude
+float64 heading
+
+uint8 gps_quality_indicator
+uint8 num_satellites
+float64 horizontal_dilution_of_precision
+
+# in meter
+float64 geoid

package.xml

@@ -55,6 +55,7 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>nav_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
   <build_depend>cv_bridge</build_depend>
   <build_depend>OpenCV</build_depend>
   <build_export_depend>roscpp</build_export_depend>
@@ -73,6 +74,7 @@
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>cv_bridge</exec_depend>
   <exec_depend>OpenCV</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->