def process(self, message):
"""Processing image message."""
if not check_stationary_pole(self._pose, self._is_primary):
return
image = CompressedImage()
image.ParseFromString(message)
camera_image = frame_pb2.CameraImage()
camera_image.timestamp = image.header.timestamp_sec
dump_img_bin(image.data, self._output_dir, ImageSensor._image_seq)
camera_image.image_url = '{}/{}/pic-{}.jpg'.format(
self._image_url, os.path.basename(self._output_dir),
ImageSensor._image_seq)
ImageSensor._image_seq += 1
camera_image.k1 = self._intrinsics['D'][0]
camera_image.k2 = self._intrinsics['D'][1]
camera_image.k3 = self._intrinsics['D'][4]
camera_image.p1 = self._intrinsics['D'][2]
camera_image.p2 = self._intrinsics['D'][3]
camera_image.skew = self._intrinsics['K'][1]
camera_image.fx = self._intrinsics['K'][0]
camera_image.fy = self._intrinsics['K'][4]
camera_image.cx = self._intrinsics['K'][2]
camera_image.cy = self._intrinsics['K'][5]
point = Point3D()
point.x = 0
point.y = 0
point.z = 0
transform = get_world_coordinate(self._transform, self._pose)
convert_to_world_coordinate(point, transform, None)
camera_image.position.x = point.x
camera_image.position.y = point.y
camera_image.position.z = point.z
rotation = get_rotation_from_tranform(transform)
q = rotation_to_quaternion(rotation)
# TODO: either apply it to all or do not apply it
#q = apply_scale_rotation(q)
camera_image.heading.x = q.qx
camera_image.heading.y = q.qy
camera_image.heading.z = q.qz
camera_image.heading.w = q.qw
camera_image.channel = self._channel
self._current_list.append(camera_image)
if len(self._current_list) > self._accumulated_num:
self._current_list.pop(0)
def sensor_data_updated(self, carla_image):
"""
Function (override) to transform the received carla image data
into a ROS image message
:param carla_image: carla image object
:type carla_image: carla.Image
"""
if ((carla_image.height != int(
self.carla_actor.attributes['image_size_y']))
or (carla_image.width != int(
self.carla_actor.attributes['image_size_x']))):
logging.error(
"Camera{} received image not matching configuration".format(
self.topic_name()))
image_data_array, encoding = self.get_carla_image_data_array(
carla_image=carla_image)
cyber_img = CompressedImage()
cyber_img.header.CopyFrom(self.parent.get_cyber_header())
cyber_img.header.frame_id = self.carla_actor.attributes['role_name']
cyber_img.frame_id = cyber_img.header.frame_id
cyber_img.format = 'jpeg'
cyber_img.measurement_time = cyber_img.header.timestamp_sec
cyber_img.data = cv2.imencode('.jpg', image_data_array)[1].tostring()
self.write_cyber_message(
'/apollo/sensor/camera/%s/image/compressed' %
self.carla_actor.attributes['role_name'], cyber_img)
def parse_data(channelname, msg, out_folder):
"""
parser images from Apollo record file
"""
msg_camera = CompressedImage()
msg_camera.ParseFromString(str(msg))
tstamp = msg_camera.measurement_time
temp_time = str(tstamp).split('.')
if len(temp_time[1]) == 1:
temp_time1_adj = temp_time[1] + '0'
else:
temp_time1_adj = temp_time[1]
image_time = temp_time[0] + '_' + temp_time1_adj
image_filename = "image_" + image_time + ".jpeg"
f = open(out_folder + image_filename, 'w+b')
f.write(msg_camera.data)
f.close()
return tstamp
def image_message_to_proto(py_message):
"""Message to prototype"""
message_proto = CompressedImage()
message_proto.ParseFromString(py_message.message)
return message_proto