def __get_ros_publishers(self, reader):
topics_types = RosUtil.get_topics_types(reader)
for ros_topic, data_type in topics_types.items():
if not ros_topic in self.ros_publishers:
ros_data_class = RosUtil.get_data_class(data_type)
self.ros_publishers[ros_topic] = rospy.Publisher(ros_topic, ros_data_class, queue_size=64)
time.sleep(1)
def __publish_msgs(self, bag_path):
reader = rosbag.Bag(bag_path)
self.__get_ros_publishers(reader)
for ros_topic, ros_msg, _ in reader.read_messages():
if self.use_time == "received":
RosUtil.set_ros_msg_received_time(ros_msg)
self.ros_publishers[ros_topic].publish(ros_msg)
reader.close()
def __process_s3_pcl_files(self,
ros_topic=None,
files=None,
resp=None,
frame_id=None,
s3_client=None,
lidar_view=None,
vehicle_transform_matrix=None):
for f in files:
try:
path = resp.get(block=True).split(" ", 1)[0]
npz = np.load(path)
pcl_ts = int(f[2])
points, reflectance = RosUtil.parse_pcl_npz(
npz=npz,
lidar_view=lidar_view,
vehicle_transform_matrix=vehicle_transform_matrix)
if not np.isnan(points).any():
self.__record_pcl_msg(ros_topic=ros_topic,
points=points,
reflectance=reflectance,
pcl_ts=pcl_ts,
frame_id=frame_id,
s3_client=s3_client)
os.remove(path)
except BaseException as _:
exc_type, exc_value, exc_traceback = sys.exc_info()
traceback.print_tb(exc_traceback, limit=20, file=sys.stdout)
self.logger.error(str(exc_type))
self.logger.error(str(exc_value))
def __record_bus(self, manifest=None, ros_topic=None, frame_id=None):
self.bus_topic = ros_topic
while True:
rows = None
while not rows and manifest.is_open():
rows = manifest.fetch()
if not rows:
break
for row in rows:
try:
ros_msg = RosUtil.bus_msg(row=row, frame_id=frame_id)
self.__record_ros_msg(topic=ros_topic,
msg=ros_msg,
ts=ros_msg.header.stamp)
self.__topic_sleep(ros_topic=ros_topic)
except BaseException as _:
exc_type, exc_value, exc_traceback = sys.exc_info()
traceback.print_tb(exc_traceback,
limit=20,
file=sys.stdout)
self.logger.error(str(exc_type))
self.logger.error(str(exc_value))
if self.request['preview']:
break
self.topic_active[ros_topic] = False
def __record_image_msg(self,
ros_topic=None,
image=None,
image_ts=None,
frame_id=None,
s3_client=None):
try:
ros_image_msg = self.img_cv_bridge.cv2_to_imgmsg(image)
RosUtil.set_ros_msg_header(ros_msg=ros_image_msg,
ts=image_ts,
frame_id=frame_id)
self.__record_ros_msg(topic=ros_topic,
msg=ros_image_msg,
ts=ros_image_msg.header.stamp,
s3_client=s3_client)
self.__topic_sleep(ros_topic=ros_topic)
except BaseException as _:
exc_type, exc_value, exc_traceback = sys.exc_info()
traceback.print_tb(exc_traceback, limit=20, file=sys.stdout)
self.logger.error(str(exc_type))
self.logger.error(str(exc_value))
def __record_images_from_fs(self,
manifest=None,
ros_topic=None,
sensor=None,
frame_id=None):
image_reader = dict()
image_data = dict()
image_ts = dict()
image_request = self.request.get("image", "original")
if image_request == "undistorted":
lens, dist_parms, intr_mat_dist, intr_mat_undist = self.__get_camera_info(
sensor=sensor)
while True:
files = None
while not files and manifest.is_open():
files = manifest.fetch()
if not files:
break
count = read_images_from_fs(data_store=self.data_store,
files=files,
image_reader=image_reader,
image_data=image_data,
image_ts=image_ts)
for i in range(0, count):
image_reader[i].join()
if image_request == "undistorted":
image = RosUtil.undistort_image(
image=image_data[i],
lens=lens,
dist_parms=dist_parms,
intr_mat_dist=intr_mat_dist,
intr_mat_undist=intr_mat_undist)
else:
image = image_data[i]
self.__record_image_msg(ros_topic=ros_topic,
image=image,
image_ts=image_ts[i],
frame_id=frame_id)
if self.request['preview']:
break
self.topic_active[ros_topic] = False
def __record_pcl_from_fs(self,
manifest=None,
ros_topic=None,
sensor=None,
frame_id=None):
pcl_reader = dict()
pcl_ts = dict()
npz = dict()
lidar_view = self.request.get("lidar_view", "camera")
vehicle_transform_matrix = self.__sensor_to_vehicle_matrix(
sensor=sensor) if lidar_view == "vehicle" else None
while True:
files = None
while not files and manifest.is_open():
files = manifest.fetch()
if not files:
break
count = read_pcl_from_fs(data_store=self.data_store,
files=files,
pcl_reader=pcl_reader,
pcl_ts=pcl_ts,
npz=npz)
for i in range(0, count):
pcl_reader[i].join()
points, reflectance = RosUtil.parse_pcl_npz(
npz=npz[i],
lidar_view=lidar_view,
vehicle_transform_matrix=vehicle_transform_matrix)
if not np.isnan(points).any():
self.__record_pcl_msg(ros_topic=ros_topic,
points=points,
reflectance=reflectance,
pcl_ts=pcl_ts[i],
frame_id=frame_id)
if self.request['preview']:
break
self.topic_active[ros_topic] = False
def __record_pcl_msg(self,
ros_topic=None,
points=None,
reflectance=None,
pcl_ts=None,
frame_id=None,
s3_client=None):
try:
ros_pcl_msg = RosUtil.pcl_dense_msg(points=points,
reflectance=reflectance,
ts=pcl_ts,
frame_id=frame_id)
self.__record_ros_msg(topic=ros_topic,
msg=ros_pcl_msg,
ts=ros_pcl_msg.header.stamp,
s3_client=s3_client)
self.__topic_sleep(ros_topic=ros_topic)
except BaseException as _:
exc_type, exc_value, exc_traceback = sys.exc_info()
traceback.print_tb(exc_traceback, limit=20, file=sys.stdout)
self.logger.error(str(exc_type))
self.logger.error(str(exc_value))
def __process_s3_image_files(self,
ros_topic=None,
files=None,
resp=None,
frame_id=None,
s3_client=None,
image_request=None,
lens=None,
dist_parms=None,
intr_mat_dist=None,
intr_mat_undist=None):
for f in files:
try:
path = resp.get(block=True).split(" ", 1)[0]
image_data = cv2.imread(path)
if image_request == "undistorted":
image = RosUtil.undistort_image(
image_data,
lens=lens,
dist_parms=dist_parms,
intr_mat_dist=intr_mat_dist,
intr_mat_undist=intr_mat_undist)
else:
image = image_data
image_ts = int(f[2])
self.__record_image_msg(ros_topic=ros_topic,
image=image,
image_ts=image_ts,
frame_id=frame_id,
s3_client=s3_client)
os.remove(path)
except BaseException as _:
exc_type, exc_value, exc_traceback = sys.exc_info()
traceback.print_tb(exc_traceback, limit=20, file=sys.stdout)
self.logger.error(str(exc_type))
self.logger.error(str(exc_value))