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