def _store_datapoint(self, data: Union[Odometry, TwistStamped], label: str = "observed"): if not self.show_graph: return if isinstance(data, Odometry): # Note that pose is expressed in global frame ==> rotate to rotated global frame # While velocity is expressed in rotated global frame following yaw. stamp = float(data.header.stamp.to_sec()) _, _, yaw = euler_from_quaternion((data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w)) rotated_position = transform(points=[data.pose.pose.position], orientation=self.pose_est.pose.orientation, invert=True)[0] self.data['pose']['x'][label].append((stamp, rotated_position.x)) self.data['pose']['y'][label].append((stamp, rotated_position.y)) self.data['pose']['z'][label].append((stamp, rotated_position.z)) self.data['pose']['yaw'][label].append((stamp, yaw)) self.data['velocity']['x'][label].append((stamp, data.twist.twist.linear.x)) self.data['velocity']['y'][label].append((stamp, data.twist.twist.linear.y)) self.data['velocity']['z'][label].append((stamp, data.twist.twist.linear.z)) self.data['velocity']['yaw'][label].append((stamp, data.twist.twist.angular.z)) elif isinstance(data, TwistStamped): stamp = float(data.header.stamp.to_sec()) if len(self.data['command']['x']['observed']) > 1 and \ abs(stamp - self.data['command']['x']['observed'][-1][0]) < 0.1: return self.data['command']['x'][label].append((stamp, data.twist.linear.x)) self.data['command']['y'][label].append((stamp, data.twist.linear.y)) self.data['command']['z'][label].append((stamp, data.twist.linear.z)) self.data['command']['yaw'][label].append((stamp, data.twist.angular.z))
def _reference_update(self, pose_ref: PointStamped): if pose_ref.header.frame_id == "agent": cprint(f'got pose_ref in agent frame: {pose_ref.point}', self._logger) # TODO: # Use time stamp of message to project reference point to global frame # corresponding to the agent's pose at that time # Although, as references should be said once per second the small delay probably won't matter that much. # transform from agent to world frame. # Note that agent frame is not necessarily the same as agent_horizontal, so a small error is induced here # as pose_estimate.orientation only incorporates yaw turn because KF does not maintain roll and pitch. pose_ref.point = transform(points=[pose_ref.point], orientation=self.pose_est.pose.orientation, translation=self.pose_est.pose.position)[0] pose_ref.header.frame_id = "global" cprint(f'mapped to global frame: {pose_ref.point}', self._logger) if pose_ref != self.pose_ref: # reset pose error when new reference comes in. self.prev_pose_error = PointStamped(header=Header(frame_id="global")) self.prev_vel_error = PointStamped(header=Header(frame_id="global_rotated")) self.last_cmd = TwistStamped(header=Header(stamp=rospy.Time().now())) self.pose_ref = pose_ref _, _, yaw = euler_from_quaternion(self.pose_est.pose.orientation) self.desired_yaw = yaw + calculate_relative_orientation(robot_pose=self.pose_est, reference_pose=self.pose_ref) cprint(f'set pose_ref: {self.pose_ref.point}', self._logger)
def _feedback(self): '''Whenever the target is reached, apply position feedback to the desired end position to remain in the correct spot and compensate for drift. Tustin discretized PID controller for x and y, PI for z. Returns a twist containing the control command. ''' cmd = Twist() if self.pose_ref is None: return cmd # PID prev_pose_error = self.prev_pose_error pose_error = PointStamped() pose_error.point.x = (self.pose_ref.point.x - self.pose_est.pose.position.x) pose_error.point.y = (self.pose_ref.point.y - self.pose_est.pose.position.y) pose_error.point.z = (self.pose_ref.point.z - self.pose_est.pose.position.z) pose_error.point = transform(points=[pose_error.point], orientation=self.pose_est.pose.orientation, invert=True)[0] pose_error.header.frame_id = "global_rotated" prev_vel_error = self.prev_vel_error vel_error = PointStamped() vel_error.header.frame_id = "global_rotated" vel_error.point.x = self.vel_ref.twist.linear.x - self.vel_est.point.x vel_error.point.y = self.vel_ref.twist.linear.y - self.vel_est.point.y # calculations happen in global_rotated frame cmd.linear.x = max(-self.max_input, min(self.max_input, ( self.last_cmd.twist.linear.x + (self.Kp_x + self.Ki_x * self._control_period / 2) * pose_error.point.x + (-self.Kp_x + self.Ki_x * self._control_period / 2) * prev_pose_error.point.x + self.Kd_x * (vel_error.point.x - prev_vel_error.point.x)))) cmd.linear.y = max(-self.max_input, min(self.max_input, ( self.last_cmd.twist.linear.y + (self.Kp_y + self.Ki_y * self._control_period / 2) * pose_error.point.y + (-self.Kp_y + self.Ki_y * self._control_period / 2) * prev_pose_error.point.y + self.Kd_y * (vel_error.point.y - prev_vel_error.point.y)))) cmd.linear.z = max(-self.max_input, min(self.max_input, ( self.last_cmd.twist.linear.z + (self.Kp_z + self.Ki_z * self._control_period / 2) * pose_error.point.z + (-self.Kp_z + self.Ki_z * self._control_period / 2) * prev_pose_error.point.z + self.Kd_z * (vel_error.point.z - prev_vel_error.point.z)))) # Added derivative term # if target is more than 1m away, look in that direction _, _, yaw = euler_from_quaternion(self.pose_est.pose.orientation) if self.desired_yaw is not None: # in case there is a desired yaw angle_error = ((((self.desired_yaw - yaw) - np.pi) % (2*np.pi)) - np.pi) K_theta = self.K_theta + (np.pi - abs(angle_error))/np.pi*0.2 cmd.angular.z = (K_theta * angle_error) self.prev_pose_error = pose_error self.prev_vel_error = vel_error return cmd
def _correct_step_calc(self, measurement: Odometry, x_hat_rot_tmeas: np.ndarray, error_cov_hat_tmeas: np.ndarray): """ Correction step of the kalman filter. Update the position of the drone in world_yaw frame using the measurements. Note that timings between measurment, state and covariance matrix should be aligned. Argument: - measurement = Odometry expressed in world frame. - x_hat_rot_tmeas = state at measurement time - error_cov_hat_tmeas = error covariance at measurement time Returns: """ _, _, yaw = euler_from_quaternion((measurement.pose.pose.orientation.x, measurement.pose.pose.orientation.y, measurement.pose.pose.orientation.z, measurement.pose.pose.orientation.w)) global_to_rotated = R.from_euler('XYZ', (0, 0, -yaw), degrees=False).as_matrix() position = np.array([[measurement.pose.pose.position.x], [measurement.pose.pose.position.y], [measurement.pose.pose.position.z]]) position_rot = np.matmul(global_to_rotated, position) # assumption: velocity is expressed in global rotated frame => no need to rotate velocity_rot = np.array([[measurement.twist.twist.linear.x], [measurement.twist.twist.linear.y], [measurement.twist.twist.linear.z]]) # output vector contains pose and velocity in global-rotated frame (following drone's yaw) y = np.zeros((8, 1)) y[0:3] = position_rot y[3, 0] = yaw # yaw is kept in global frame y[4:7] = velocity_rot y[7, 0] = measurement.twist.twist.angular.z # angular y yaw velocity is kept in global frame # innovation = what is measured differently from what we would be expected # (note D is zero in y(i+1) = C x(i+1) + D j(i) ) # only use pose to correct, don't correct velocity terms. nu = y - np.matmul(self.C, x_hat_rot_tmeas) # innovation_covariance depends on covariance of prediction Phat (reliability of prediction) # and measurement noise covariance R (reliability of measurement) innovation_covariance = np.matmul(self.C, np.matmul(error_cov_hat_tmeas, np.transpose(self.C))) + self.meas_noise_cov # Kalman gain represents impact of innovation on state estimate, # increases with covariance prediction (unreliable prediction) # decreases with covariance measurement error (unreliable measurement) kalman_gain = np.matmul(error_cov_hat_tmeas, np.matmul(np.transpose(self.C), np.linalg.inv(innovation_covariance))) # update state x_hat_rot_tmeas = x_hat_rot_tmeas + np.matmul(kalman_gain, nu) # decrease state covariance error_cov_hat_tmeas = np.matmul((np.identity(10) - np.matmul(kalman_gain, self.C)), error_cov_hat_tmeas) # get pose and velocity information (note D is zero in y(i+1) = C x(i+1) + D j(i) ) y_hat_rot_tnext = np.matmul(self.C, x_hat_rot_tmeas) return x_hat_rot_tmeas, y_hat_rot_tnext, error_cov_hat_tmeas
def get_pose(self): if rospy.get_param('/robot/position_sensor/topic') in self.ros_topic.topic_values.keys(): odom = self.ros_topic.topic_values[rospy.get_param('/robot/position_sensor/topic')] quaternion = (odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w) _, _, yaw = euler_from_quaternion(quaternion) return odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z, yaw else: return None
def _reset(self): self.data = {title: { axis: {label: [] for label in ['predicted', 'observed', 'adjusted']} for axis in ['x', 'y', 'z', 'yaw']} for title in ['pose', 'velocity', 'command'] } self.prev_pose_error = PointStamped(header=Header(frame_id="global")) self.prev_vel_error = PointStamped(header=Header(frame_id="global_rotated")) self.last_cmd = TwistStamped(header=Header(stamp=rospy.Time().now())) if self.pose_ref is not None: _, _, yaw = euler_from_quaternion(self.pose_est.pose.orientation) self.desired_yaw = yaw + calculate_relative_orientation(robot_pose=self.pose_est, reference_pose=self.pose_ref)
def print_result(result: Odometry, tag: str = ''): if result is None: print('None') else: _, _, yaw = euler_from_quaternion( (result.pose.pose.orientation.x, result.pose.pose.orientation.y, result.pose.pose.orientation.z, result.pose.pose.orientation.w)) print( f'{tag}{result.header.stamp.to_sec()}: pose ({result.pose.pose.position.x}, ' f'{result.pose.pose.position.y}, {result.pose.pose.position.z}, {yaw}), ' f'velocity ({result.twist.twist.linear.x}, {result.twist.twist.linear.y}, {result.twist.twist.linear.z},' f'{result.twist.twist.angular.z})')
def run(self): while not rospy.is_shutdown(): if self._fsm_state == FsmState.Running: twist = self._update_twist() if twist is not None: self.last_cmd = TwistStamped( header=Header(stamp=rospy.Time().now()), twist=twist ) self._store_datapoint(self.last_cmd) self._publisher.publish(twist) self.count += 1 if self.count % 10 * self._rate_fps == 0 and self.pose_est is not None and self.pose_ref is not None: _, _, yaw = euler_from_quaternion(self.pose_est.pose.orientation) msg = f'<<reference: {self.pose_ref.point}, \n<<pose: {self.pose_est.pose.position} \n ' \ f'<<yaw {yaw}, \ncontrol: {self.last_cmd.twist.linear}' cprint(msg, self._logger) rospy.sleep(duration=1/self._rate_fps)
def test_modified_state_publisher(self): self.start() time.sleep(5) y_pos = 3 tracking_pose = get_fake_pose_stamped(1, 2, 3, 0, 0, -0.258819, 0.9659258) fleeing_pose = get_fake_pose_stamped(4, 5, 6) self.ros_topic.publishers[self.tracking_pose_topic].publish( tracking_pose) self.ros_topic.publishers[self.fleeing_pose_topic].publish( fleeing_pose) # wait safely max_duration = 30 stime = time.time() while self.modified_state_topic not in self.ros_topic.topic_values.keys() \ and time.time() - stime < max_duration: time.sleep(0.1) self.assertTrue(time.time() - stime < max_duration) time.sleep(1) roll, pitch, yaw = euler_from_quaternion( (tracking_pose.pose.orientation.x, tracking_pose.pose.orientation.y, tracking_pose.pose.orientation.z, tracking_pose.pose.orientation.w)) modified_state = self.ros_topic.topic_values[self.modified_state_topic] self.assertTrue( modified_state.tracking_x == tracking_pose.pose.position.x) self.assertTrue( modified_state.tracking_y == tracking_pose.pose.position.y) self.assertTrue( modified_state.tracking_z == tracking_pose.pose.position.z) self.assertTrue( modified_state.fleeing_x == fleeing_pose.pose.position.x) self.assertTrue( modified_state.fleeing_y == fleeing_pose.pose.position.y) self.assertTrue( modified_state.fleeing_z == fleeing_pose.pose.position.z) self.assertTrue(modified_state.tracking_roll == roll) self.assertTrue(modified_state.tracking_pitch == pitch) self.assertAlmostEqual(modified_state.tracking_yaw, yaw)
def _process_odometry(self, msg: Odometry, args: tuple) -> None: if self.last_measurement is not None: difference = get_timestamp(msg) - self.last_measurement if difference < 1. / 15: return self.last_measurement = get_timestamp(msg) sensor_topic, sensor_stats = args # adjust orientation towards current_waypoint quaternion = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w) _, _, self.real_yaw = euler_from_quaternion(quaternion) # cprint(f'yaw world frame: {self.real_yaw}', self._logger) self.pose_est = msg.pose.pose self.vel_est.x = msg.twist.twist.linear.x self.vel_est.y = msg.twist.twist.linear.y self.vel_est.z = msg.twist.twist.linear.z if self._fsm_state == FsmState.Running: self.last_cmd = self._update_twist() self._publisher.publish(self.last_cmd)
def _process_odometry(self, msg: Odometry, args: tuple) -> None: sensor_topic, sensor_stats = args self._set_height(z=msg.pose.pose.position.z) if not self._next_waypoint: return # adjust orientation towards current_waypoint quaternion = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w) _, _, yaw_drone = euler_from_quaternion(quaternion) dx = (self._next_waypoint[0] - msg.pose.pose.position.x) dy = (self._next_waypoint[1] - msg.pose.pose.position.y) # adjust for quadrants... yaw_goal = np.arctan(dy / (dx + 1e-6)) # cprint(f'yaw_goal: {yaw_goal}', self._logger) if np.sign(dx) == -1 and np.sign(dy) == +1: yaw_goal += np.pi # cprint("adjusted yaw_goal to 2th quadrant: {0} > 0".format(yaw_goal), self._logger) elif np.sign(dx) == -1 and np.sign(dy) == -1: yaw_goal -= np.pi # cprint("adjusted yaw_goal to 3th quadrant: {0} < 0".format(yaw_goal), self._logger) if np.abs(yaw_goal - yaw_drone ) > self._specs['max_yaw_deviation_waypoint'] * np.pi / 180: # cprint(f"threshold reached: {np.abs(yaw_goal - yaw_drone)} >" # f" {self._specs['max_yaw_deviation_waypoint'] * np.pi / 180}", self._logger) self._adjust_yaw_waypoint_following = np.sign(yaw_goal - yaw_drone) # if difference between alpha and beta is bigger than pi: # swap direction because the other way is shorter. if np.abs(yaw_goal - yaw_drone) > np.pi: self._adjust_yaw_waypoint_following = -1 * self._adjust_yaw_waypoint_following else: # cprint(f"threshold NOT reached: {np.abs(yaw_goal - yaw_drone)} <" # f" {self._specs['max_yaw_deviation_waypoint'] * np.pi / 180}", self._logger) self._adjust_yaw_waypoint_following = 0
def _process_odometry(self, msg: Odometry, args: tuple) -> None: sensor_topic, sensor_stats = args # impose 7Hz to avoid 1000Hz simulation updates if self.last_measurement is not None: difference = get_timestamp(msg) - self.last_measurement if difference < 1./7: return self.last_measurement = get_timestamp(msg) # simulated robots use /gt_states where twist message of odometry is expressed globally instead of locally if 'real' not in self._robot: _, _, yaw = euler_from_quaternion(msg.pose.pose.orientation) msg.twist.twist.linear = transform(points=[msg.twist.twist.linear], orientation=R.from_euler('XYZ', (0, 0, yaw)).as_matrix(), invert=True)[0] result = self.filter.kalman_correction(msg, self._control_period) if result is not None: self._store_datapoint(msg, 'observed') self._store_datapoint(result, 'adjusted') self.pose_est.pose = result.pose.pose self.vel_est.point.x = result.twist.twist.linear.x self.vel_est.point.y = result.twist.twist.linear.y self.vel_est.point.z = result.twist.twist.linear.z