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_)