Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix lidar timestamp #19

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

fix lidar timestamp #19

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 10 additions & 9 deletions helpers/bagdecoder.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -204,11 +204,10 @@ def convert_bag(self):

# Process topic and update msg with point cloud if all packets received
if topic_type=="ouster_ros/PacketMsg":
lidar_state_dict, msg = self.qpacket(topic, topic_type, msg, ts, lidar_state_dict)
lidar_state_dict, msg, ts = self.qpacket(topic, topic_type, msg, ts, lidar_state_dict)
if msg is not None: # Convert points numpy to pointcloud2
ts = lidar_state_dict['qp_ts']
msg = pub_pc_to_rviz(
msg, topic_pubs[topic], ts,
msg, topic_pubs[topic], ts
point_type=self.point_fields,
seq=lidar_state_dict['frame'],
publish=False
Expand Down Expand Up @@ -274,19 +273,21 @@ def qpacket(self, topic, topic_type, msg, ts, state_dict, do_sync=True):
packet = get_ouster_packet_info(self.os1_info, msg.buf)

pc_np = None
init_ts = state_dict['qp_ts'] # initial timestamp of scan to process
if packet.frame_id!=state_dict['qp_frame_id']:
if state_dict['qp_counter']==self.OS1_PACKETS_PER_FRAME:
pc_np, ts = self.process_topic(topic, topic_type, state_dict['qp_scan_queue'], ts)
state_dict['qp_ts'] = ts # Update timestamp to earliest packet
pc_np, _ = self.process_topic(topic, topic_type,
state_dict['qp_scan_queue'], init_ts)

state_dict['qp_counter'] = 0
state_dict['qp_frame_id'] = packet.frame_id
state_dict['qp_scan_queue'] = []
state_dict['qp_ts'] = ts # Update timestamp to earliest packet

state_dict['qp_counter'] += 1
state_dict['qp_scan_queue'].append(packet)

return state_dict, pc_np
return state_dict, pc_np, init_ts

def save_topic(self, data, topic, topic_type, trajectory, frame):
info = self.sensor_topics[topic]
Expand Down Expand Up @@ -353,8 +354,8 @@ def process_topic(self, topic, topic_type, msg, t):
data = None
sensor_ts = t
if topic_type=="ouster_ros/PacketMsg":
data, sensor_ts = process_ouster_packet(
self.os1_info, self.os1_dict, msg, topic, sensor_ts, point_fields=self.point_fields
data = process_ouster_packet(
self.os1_info, self.os1_dict, msg, topic, point_fields=self.point_fields
)
elif topic_type=="sensor_msgs/Image":
data, sensor_ts = process_image(msg)
Expand Down Expand Up @@ -417,4 +418,4 @@ def load_cam_calibrations(outdir, trajectory):
cam_calibrations[cal_id]['cam_id'] = cam_id

cam_calibrations[cal_id].update(cal)
return cam_calibrations
return cam_calibrations
11 changes: 3 additions & 8 deletions helpers/sensors.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ def read_ouster_info(os_metadata):
return os1_dict, os1_info

def process_ouster_packet(
os1_info, os1_dict, packet_arr, topic, sensor_ts, point_fields="x y z i t"
os1_info, os1_dict, packet_arr, topic, point_fields="x y z i t"
):
def nth(iterable, n, default=None):
try:
Expand Down Expand Up @@ -102,12 +102,7 @@ def nth(iterable, n, default=None):
raise NotImplementedError
pc = pc.astype(np.float32)

# Interpolate original point cloud timestamp according ouster SDK
# https://github.com/ouster-lidar/ouster-ros/blob/5a08f89e21dabfb78ae359f9bfc4ba869a275104/src/lidar_packet_handler.h#L151
delta_ts = max(0, (ts[-1] - init_ts)*1e-9) # in seconds
received_ts = rospy.Time(sensor_ts.to_sec() - delta_ts)

return pc, received_ts
return pc

def set_filename_by_topic(topic, sensor_subpath, filetype, trajectory, frame):
sensor_prefix = sensor_subpath.replace("/", "_") #get sensor name
Expand Down Expand Up @@ -328,4 +323,4 @@ def process_gps(gps_data):
if gps_data.status==-1:
return None, sensor_ts

return gps_data, sensor_ts
return gps_data, sensor_ts