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