forked from haeyeoni/lidar_slam_evaluator
-
Notifications
You must be signed in to change notification settings - Fork 0
/
autopia_only_nav_gt.py
70 lines (58 loc) · 2.34 KB
/
autopia_only_nav_gt.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
from geodesy import utm
import rospy
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path
import rosbag
import heapq
import sys
import math
def find_pairs_and_publish(navsatfix_msgs, navrelposned_msgs,pose_pub, path, time_tolerance=0.1):
tolerance = rospy.Duration.from_sec(time_tolerance)
bag = rosbag.Bag(path + 'autopia_gt.bag', 'w')
fix_heap = []
ned_heap = []
gt_path = Path()
gt_path.header.frame_id = '/map'
origin = None
for msg, t in navsatfix_msgs:
heapq.heappush(fix_heap, (t, msg))
fix_time, fix_msg = heapq.heappop(fix_heap)
if origin is None:
origin = utm.fromLatLong(fix_msg.latitude, fix_msg.longitude, fix_msg.altitude)
origin_easting = origin.easting
origin_northing = origin.northing
origin_altitude = fix_msg.altitude
# Convert NavSatFix to UTM
utm_point = utm.fromLatLong(fix_msg.latitude, fix_msg.longitude, fix_msg.altitude)
# Calculate relative position from origin
dx = utm_point.easting - origin_easting
dy = utm_point.northing - origin_northing
dz = fix_msg.altitude - origin_altitude
pose_msg = PoseStamped()
pose_msg.header.stamp = fix_time
pose_msg.header.frame_id = "world"
pose_msg.pose.position.x = dx
pose_msg.pose.position.y = dy
pose_msg.pose.position.z = dz
pose_msg.pose.orientation.w = 1 # Assuming no orientation information
gt_path.header.stamp = pose_msg.header.stamp
gt_path.poses.append(pose_msg)
bag.write('ground_truth', gt_path, t=gt_path.header.stamp)
bag.close()
def process_rosbag(path, name):
bag = rosbag.Bag(path + name)
pose_pub = rospy.Publisher('path', PoseStamped, queue_size=10)
navsatfix_msgs = []
navrelposned9_msgs = []
for topic, msg, t in bag.read_messages(topics=['/ublox/fix','/ublox/navrelposned']):
if topic == '/ublox/fix':
navsatfix_msgs.append((msg, msg.header.stamp))
else:
navrelposned9_msgs.append((msg, msg.header.stamp))
find_pairs_and_publish(navsatfix_msgs,navrelposned9_msgs, pose_pub, path)
bag.close()
if __name__ == '__main__':
if len(sys.argv) < 3:
print("Usage: script.py <path_to_bag> <bag_filename>")
sys.exit(1)
process_rosbag(sys.argv[1], sys.argv[2])