def _update(self, delta_time):
"""
Update blink effect
Args:
delta_time (float): the change of time in second from last call
"""
if self.current_duration < self.duration:
cur_alpha = lerp(self.source_alpha, self.target_alpha,
self.current_interval / self.interval)
transparencies = [
1.0 - cur_alpha for _ in self.orig_visuals.transparencies
]
for visual_name, link_name, transparency in zip(
self.orig_visuals.visual_names,
self.orig_visuals.link_names, transparencies):
SetVisualTransparencyTracker.get_instance(
).set_visual_transparency(visual_name, link_name, transparency)
self.current_interval += delta_time
if self.current_interval >= self.interval:
temp = self.source_alpha
self.source_alpha = self.target_alpha
self.target_alpha = temp
self.current_interval = 0.0
self.current_duration += delta_time
else:
self.detach()
def _update(self, car_model_state, delta_time):
# Calculate target Camera position based on nearest center track from the car.
near_dist = self.track_data._center_line_.project(
Point(car_model_state.pose.position.x,
car_model_state.pose.position.y))
near_pnt_ctr = self.track_data._center_line_.interpolate(near_dist)
yaw = self.track_data._center_line_.interpolate_yaw(
distance=near_dist,
normalized=False,
reverse_dir=False,
position=near_pnt_ctr)
yaw = utils.lerp_angle_rad(self.last_yaw, yaw,
delta_time * self.damping)
quaternion = np.array(
euler_to_quaternion(pitch=self.look_down_angle_rad, yaw=yaw))
target_camera_location = np.array([near_pnt_ctr.x,
near_pnt_ctr.y,
0.0]) + \
apply_orientation(quaternion, np.array([-self.cam_dist_offset, 0, 0]))
target_camera_location_2d = target_camera_location[0:2]
# Linear interpolate Camera position to target position
cur_camera_2d_pos = np.array([
self.last_camera_state.pose.position.x,
self.last_camera_state.pose.position.y
])
new_cam_pos_2d = utils.lerp(cur_camera_2d_pos,
target_camera_location_2d,
delta_time * self.damping)
# Calculate camera rotation quaternion based on lookAt yaw
look_at_yaw = utils.get_angle_between_two_points_2d_rad(
self.last_camera_state.pose.position,
car_model_state.pose.position)
cam_quaternion = euler_to_quaternion(pitch=self.look_down_angle_rad,
yaw=look_at_yaw)
# Configure Camera Model State
camera_model_state = ModelState()
camera_model_state.model_name = self.topic_name
camera_model_state.pose.position.x = new_cam_pos_2d[0]
camera_model_state.pose.position.y = new_cam_pos_2d[1]
camera_model_state.pose.position.z = self.cam_fixed_height
camera_model_state.pose.orientation.x = cam_quaternion[0]
camera_model_state.pose.orientation.y = cam_quaternion[1]
camera_model_state.pose.orientation.z = cam_quaternion[2]
camera_model_state.pose.orientation.w = cam_quaternion[3]
self.model_state_client(camera_model_state)
self.last_camera_state = camera_model_state
self.last_yaw = yaw
def _update(self, car_model_state, delta_time):
# Calculate target Camera position based on car position
_, _, car_yaw = quaternion_to_euler(
x=car_model_state.pose.orientation.x,
y=car_model_state.pose.orientation.y,
z=car_model_state.pose.orientation.z,
w=car_model_state.pose.orientation.w)
# Linear Interpolate Yaw angle
car_yaw = utils.lerp_angle_rad(self.last_yaw, car_yaw,
delta_time * self.damping)
target_cam_quaternion = np.array(
euler_to_quaternion(pitch=self.look_down_angle_rad, yaw=car_yaw))
target_camera_location = np.array([car_model_state.pose.position.x,
car_model_state.pose.position.y,
0.0]) + \
apply_orientation(target_cam_quaternion, np.array([-self.cam_dist_offset, 0, 0]))
target_camera_location_2d = target_camera_location[0:2]
# Linear interpolate Camera position to target position
cur_camera_2d_pos = np.array([
self.last_camera_state.pose.position.x,
self.last_camera_state.pose.position.y
])
new_cam_pos_2d = utils.lerp(cur_camera_2d_pos,
target_camera_location_2d,
delta_time * self.damping)
# Calculate camera rotation quaternion based on lookAt yaw
look_at_yaw = utils.get_angle_between_two_points_2d_rad(
self.last_camera_state.pose.position,
car_model_state.pose.position)
cam_quaternion = euler_to_quaternion(pitch=self.look_down_angle_rad,
yaw=look_at_yaw)
# Configure Camera Model State
camera_model_state = ModelState()
camera_model_state.model_name = self.topic_name
camera_model_state.pose.position.x = new_cam_pos_2d[0]
camera_model_state.pose.position.y = new_cam_pos_2d[1]
camera_model_state.pose.position.z = self.cam_fixed_height
camera_model_state.pose.orientation.x = cam_quaternion[0]
camera_model_state.pose.orientation.y = cam_quaternion[1]
camera_model_state.pose.orientation.z = cam_quaternion[2]
camera_model_state.pose.orientation.w = cam_quaternion[3]
self.model_state_client(camera_model_state)
self.last_camera_state = camera_model_state
self.last_yaw = car_yaw
def _update(self, car_model_state, delta_time):
# Calculate target Camera position based on nearest center track from the car.
# 1. Project the car position to 1-d global distance of track
# 2. Minus track with offset and get the point of center track
near_dist = self.track_data._center_line_.project(
Point(car_model_state.pose.position.x,
car_model_state.pose.position.y))
near_pnt_ctr = self.track_data._center_line_.interpolate(
near_dist - self.cam_dist_offset)
target_camera_location_2d = np.array([near_pnt_ctr.x, near_pnt_ctr.y])
# Linear interpolate Camera position to target position
cur_camera_2d_pos = np.array([
self.last_camera_state.pose.position.x,
self.last_camera_state.pose.position.y
])
new_cam_pos_2d = utils.lerp(cur_camera_2d_pos,
target_camera_location_2d,
delta_time * self.damping)
# Calculate camera rotation quaternion based on lookAt yaw
look_at_yaw = utils.get_angle_between_two_points_2d_rad(
self.last_camera_state.pose.position,
car_model_state.pose.position)
cam_quaternion = euler_to_quaternion(pitch=self.look_down_angle_rad,
yaw=look_at_yaw)
# Configure Camera Model State
camera_model_state = ModelState()
camera_model_state.model_name = self.topic_name
camera_model_state.pose.position.x = new_cam_pos_2d[0]
camera_model_state.pose.position.y = new_cam_pos_2d[1]
camera_model_state.pose.position.z = self.cam_fixed_height
camera_model_state.pose.orientation.x = cam_quaternion[0]
camera_model_state.pose.orientation.y = cam_quaternion[1]
camera_model_state.pose.orientation.z = cam_quaternion[2]
camera_model_state.pose.orientation.w = cam_quaternion[3]
self.model_state_client(camera_model_state)
self.last_camera_state = camera_model_state