def _reset(self, car_model_state):
camera_model_state = ModelState()
camera_model_state.model_name = self.topic_name
# 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])
# Calculate camera rotation quaternion based on lookAt yaw with respect to
# current camera position and car position
look_at_yaw = utils.get_angle_between_two_points_2d_rad(
Point(target_camera_location_2d[0], target_camera_location_2d[1]),
car_model_state.pose.position)
cam_quaternion = euler_to_quaternion(pitch=self.look_down_angle_rad,
yaw=look_at_yaw)
camera_model_state.pose.position.x = target_camera_location_2d[0]
camera_model_state.pose.position.y = target_camera_location_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
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