def __init__(self):
self.cuprint = CUPrint("SonarAssociator")
self.cuprint("Loading")
self.association_threshold_distance = rospy.get_param("~threshold", 1)
self.landmark_dict = rospy.get_param("~landmarks", {})
# pose_topic = "etddf/estimate" + rospy.get_namespace()[:-1]
pose_topic = rospy.get_namespace()[:-1] + "/pose_gt"
rospy.Subscriber(pose_topic, Odometry, self.pose_callback)
rospy.wait_for_message(pose_topic, Odometry)
self.red_team_names = rospy.get_param("~red_team_names")
blue_team = rospy.get_param("~blue_team_names")
self.blue_team = {}
for b in blue_team:
if b == "surface":
continue
rospy.Subscriber("etddf/estimate/" + b,
Odometry,
self.blue_team_callback,
callback_args=b)
self.cuprint("Waiting for msg: " + "etddf/estimate/" + b)
rospy.wait_for_message("etddf/estimate/" + b, Odometry)
self.pub = rospy.Publisher("sonar_processing/target_list/associated",
SonarTargetList,
queue_size=10)
sonar_topic = "sonar_processing/target_list"
rospy.Subscriber(sonar_topic, SonarTargetList, self.sonar_callback)
self.cuprint("Loaded")
def __init__(self, name):
rospy.Subscriber("{}/mavros/global_position/rel_alt".format(name),
Float64, self.baro_callback)
rospy.Subscriber("{}/dvl".format(name), TwistWithCovarianceStamped,
self.dvl_callback)
rospy.Subscriber("{}/imu/data_raw".format(name), Imu,
self.compass_callback)
rospy.Subscriber("{}/etddf/packages_in".format(name),
MeasurementPackage, self.modem_callback)
self.cuprint = CUPrint("tuner")
self.baro_msgs = []
self.dvl_x_msgs = []
self.dvl_y_msgs = []
self.dvl_z_msgs = []
# Compass
self.compass_roll_msgs = []
self.compass_pitch_msgs = []
self.compass_yaw_msgs = []
self.gyro_x_msgs = []
self.gyro_y_msgs = []
self.gyro_z_msgs = []
self.accel_x_msgs = []
self.accel_y_msgs = []
self.accel_z_msgs = []
# Modem
self.range_msgs = []
self.azimuth_msgs = []
def __init__(self, x0, P0, Q, default_meas_variance, strapdown=True):
self.cuprint = CUPrint(rospy.get_name())
self.last_update_time = None
self.update_lock = threading.Lock()
# initialize estimate and covariance
self.x = x0.reshape(-1)
self.P = P0
self.Q = Q
self.pub = rospy.Publisher("strapdown/estimate",
Odometry,
queue_size=10)
self.ci_pub = rospy.Publisher("strapdown/intersection",
PositionVelocity,
queue_size=1)
self.last_depth = None
self.last_intersection = None
self.data_x, self.data_y = None, None
self.dvl_x, self.dvl_y = None, None
self.skip_multiplexer = 0
rospy.sleep(rospy.get_param("~wait_on_startup"))
# rospy.Subscriber("mavros/global_position/rel_alt", Float64, self.depth_callback, queue_size=1)
rospy.Subscriber("baro", Float64, self.depth_callback, queue_size=1)
rospy.Subscriber("dvl", Vector3, self.dvl_callback, queue_size=1)
# rospy.Subscriber("pose_gt", Odometry, self.gps_callback, queue_size=1)
if strapdown:
rospy.Subscriber("imu", Imu, self.propagate_strap, queue_size=1)
else:
rospy.Subscriber("imu", Imu, self.propagate_normal, queue_size=1)
rospy.Subscriber("strapdown/intersection_result", PositionVelocity,
self.intersection_result)
self.cuprint("loaded")
def __init__(self):
self.cuprint = CUPrint("Sonar Control")
self.own_yaw = None
# ownship = "etddf/estimate" + rospy.get_namespace()[:-1]
ownship = rospy.get_namespace()[:-1] + "/pose_gt"
rospy.Subscriber(ownship, Odometry, self.pose_callback)
rospy.wait_for_message(ownship, Odometry)
# rospy.Subscriber("ping_360_target",SonarTargetList,self.check_for_landmark)
self.landmark_dict = rospy.get_param("~landmarks", {})
default_track = rospy.get_param("~default_track")
print(default_track)
if default_track != "None":
self.last_req = SetSonarModeRequest()
self.last_req.mode.object = default_track
self.last_req.mode.mode = self.last_req.mode.TRACK
self.cuprint("Tracking Landmark: " +
self.last_req.mode.object[len("landmark_"):])
else:
self.last_req = None
self.scan_update_rate = rospy.get_param("~scan_update_rate_hz")
self.scan_range_360 = rospy.get_param("~360_scan_range_m",
DEFAULT_SCAN_360_RANGE_M)
self._red_team_names = rospy.get_param('~red_team_names', [])
if len(self._red_team_names) > 0:
self._red_agent_id = self._red_team_names[0]
for i in self._red_team_names:
rospy.Subscriber("etddf/estimate/" + i, Odometry,
self.red_actor_callback)
else:
self._red_agent_id = -1
self.cuprint("waiting for set_sonar_settings service")
rospy.wait_for_service("ping360_node/sonar/set_sonar_settings")
self.set_sonar = rospy.ServiceProxy(
"ping360_node/sonar/set_sonar_settings", SetSonarSettings)
self.cuprint("...service found")
rospy.Service("ping360_node/sonar/set_scan_mode", SetSonarMode,
self.handle_scan_mode)
def __init__(self):
self.my_name = rospy.get_namespace()[:-1].strip("/")
self.cuprint = CUPrint("{}/associator_node".format(self.my_name))
# Associator Params
self.association_sigma = rospy.get_param("~association_sigma")
time_to_drop = rospy.get_param("~time_to_drop")
self.lost_agent_unc = rospy.get_param("~lost_agent_unc")
proto_track_points = rospy.get_param("~proto_track_points")
process_noise = rospy.get_param("~position_process_noise")
proto_Q = np.array([[process_noise, 0], [0, process_noise]])
self.associator = Associator(time_to_drop, self.lost_agent_unc,
proto_track_points, proto_Q)
self.bearing_var = rospy.get_param("~force_sonar_az_var")
self.range_var = rospy.get_param("~force_sonar_range_var")
# Subscribe to all blue team poses
blue_team = rospy.get_param("~blue_team_names")
self.agent_poses = {}
for b in blue_team:
if b == "surface":
continue
rospy.Subscriber("etddf/estimate/" + b,
Odometry,
self.blue_team_callback,
callback_args=b)
# Get my pose
self.my_name = rospy.get_namespace()[:-1].strip("/")
pose_topic = "etddf/estimate/" + self.my_name
rospy.Subscriber(pose_topic, Odometry, self.pose_callback)
self.cuprint("Waiting for orientation")
rospy.wait_for_message(pose_topic, Odometry) # TODO add back in
# Debug purposes
# o = Odometry() # main agent at zero-zero
# cov = np.eye(6)
# o.pose.covariance = list( cov.flatten() )
# o.pose.pose.orientation.w = 1
# self.pose_callback(o)
# self.cuprint("Orientation found")
self.red_agent_name = rospy.get_param("~red_agent_name")
if self.red_agent_name != "":
rospy.Subscriber("etddf/estimate/" + self.red_agent_name, Odometry,
self.red_agent_callback)
self.pub = rospy.Publisher("sonar_processing/target_list/associated",
SonarTargetList,
queue_size=10)
# Sonar Controller Params
self.enable_sonar_control = rospy.get_param("~enable_sonar_control")
rospy.Subscriber("associator/enable_scan_control", Bool,
self.enable_sonar_control_callback)
self.sonar_control_pub = rospy.Publisher("ping360_node/sonar/set_scan",
SonarSettings,
queue_size=10)
self.scan_size_deg = rospy.get_param("~scan_size_deg")
self.ping_thresh = rospy.get_param("~ping_thresh")
self.scan_angle = None
self.prototrack = None
rospy.Subscriber("ping360_node/sonar/scan_complete", UInt16,
self.scan_angle_callback)
if self.enable_sonar_control:
self.cuprint("Waiting for scan to complete")
rospy.wait_for_message("ping360_node/sonar/scan_complete", UInt16)
sonar_topic = "sonar_processing/target_list"
rospy.Subscriber(sonar_topic, SonarTargetList, self.sonar_callback)
self.cuprint("Loaded")
def __init__(self,
my_name, \
update_rate, \
delta_tiers, \
asset2id, \
delta_codebook_table, \
buffer_size, \
meas_space_table, \
x0,\
P0,\
Q,\
default_meas_variance,
use_control_input):
# self.br = tf.TransformBroadcaster()
self.update_rate = update_rate
self.asset2id = asset2id
self.Q = Q
self.use_control_input = use_control_input
self.default_meas_variance = default_meas_variance
self.my_name = my_name
self.landmark_dict = rospy.get_param("~landmarks", {})
self.topside_name = rospy.get_param("mission_config/surface_beacon_name")
self.cuprint = CUPrint(rospy.get_name())
self.cuprint(self.topside_name)
if rospy.get_param("~simple_sharing"):
self.cuprint("Sharing most recent")
self.filter = MostRecent(NUM_OWNSHIP_STATES, \
x0,\
P0,\
buffer_size,\
meas_space_table,\
delta_codebook_table,\
delta_tiers,\
self.asset2id,\
my_name,
default_meas_variance
)
else:
self.cuprint("Delta Tiering")
self.filter = DeltaTier(NUM_OWNSHIP_STATES, \
x0,\
P0,\
buffer_size,\
meas_space_table,\
delta_codebook_table,\
delta_tiers,\
self.asset2id,\
my_name, \
default_meas_variance)
self.network_pub = rospy.Publisher("etddf/estimate/network", NetworkEstimate, queue_size=10)
self.cuprint("Loading...")
self.asset_pub_dict = {}
for asset in self.asset2id.keys():
if self.topside_name in asset:
continue
self.asset_pub_dict[asset] = rospy.Publisher("etddf/estimate/" + asset, Odometry, queue_size=10)
self.update_seq = 0
self.last_depth_meas = None
rospy.sleep(rospy.Duration(1 / self.update_rate))
self.last_update_time = rospy.get_rostime() - rospy.Duration(1 / self.update_rate)
self.meas_lock = threading.Lock()
self.update_lock = threading.Lock()
self.last_orientation = None
self.red_asset_found = False
self.red_asset_names = rospy.get_param("~red_team_names")
# Modem & Measurement Packages
rospy.Subscriber("etddf/packages_in", MeasurementPackage, self.meas_pkg_callback, queue_size=1)
# Get modem corrections
modem_az_bias = rospy.get_param("mission_config/modem_az/bias") # Just configure global pose
modem_az_std_deg = np.sqrt( rospy.get_param("mission_config/modem_az/var") )
self.modem_az_var = np.radians(modem_az_std_deg) ** 2
self.modem_range_bias = rospy.get_param("mission_config/modem_range/bias")
self.modem_range_var = rospy.get_param("mission_config/modem_range/var")
self.surface_beacon_pose = rospy.get_param("mission_config/surface_beacon_position")
self.surface_beacon_pose.append( np.radians( modem_az_bias) )
self.cuprint(str(self.modem_az_var))
self.cuprint(str(self.surface_beacon_pose))
if self.use_control_input:
raise NotImplementedError("Control input")
self.control_input = None
rospy.Subscriber("uuv_control/control_status", ControlStatus, self.control_status_callback, queue_size=1)
if rospy.get_param("~strapdown_topic") != "None":
self.cuprint("Intersecting with strapdown")
rospy.Subscriber( rospy.get_param("~strapdown_topic"), Odometry, self.nav_filter_callback, queue_size=1)
# Set up publisher for correcting the odom estimate
self.intersection_pub = rospy.Publisher("set_pose", PoseWithCovarianceStamped, queue_size=1)
rospy.wait_for_message( rospy.get_param("~strapdown_topic"), Odometry)
else:
self.cuprint("Not intersecting with strapdown filter")
rospy.Timer(rospy.Duration(1 / self.update_rate), self.no_nav_filter_callback)
# Initialize Buffer Service
rospy.Service('etddf/get_measurement_package', GetMeasurementPackage, self.get_meas_pkg_callback)
# Wait for our first strapdown msg
self.cuprint("loaded, sleeping for RL to correct...")
rospy.sleep(10) # Wait for RL to correct
# Sonar Subscription
if rospy.get_param("~measurement_topics/sonar") != "None":
self.cuprint("Subscribing to sonar")
rospy.Subscriber(rospy.get_param("~measurement_topics/sonar"), SonarTargetList, self.sonar_callback)
def __init__(self, my_name, \
update_rate, \
delta_tiers, \
asset2id, \
delta_codebook_table, \
buffer_size, \
meas_space_table, \
missed_meas_tolerance_table, \
x0,\
P0,\
Q,\
default_meas_variance,
use_control_input):
self.update_rate = update_rate
self.asset2id = asset2id
self.Q = Q
self.use_control_input = use_control_input
self.default_meas_variance = default_meas_variance
self.my_name = my_name
self.filter = DeltaTier(NUM_OWNSHIP_STATES, \
x0,\
P0,\
buffer_size,\
meas_space_table,\
missed_meas_tolerance_table, \
delta_codebook_table,\
delta_tiers,\
self.asset2id,\
my_name)
self.cuprint = CUPrint(rospy.get_name())
self.network_pub = rospy.Publisher("etddf/estimate/network",
NetworkEstimate,
queue_size=10)
self.statistics_pub = rospy.Publisher("etddf/statistics",
EtddfStatistics,
queue_size=10)
self.statistics = EtddfStatistics(0, rospy.get_rostime(), 0, 0,
delta_tiers,
[0 for _ in delta_tiers], 0.0, [],
False)
self.asset_pub_dict = {}
for asset in self.asset2id.keys():
if "surface" in asset:
continue
self.asset_pub_dict[asset] = rospy.Publisher("etddf/estimate/" +
asset,
Odometry,
queue_size=10)
self.update_seq = 0
self.last_depth_meas = None
rospy.sleep(rospy.Duration(1 / self.update_rate))
self.last_update_time = rospy.get_rostime() - rospy.Duration(
1 / self.update_rate)
self.meas_lock = threading.Lock()
self.update_lock = threading.Lock()
self.last_orientation = None
# Initialize Measurement Callbacks
rospy.Subscriber("mavros/global_position/local",
Odometry,
self.depth_callback,
queue_size=1)
rospy.Subscriber("etddf/packages_in",
MeasurementPackage,
self.meas_pkg_callback,
queue_size=1)
if self.use_control_input:
rospy.Subscriber("uuv_control/control_status",
ControlStatus,
self.control_status_callback,
queue_size=1)
# IMU Covariance Intersection
if rospy.get_param("~measurement_topics/imu_ci") == "None":
rospy.Timer(rospy.Duration(1 / self.update_rate),
self.no_nav_filter_callback)
else:
rospy.Subscriber(rospy.get_param("~measurement_topics/imu_ci"),
Odometry,
self.nav_filter_callback,
queue_size=1)
self.set_pose_pub = rospy.Publisher("set_pose",
PoseWithCovarianceStamped,
queue_size=10)
# Sonar Subscription
if rospy.get_param("~measurement_topics/sonar") != "None":
rospy.Subscriber(rospy.get_param("~measurement_topics/sonar"),
SonarTargetList, self.sonar_callback)
# Initialize Buffer Service
rospy.Service('etddf/get_measurement_package', GetMeasurementPackage,
self.get_meas_pkg_callback)
self.cuprint("loaded")
def __init__(self, my_name, \
update_rate, \
delta_tiers, \
asset2id, \
delta_codebook_table, \
buffer_size, \
meas_space_table, \
missed_meas_tolerance_table, \
x0,\
P0,\
Q,\
default_meas_variance,
use_control_input):
self.update_rate = update_rate
self.asset2id = asset2id
self.Q = Q
self.use_control_input = use_control_input
self.default_meas_variance = default_meas_variance
self.my_name = my_name
self.landmark_dict = rospy.get_param("~landmarks", {})
self.cuprint = CUPrint(rospy.get_name())
self.filter = DeltaTier(NUM_OWNSHIP_STATES, \
x0,\
P0,\
buffer_size,\
meas_space_table,\
missed_meas_tolerance_table, \
delta_codebook_table,\
delta_tiers,\
self.asset2id,\
my_name)
self.network_pub = rospy.Publisher("etddf/estimate/network",
NetworkEstimate,
queue_size=10)
self.statistics_pub = rospy.Publisher("etddf/statistics",
EtddfStatistics,
queue_size=10)
self.statistics = EtddfStatistics(0, rospy.get_rostime(), 0, 0,
delta_tiers,
[0 for _ in delta_tiers], 0.0, [],
False)
self.asset_pub_dict = {}
for asset in self.asset2id.keys():
if "surface" in asset:
continue
self.asset_pub_dict[asset] = rospy.Publisher("etddf/estimate/" +
asset,
Odometry,
queue_size=10)
self.update_seq = 0
self.last_depth_meas = None
rospy.sleep(rospy.Duration(1 / self.update_rate))
self.last_update_time = rospy.get_rostime() - rospy.Duration(
1 / self.update_rate)
self.meas_lock = threading.Lock()
self.update_lock = threading.Lock()
self.last_orientation = None
self.red_asset_found = False
self.red_asset_names = rospy.get_param("~red_team_names")
# Depth Sensor
if rospy.get_param("~measurement_topics/depth") != "None":
rospy.Subscriber(rospy.get_param("~measurement_topics/depth"),
Float64,
self.depth_callback,
queue_size=1)
# Modem & Measurement Packages
rospy.Subscriber("etddf/packages_in",
MeasurementPackage,
self.meas_pkg_callback,
queue_size=1)
if self.use_control_input:
self.control_input = None
rospy.Subscriber("uuv_control/control_status",
ControlStatus,
self.control_status_callback,
queue_size=1)
if rospy.get_param("~strapdown"):
rospy.Subscriber(rospy.get_param("~measurement_topics/imu_est"),
Odometry,
self.orientation_estimate_callback,
queue_size=1)
rospy.wait_for_message(
rospy.get_param("~measurement_topics/imu_est"), Odometry)
# IMU Covariance Intersection
if rospy.get_param("~strapdown") and rospy.get_param(
"~measurement_topics/imu_ci") != "None":
self.cuprint("Intersecting with strapdown")
self.intersection_pub = rospy.Publisher(
"strapdown/intersection_result",
PositionVelocity,
queue_size=1)
rospy.Subscriber(rospy.get_param("~measurement_topics/imu_ci"),
PositionVelocity,
self.nav_filter_callback,
queue_size=1)
else:
self.cuprint("Not intersecting with strapdown filter")
rospy.Timer(rospy.Duration(1 / self.update_rate),
self.no_nav_filter_callback)
# Sonar Subscription
if rospy.get_param("~measurement_topics/sonar") != "None":
rospy.Subscriber(rospy.get_param("~measurement_topics/sonar"),
SonarTargetList, self.sonar_callback)
self.data_x, self.data_y = None, None
# rospy.Subscriber("pose_gt", Odometry, self.gps_callback, queue_size=1)
# Initialize Buffer Service
rospy.Service('etddf/get_measurement_package', GetMeasurementPackage,
self.get_meas_pkg_callback)
self.cuprint("loaded")
from logging import error
import rospy
from etddf_minau.msg import Measurement, MeasurementPackage
from geometry_msgs.msg import PoseWithCovarianceStamped
import numpy as np
from cuprint.cuprint import CUPrint
MEASUREMENT_COVARIANCE = 2
TOPSIDE_NAME = "wamv_1"
rospy.init_node("beacon_to_pose")
pub = rospy.Publisher("beacon_pose", PoseWithCovarianceStamped, queue_size=1)
my_name = rospy.get_namespace().replace("/", "")
my_cuprint = CUPrint(rospy.get_name())
def callback(msg):
global pub, my_name, MEASUREMENT_COVARIANCE, my_cuprint
my_cuprint("Receiving modem measurements")
azimuth, _range = None, None
global_pose = [0, 0, 0, 0] # x,y,z, yaw (degrees)
for meas in msg.measurements:
if meas.measured_asset != my_name:
continue
elif meas.src_asset != TOPSIDE_NAME:
rospy.logerr("Expected src asset to be topside!! was: {}".format(
meas.src_asset))
def __init__(self):
self.my_name = rospy.get_param("~my_name")
self.cuprint = CUPrint("{}/etddf".format(self.my_name))
self.blue_agent_names = rospy.get_param("~blue_team_names")
blue_positions = rospy.get_param("~blue_team_positions")
self.topside_name = rospy.get_param("~topside_name")
assert self.topside_name not in self.blue_agent_names
red_agent_name = rospy.get_param("~red_team_name")
self.update_times = []
self.red_agent_exists = red_agent_name != ""
if self.red_agent_exists:
self.red_agent_name = red_agent_name
self.red_agent_id = len(self.blue_agent_names)
self.use_strapdown = rospy.get_param("~use_strapdown")
self.do_correct_strapdown = rospy.get_param("~correct_strapdown")
self.correct_strapdown_next_seq = False
self.position_process_noise = rospy.get_param(
"~position_process_noise")
self.velocity_process_noise = rospy.get_param(
"~velocity_process_noise")
self.fast_ci = rospy.get_param("~fast_ci")
self.force_modem_pose = rospy.get_param("~force_modem_pose")
self.meas_variances = {}
self.meas_variances["sonar_range"] = rospy.get_param(
"~force_sonar_range_var")
self.meas_variances["sonar_az"] = rospy.get_param(
"~force_sonar_az_var")
self.meas_variances["modem_range"] = rospy.get_param(
"~force_modem_range_var")
self.meas_variances["modem_az"] = rospy.get_param(
"~force_modem_az_var")
known_position_uncertainty = rospy.get_param(
"~known_position_uncertainty")
unknown_position_uncertainty = rospy.get_param(
"~unknown_position_uncertainty")
self.is_deltatier = rospy.get_param("~is_deltatier")
if self.is_deltatier:
self.delta_multipliers = rospy.get_param("~delta_tiers")
self.delta_codebook_table = {
"sonar_range": rospy.get_param("~sonar_range_start_et_delta"),
"sonar_azimuth": rospy.get_param("~sonar_az_start_et_delta")
}
self.buffer_size = rospy.get_param("~buffer_space")
if self.is_deltatier:
rospy.Service('etddf/get_measurement_package',
GetMeasurementPackage, self.get_meas_pkg_callback)
self.kf = KalmanFilter(blue_positions, [], self.red_agent_exists, self.is_deltatier, \
known_posititon_unc=known_position_uncertainty,\
unknown_agent_unc=unknown_position_uncertainty)
self.network_pub = rospy.Publisher("etddf/estimate/network",
NetworkEstimate,
queue_size=10)
self.asset_pub_dict = {}
for asset in self.blue_agent_names:
self.asset_pub_dict[asset] = rospy.Publisher("etddf/estimate/" +
asset,
Odometry,
queue_size=10)
if self.red_agent_exists:
self.asset_pub_dict[self.red_agent_name] = rospy.Publisher(
"etddf/estimate/" + self.red_agent_name,
Odometry,
queue_size=10)
self.last_update_time = rospy.get_rostime()
# Modem & Measurement Packages
rospy.Subscriber("etddf/packages_in",
MeasurementPackage,
self.meas_pkg_callback,
queue_size=1)
# Strapdown configuration
self.update_seq = 0
self.strapdown_correction_period = rospy.get_param(
"~strapdown_correction_period")
strap_topic = "odometry/filtered/odom"
rospy.Subscriber(strap_topic,
Odometry,
self.nav_filter_callback,
queue_size=1)
self.intersection_pub = rospy.Publisher("set_pose",
PoseWithCovarianceStamped,
queue_size=1)
self.cuprint("Waiting for strapdown")
rospy.wait_for_message(strap_topic, Odometry)
self.cuprint("Strapdown found")
# Sonar Subscription
rospy.Subscriber("sonar_processing/target_list/associated",
SonarTargetList, self.sonar_callback)
self.cuprint("Loaded")