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_pose):
"""
Update the frustums
Args:
car_pose (Pose): Gazebo Pose of the agent
"""
with self.lock:
self.frustums = []
self.cam_poses = []
self.near_plane_infos = []
# Retrieve car pose to calculate camera pose.
car_pos = np.array([car_pose.position.x, car_pose.position.y,
car_pose.position.z])
car_quaternion = [car_pose.orientation.x,
car_pose.orientation.y,
car_pose.orientation.z,
car_pose.orientation.w]
for camera_offset in self.camera_offsets:
# Get camera position by applying position offset from the car position.
cam_pos = car_pos + apply_orientation(car_quaternion, camera_offset)
# Get camera rotation by applying car rotation and pitch angle of camera.
_, _, yaw = quaternion_to_euler(x=car_quaternion[0],
y=car_quaternion[1],
z=car_quaternion[2],
w=car_quaternion[3])
cam_quaternion = np.array(euler_to_quaternion(pitch=self.camera_pitch, yaw=yaw))
# Calculate frustum with camera position and rotation.
planes, cam_pose, near_plane_info = self._calculate_frustum_planes(cam_pos=cam_pos,
cam_quaternion=cam_quaternion)
self.frustums.append(planes)
self.cam_poses.append(cam_pose)
self.near_plane_infos.append(near_plane_info)
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_temp(self):
with self.lock:
self.frustums = []
self.cam_poses = []
self.near_plane_infos = []
car_model_state = self.model_state_client('racecar', '')
car_pos = np.array([
car_model_state.pose.position.x,
car_model_state.pose.position.y,
car_model_state.pose.position.z
])
car_quaternion = [
car_model_state.pose.orientation.x,
car_model_state.pose.orientation.y,
car_model_state.pose.orientation.z,
car_model_state.pose.orientation.w
]
for camera_offset in self.camera_offsets:
cam_pos = car_pos + apply_orientation(car_quaternion,
camera_offset)
_, _, yaw = quaternion_to_euler(x=car_quaternion[0],
y=car_quaternion[1],
z=car_quaternion[2],
w=car_quaternion[3])
cam_quaternion = np.array(
euler_to_quaternion(pitch=self.camera_pitch, yaw=yaw))
planes, cam_pose, near_plane_info = self._calculate_frustum_planes(
cam_pos=cam_pos, cam_quaternion=cam_quaternion)
self.frustums.append(planes)
self.cam_poses.append(cam_pose)
self.near_plane_infos.append(near_plane_info)
def _compute_bot_car_poses(self):
'''Compute bot car poses
Returns:
list: list of bot car Pose instance
'''
# Evaluate the splines
with self.lock:
bot_cars_spline_derivs = \
[spline.eval_spline(self._get_dist_from_sim_time(initial_dist,
self.current_sim_time))
for initial_dist, spline in zip(self.bot_cars_initial_dists,
self.bot_cars_trajectories)]
# Compute the bot car poses
bot_car_poses = []
for bot_car_spline_derivs in bot_cars_spline_derivs:
bot_car_x, bot_car_y = bot_car_spline_derivs[0][0], bot_car_spline_derivs[1][0]
bot_car_dx, bot_car_dy = bot_car_spline_derivs[0][1], bot_car_spline_derivs[1][1]
bot_car_yaw = math.atan2(bot_car_dy, bot_car_dx)
bot_car_orientation = euler_to_quaternion(yaw=bot_car_yaw, pitch=0, roll=0)
bot_car_pose = Pose()
bot_car_pose.position.x = bot_car_x
bot_car_pose.position.y = bot_car_y
bot_car_pose.position.z = BOT_CAR_Z
bot_car_pose.orientation.x = bot_car_orientation[0]
bot_car_pose.orientation.y = bot_car_orientation[1]
bot_car_pose.orientation.z = bot_car_orientation[2]
bot_car_pose.orientation.w = bot_car_orientation[3]
bot_car_poses.append(bot_car_pose)
return bot_car_poses
def project_to_2d(point_on_plane, plane_center, plane_width, plane_height, plane_quaternion):
if not isinstance(point_on_plane, list) and not isinstance(point_on_plane, tuple) \
and not isinstance(point_on_plane, np.ndarray):
raise GenericRolloutException("point_on_plane must be a type of list, tuple, or numpy.ndarray")
if not isinstance(plane_center, list) and not isinstance(plane_center, tuple) \
and not isinstance(plane_center, np.ndarray):
raise GenericRolloutException("plane_center must be a type of list, tuple, or numpy.ndarray")
if not isinstance(plane_quaternion, list) and not isinstance(plane_quaternion, tuple) \
and not isinstance(plane_quaternion, np.ndarray):
raise GenericRolloutException("plane_quaternion must be a type of list, tuple, or numpy.ndarray")
point_on_plane = np.array(point_on_plane)
plane_center = np.array(plane_center)
plane_quaternion = np.array(plane_quaternion)
# Transpose the center back to origin
point_on_plane_from_origin = point_on_plane - plane_center
# Reverse the rotation so plane can align back to y-axis
inverse_cam_quaternion = inverse_quaternion(plane_quaternion)
point_on_y_axis = apply_orientation(inverse_cam_quaternion, point_on_plane_from_origin)
# Rotate pitch 90 degree and yaw 90 degree, so plane will align to x and y axis
# Remember rotation order is roll, pitch, yaw in euler_to_quaternion method
project_2d_quaternion = euler_to_quaternion(pitch=np.pi / 2.0, yaw=np.pi / 2.0)
point_on_2d_plane = apply_orientation(np.array(project_2d_quaternion), point_on_y_axis)
# Align plane to origin at x, y = (0, 0)
point_on_2d_plane = point_on_2d_plane + np.array([plane_width / 2.0, plane_height / 2.0, 0.0])
# Re-scale x and y space between 0 and 1
return (point_on_2d_plane[0] / plane_width), (point_on_2d_plane[1] / plane_height)
def _construct_model_pose(self, model_position, yaw):
car_pose = Pose()
orientation = euler_to_quaternion(yaw=yaw)
car_pose.position.x = model_position[0]
car_pose.position.y = model_position[1]
car_pose.position.z = 0.0
car_pose.orientation.x = orientation[0]
car_pose.orientation.y = orientation[1]
car_pose.orientation.z = orientation[2]
car_pose.orientation.w = orientation[3]
return car_pose
def _get_initial_camera_pose(self, car_pose):
# get the bounds
x_min, y_min, x_max, y_max = self.track_data.outer_border.bounds
# update camera position
model_pose = Pose()
model_pose.position.x = (x_min+x_max) / 2.0
model_pose.position.y = (y_min+y_max) / 2.0
model_pose.position.z = CAMERA_HEIGHT
x, y, z, w = euler_to_quaternion(roll=1.57079, pitch=1.57079, yaw=3.14159)
model_pose.orientation.x = x
model_pose.orientation.y = y
model_pose.orientation.z = z
model_pose.orientation.w = w
return model_pose
def interpolate_pose(
self,
distance,
normalized=False,
finite_difference=FiniteDifference.CENTRAL_DIFFERENCE):
pose = Pose()
position = self.interpolate(distance, normalized)
yaw = self.interpolate_yaw(distance, normalized, position,
finite_difference)
orientation = euler_to_quaternion(yaw=yaw)
pose.position.x = position.x
pose.position.y = position.y
pose.position.z = 0.0
pose.orientation.x = orientation[0]
pose.orientation.y = orientation[1]
pose.orientation.z = orientation[2]
pose.orientation.w = orientation[3]
return pose
def _get_initial_camera_pose(self, car_model_state):
_, _, 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)
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]))
camera_model_pose = Pose()
camera_model_pose.position.x = target_camera_location[0]
camera_model_pose.position.y = target_camera_location[1]
camera_model_pose.position.z = self.cam_fixed_height
camera_model_pose.orientation.x = target_cam_quaternion[0]
camera_model_pose.orientation.y = target_cam_quaternion[1]
camera_model_pose.orientation.z = target_cam_quaternion[2]
camera_model_pose.orientation.w = target_cam_quaternion[3]
self.last_yaw = car_yaw
return camera_model_pose
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
def _park_car_model(self):
'''Park agent after racer complete F1 race
'''
car_model_state = ModelState()
car_model_state.model_name = self._agent_name_
orientation = euler_to_quaternion(yaw=0.0)
car_model_state.pose.position.x = self._park_position[0]
car_model_state.pose.position.y = self._park_position[1]
car_model_state.pose.position.z = 0.0
car_model_state.pose.orientation.x = orientation[0]
car_model_state.pose.orientation.y = orientation[1]
car_model_state.pose.orientation.z = orientation[2]
car_model_state.pose.orientation.w = orientation[3]
car_model_state.twist.linear.x = 0
car_model_state.twist.linear.y = 0
car_model_state.twist.linear.z = 0
car_model_state.twist.angular.x = 0
car_model_state.twist.angular.y = 0
car_model_state.twist.angular.z = 0
SetModelStateTracker.get_instance().set_model_state(car_model_state)
self.camera_manager.reset(car_pose=car_model_state.pose,
namespace=self._agent_name_)
