def get_point(a, b, ratio):
"""get point from a to b with ratio"""
p = Point3D()
p.x = a[0] + ratio * (b[0] - a[0])
p.y = a[1] + ratio * (b[1] - a[1])
p.z = a[2] + ratio * (b[2] - a[2])
return p
def generate_polygon(point, heading, length, width):
"""
Generate polygon
"""
points = []
half_l = length / 2.0
half_w = width / 2.0
sin_h = math.sin(heading)
cos_h = math.cos(heading)
vectors = [(half_l * cos_h - half_w * sin_h,
half_l * sin_h + half_w * cos_h),
(-half_l * cos_h - half_w * sin_h,
- half_l * sin_h + half_w * cos_h),
(-half_l * cos_h + half_w * sin_h,
- half_l * sin_h - half_w * cos_h),
(half_l * cos_h + half_w * sin_h,
half_l * sin_h - half_w * cos_h)]
for x, y in vectors:
p = Point3D()
p.x = point.x + x
p.y = point.y + y
p.z = point.z
points.append(p)
return points
def get_velocity(theta, speed):
"""get velocity from theta and speed"""
point = Point3D()
point.x = math.cos(theta) * speed
point.y = math.sin(theta) * speed
point.z = 0.0
return point
def process(self, message):
"""Processing radar message."""
if not check_stationary_pole(self._pose, self._is_primary):
return
radar = ContiRadar()
radar.ParseFromString(message)
transform = get_world_coordinate(self._transform, self._pose)
for point in radar.contiobs:
point3d = Point3D()
point3d.x = point.longitude_dist
point3d.y = point.lateral_dist
point3d.z = 0
convert_to_world_coordinate(point3d, transform, None)
radar_point = frame_pb2.RadarPoint()
radar_point.type = self._type
radar_point.position.x = point3d.x
radar_point.position.y = point3d.y
radar_point.position.z = point3d.z
#radar_point.position.z = 0
point3d.x = point.longitude_dist + point.longitude_vel
point3d.y = point.lateral_dist + point.lateral_vel
point3d.z = 0
convert_to_world_coordinate(point3d, transform, None)
radar_point.direction.x = point3d.x - radar_point.position.x
radar_point.direction.y = point3d.y - radar_point.position.y
radar_point.direction.z = point3d.z - radar_point.position.z
#radar_point.direction.z = 0
self._current_list.append(radar_point)
if len(self._current_list) > self._accumulated_num:
self._current_list.pop(0)
def initialize_transform(self):
if self._extrinsics is None:
return
q = Quaternion()
q.qw = self._extrinsics['transform']['rotation']['w']
q.qx = self._extrinsics['transform']['rotation']['x']
q.qy = self._extrinsics['transform']['rotation']['y']
q.qz = self._extrinsics['transform']['rotation']['z']
d = Point3D()
d.x = self._extrinsics['transform']['translation']['x']
d.y = self._extrinsics['transform']['translation']['y']
d.z = self._extrinsics['transform']['translation']['z']
self._transform = generate_transform(q, d)
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 offer_data(self, frame):
"""Provide data to current frame."""
for vector4 in self._current_list:
point = frame.points.add()
point.CopyFrom(vector4)
if not self._is_primary:
return
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)
frame.device_position.x = point.x
frame.device_position.y = point.y
frame.device_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)
frame.device_heading.x = q.qx
frame.device_heading.y = q.qy
frame.device_heading.z = q.qz
frame.device_heading.w = q.qw
