def sonar_callback(self, sonar_list):
for target in sonar_list.targets:
# self.cuprint("Receiving sonar data")
if self.last_orientation is None: # No orientation, no linearization of the sonar measurement
return
# Convert quaternions to Euler angles.
(r, p, y) = tf.transformations.euler_from_quaternion([self.last_orientation.x, \
self.last_orientation.y, self.last_orientation.z, self.last_orientation.w])
bearing_world = y + target.bearing_rad
z = target.range_m * np.sin(target.elevation_rad)
xy_dist = target.range_m * np.cos(target.elevation_rad)
x = xy_dist * np.cos(bearing_world)
y = xy_dist * np.sin(bearing_world)
now = rospy.get_rostime()
sonar_x = Measurement("sonar_x", now, self.my_name, target.id, x,
self.default_meas_variance["sonar_x"], [])
sonar_y = Measurement("sonar_y", now, self.my_name, target.id, y,
self.default_meas_variance["sonar_y"], [])
sonar_z = Measurement("sonar_z", now, self.my_name, target.id, z,
self.default_meas_variance["sonar_z"], [])
self.filter.add_meas(sonar_x)
self.filter.add_meas(sonar_y)
self.filter.add_meas(sonar_z)
def sonar_callback(self, sonar_list):
for target in sonar_list.targets:
# self.cuprint("Receiving sonar measurements")
if self.last_orientation is None: # No orientation, no linearization of the sonar measurement
# print("no ori")
return
if target.id == "detection":
continue
# self.cuprint("Receiving sonar data")
# Convert quaternions to Euler angles.
self.meas_lock.acquire()
(r, p, y) = tf.transformations.euler_from_quaternion([self.last_orientation.x, \
self.last_orientation.y, self.last_orientation.z, self.last_orientation.w])
self.meas_lock.release()
# y = (np.pi/180.0) * 8
bearing_world = y + target.bearing_rad
z = target.range_m * np.sin(target.elevation_rad)
xy_dist = target.range_m * np.cos(target.elevation_rad)
x = xy_dist * np.cos(bearing_world)
y = xy_dist * np.sin(bearing_world)
now = rospy.get_rostime()
sonar_x, sonar_y = None, None
if "landmark_" in target.id:
sonar_x = Measurement(
"sonar_x", now, self.my_name, "", x,
self.default_meas_variance["sonar_x"],
self.landmark_dict[target.id[len("landmark_"):]])
sonar_y = Measurement(
"sonar_y", now, self.my_name, "", y,
self.default_meas_variance["sonar_x"],
self.landmark_dict[target.id[len("landmark_"):]])
else:
sonar_x = Measurement("sonar_x", now, self.my_name, target.id,
x, self.default_meas_variance["sonar_x"],
[])
sonar_y = Measurement("sonar_y", now, self.my_name, target.id,
y, self.default_meas_variance["sonar_y"],
[])
if target.id in self.red_asset_names and not self.red_asset_found:
self.cuprint("Red Asset detected!")
self.red_asset_found = True
# sonar_z = Measurement("sonar_z", now, self.my_name, target.id, z, self.default_meas_variance["sonar_z"], []
self.filter.add_meas(sonar_x)
self.filter.add_meas(sonar_y)
def no_nav_filter_callback(self, event):
t_now = rospy.get_rostime()
delta_t_ros = t_now - self.last_update_time
self.update_lock.acquire()
### Run Prediction ###
if self.use_control_input:
raise NotImplementedError("using control input not ready yet")
else:
self.filter.predict(np.zeros((3, 1)), self.Q, delta_t_ros.to_sec(),
False)
### Run Correction ###
# Construct depth measurement
z_r = self.default_meas_variance["depth"]
z_data = self.last_depth_meas
if z_data != None:
z = Measurement("depth", t_now, self.my_name, "", z_data, z_r, [])
self.filter.add_meas(z)
self.last_depth_meas = None
# correction
self.filter.correct(t_now)
self.publish_estimates(t_now, Odometry())
self.last_update_time = t_now
self.update_seq += 1
self.update_lock.release()
self.publish_stats(t_now)
def flush(self, final_time):
"""Returns and clears the buffer
Arguments:
final_time {time} -- the last time measurements were considered to be added to the buffer
Returns:
buffer {list} -- the flushed buffer
"""
# Insert the "final_time" marker at the end of the buffer
final_time_marker = Measurement("final_time", final_time, "","",0.0,0.0,[])
self.buffer.append(final_time_marker)
old_buffer = deepcopy(self.buffer)
# Clear buffer
self.buffer = []
self.overflown = False
self.size = 0
return old_buffer
t = rospy.get_rostime()
if "ping" in curr_action:
new_str = curr_action[len("ping_"):]
action_executed_by = new_str[:new_str.index("_to_")]
measured_asset = new_str[new_str.index("_to_") + len("_to_"):]
action_executed_by_pose = seasnub.poses[action_executed_by]
measured_asset_pose = seasnub.poses[measured_asset]
diff_x = measured_asset_pose.position.x - action_executed_by_pose.position.x
diff_y = measured_asset_pose.position.y - action_executed_by_pose.position.y
diff_z = measured_asset_pose.position.z - action_executed_by_pose.position.z
dist = np.linalg.norm([diff_x, diff_y, diff_z]) + np.random.normal(
0, RANGE_SD)
range_meas = Measurement("range", t, action_executed_by,
measured_asset, dist, RANGE_SD**2)
if "surface" in curr_action:
latest_meas_pkg.src_asset = action_executed_by
surface_meas_pkg.measurements.append(range_meas)
# include azimuth
diff_x, diff_y = diff_y, diff_x # transform to NED in gazebo
az_sd = (15 * np.random.uniform() + 30) * (np.pi / 180)
ang = np.arctan2(diff_y, diff_x) #+ np.random.normal(0, az_sd)
az_meas = Measurement("azimuth", t, action_executed_by,
measured_asset, ang, az_sd**2)
surface_meas_pkg.measurements.append(az_meas)
else:
latest_meas_pkg.src_asset = action_executed_by
latest_meas_pkg.measrements.append(range_meas)
"dvl_y": 2,
"bookend": 1,
"bookstart": 1,
"final_time": 0
}
delta_codebook_table = {"depth": 1.0, "dvl_x": 1, "dvl_y": 1}
asset2id = {"my_name": 0}
buffer_cap = 10
dt = DeltaTier(6, x0, P, buffer_cap, meas_space_table,
delta_codebook_table, [0.5, 1.5], asset2id, "my_name")
Q = np.eye(6)
Q[3:, 3:] = np.zeros(Q[3:, 3:].shape)
u = np.array([[0.1, 0.1, -0.1]]).T
t1 = time.time()
z = Measurement("depth", t1, "my_name", "", -1, 0.1, [])
dvl_x = Measurement("dvl_x", t1, "my_name", "", 1, 0.1, [])
dvl_y = Measurement("dvl_y", t1, "my_name", "", 1, 0.1, [])
test_normal, test_buffer_pull, test_catch_up = False, False, True
##### Test Normal Delta Tiering: bookstarts, bookends
dt.add_meas(z)
dt.add_meas(dvl_x)
dt.add_meas(dvl_y)
dt.predict(u, Q)
dt.correct(t1)
if test_normal:
print("0.5 should have depth,dvl_x,dvl_y")
print("1.5 should have bookstarts for depth,dvl_x,dvl_y")
dt.debug_print_buffers()
def bytes2MeasPkg(byte_arr, transmission_time, asset_landmark_dict,
global_pose):
"""Converts a byte stream compressed using measPkg2Bytes() to a Measurement Package
Args:
byte_arr (str): byte stream to decompress
transmission_time (int): Estimated time delta between when measPkg2Bytes() was called
and this method has been called
If unknown set to 0; Not critical to be accurate
asset_landmark_dict (dict): Agreed upon dictionary to go from asset_name --> integer 0-15.
Must be the same on every agent.
e.g. {'bluerov2_4' : 0, 'bluerov2_3' : 1, 'landmark_pole1' : 2, 'red_agent' : 3}
global_pose (list): Pose of the surface beacon
e.g. [x,y,z,theta]
Returns:
etddf/MeasurementPackage.msg: Measurement Package
"""
# Map all values 0 to 255
byte_arr = [x + 128 for x in byte_arr]
mp = MeasurementPackage()
primary_header = byte_arr[0]
src_asset_code = primary_header & 15
mp.src_asset = asset_landmark_dict.keys()[
asset_landmark_dict.values().index(src_asset_code)]
dm_index = (primary_header & (15 << 4)) >> 4
mp.delta_multiplier = delta_multiplier_options[dm_index]
index = 1
present_time = rospy.get_rostime()
while index < len(byte_arr):
msg_global_pose = global_pose
header = byte_arr[index]
meas_type = HEADERS.keys()[HEADERS.values().index((header
& (15 << 4)) >> 4)]
if meas_type == "empty":
break
header2 = header & 15
measured_agent = ""
for mwa in MEASUREMENTS_WITH_AGENTS:
if mwa in meas_type:
measured_agent = asset_landmark_dict.keys()[
asset_landmark_dict.values().index(header2)]
break
index += 1
timestamp = rospy.Time((present_time.secs - transmission_time) -
byte_arr[index])
index += 1
if "book" not in meas_type:
data_bin = byte_arr[index]
data = 0
# Compression
if meas_type == "depth":
bin_per_meter = 255 / -3.0
data = data_bin / bin_per_meter
elif meas_type in ["sonar_x", "sonar_y"]:
bin_per_meter = 255 / 20.0
data = data_bin / bin_per_meter - 10.0
if "landmark" not in measured_agent: # Sonar measurements between agents have global_pose as empty list
msg_global_pose = []
elif meas_type == "modem_range":
bin_per_meter = 255 / 20.0
data = data_bin / bin_per_meter
elif meas_type == "modem_azimuth":
bin_per_meter = 255 / 360.0
data = data_bin / bin_per_meter
data = np.mod(data + 180, 360) - 180 # -180 to 180
m = Measurement(meas_type, timestamp, mp.src_asset, measured_agent,
data, 0.0, msg_global_pose)
mp.measurements.append(m)
index += 1
else:
if "sonar" in meas_type and "landmark" not in meas_type:
msg_global_pose = []
m = Measurement(meas_type, timestamp, mp.src_asset, measured_agent,
0.0, 0.0, msg_global_pose)
mp.measurements.append(m)
return mp
global_pose = [1, 2, 3, 4]
asset_landmark_dict = {
"bluerov2_3": 0,
"bluerov2_4": 1,
"landmark_pole1": 2,
"surface": 4
}
print('############ TEST 1 #################')
# Test compression and decompression
mp = MeasurementPackage()
mp.src_asset = "surface"
mp.delta_multiplier = 1.0
t = rospy.get_rostime()
m = Measurement("modem_range", t, mp.src_asset, "bluerov2_3", 3.65, 0.5,
global_pose)
m2 = Measurement("modem_azimuth", t, mp.src_asset, "bluerov2_3", -65.72,
0.5, global_pose)
m3 = Measurement("modem_range", t, mp.src_asset, "bluerov2_4", 7.8, 0.5,
global_pose)
m4 = Measurement("modem_azimuth", t, mp.src_asset, "bluerov2_4", 23.0, 0.5,
global_pose)
m5 = Measurement("final_time", rospy.get_rostime(), "", "", 0, 0, [])
mp.measurements.append(m)
mp.measurements.append(m2)
mp.measurements.append(m3)
mp.measurements.append(m4)
mp.measurements.append(m5)
num_bytes_buffer = 29
print(mp)
def nav_filter_callback(self, odom):
# Update at specified rate
t_now = rospy.get_rostime()
delta_t_ros = t_now - self.last_update_time
if delta_t_ros < rospy.Duration(1 / self.update_rate):
return
self.update_lock.acquire()
### Run Prediction ###
if self.use_control_input:
raise NotImplementedError("using control input not ready yet")
else:
self.filter.predict(np.zeros((3, 1)), self.Q, delta_t_ros.to_sec(),
False)
### Run Correction ###
# Construct depth measurement
z_r = self.default_meas_variance["depth"]
z_data = self.last_depth_meas
if z_data != None:
z = Measurement("depth", t_now, self.my_name, "", z_data, z_r, [])
self.filter.add_meas(z)
self.last_depth_meas = None
# correction
self.filter.correct(t_now)
self.sonar_rx = False
### Covariancee Intersect ###
# Turn odom estimate into numpy
mean = np.array([[ odom.pose.pose.position.x, \
odom.pose.pose.position.y, \
odom.pose.pose.position.z, \
odom.twist.twist.linear.x, \
odom.twist.twist.linear.y, \
odom.twist.twist.linear.z ]]).T
cov_point = np.array(odom.pose.covariance).reshape(6, 6)
cov_twist = np.array(odom.twist.covariance).reshape(6, 6)
cov = np.zeros((NUM_OWNSHIP_STATES, NUM_OWNSHIP_STATES))
cov[:3, :3] = cov_point[:3, :3]
cov[3:, 3:] = cov_twist[3:, 3:]
# Run covariance intersection
if np.trace(
cov) < 1: # Prevent Nav Filter from having zero uncertainty
cov = np.eye(NUM_OWNSHIP_STATES) * 0.1
c_bar, Pcc = self.filter.intersect(mean, cov)
self.correct_nav_filter(c_bar, Pcc, odom.header, odom)
# TODO partial state update everything
self.last_orientation = odom.pose.pose.orientation
self.publish_estimates(t_now, odom)
self.last_update_time = t_now
self.update_seq += 1
self.update_lock.release()
self.publish_stats(t_now)
def nav_filter_callback(self, pv_msg):
# Update at specified rate
t_now = rospy.get_rostime()
delta_t_ros = t_now - self.last_update_time
if delta_t_ros < rospy.Duration(1 / self.update_rate):
return
self.update_lock.acquire()
### Run Prediction ###
if self.use_control_input and self.control_input is not None:
self.filter.predict(self.control_input, self.Q,
delta_t_ros.to_sec(), False)
else:
self.filter.predict(np.zeros((3, 1)), self.Q, delta_t_ros.to_sec(),
False)
### Run Correction ###
# Construct depth measurement
z_r = self.default_meas_variance["depth"]
z_data = self.last_depth_meas
if z_data != None:
z = Measurement("depth", t_now, self.my_name, "", z_data, z_r,
[]) # Flip z data to transform enu -> NED
self.filter.add_meas(z)
self.last_depth_meas = None
if self.data_x != None:
x = Measurement("gps_x", t_now, self.my_name, "", self.data_x, 0.1,
[])
self.filter.add_meas(x)
self.data_x = None
if self.data_y != None:
y = Measurement("gps_y", t_now, self.my_name, "", self.data_y, 0.1,
[])
self.filter.add_meas(y)
self.data_y = None
# correction
self.filter.correct(t_now)
### Covariancee Intersect ###
# Turn odom estimate into numpy
mean = np.array([[pv_msg.position.x, pv_msg.position.y, pv_msg.position.z, \
pv_msg.velocity.x, pv_msg.velocity.y, pv_msg.velocity.z]]).T
cov = np.array(pv_msg.covariance).reshape(6, 6)
# Run covariance intersection
c_bar, Pcc = self.filter.intersect(mean, cov)
position = Vector3(c_bar[0, 0], c_bar[1, 0], c_bar[2, 0])
velocity = Vector3(c_bar[3, 0], c_bar[4, 0], c_bar[5, 0])
covariance = list(Pcc.flatten())
new_pv_msg = PositionVelocity(position, velocity, covariance)
self.intersection_pub.publish(new_pv_msg)
self.publish_estimates(t_now)
self.last_update_time = t_now
self.update_seq += 1
self.update_lock.release()
self.publish_stats(t_now)
new_str = curr_action[len("ping_"):]
action_executed_by = new_str[:new_str.index("_to_")]
measured_asset = new_str[new_str.index("_to_") + len("_to_"):]
print(action_executed_by + " pinging " + measured_asset)
action_executed_by_pose = seasnub.poses[action_executed_by]
measured_asset_pose = seasnub.poses[measured_asset]
diff_x = measured_asset_pose.position.x - action_executed_by_pose.position.x
diff_y = measured_asset_pose.position.y - action_executed_by_pose.position.y
diff_z = measured_asset_pose.position.z - action_executed_by_pose.position.z
dist = np.linalg.norm([diff_x, diff_y, diff_z]) + np.random.normal(
0, RANGE_SD)
range_meas = Measurement("modem_range", t, action_executed_by,
measured_asset, dist, RANGE_SD**2,
GLOBAL_POSE)
if "surface" in curr_action:
latest_meas_pkg.src_asset = action_executed_by
surface_meas_pkg.measurements.append(range_meas)
# include azimuth
# diff_x, diff_y = diff_y, diff_x # transform to NED in gazebo
# az_sd = ( 15*np.random.uniform() + 30 ) * (np.pi/180)
ang = np.arctan2(diff_y, diff_x) #+ np.random.normal(0, az_sd)
ang_deg = np.rad2deg(ang) + np.random.normal(0, AZIMUTH_SD)
az_meas = Measurement("modem_azimuth", t, action_executed_by,
measured_asset, ang_deg, AZIMUTH_SD**2,
GLOBAL_POSE)
surface_meas_pkg.measurements.append(az_meas)
else: