def __init__(self):
self.bathy_grid = gridgen.BathyGrid.from_bathymetry( \
$(BATHY_GRID), False)
self.comms = pymoos.comms()
self.comms.set_on_connect_callback(self.connect_callback)
self.comms.set_on_mail_callback(self.message_received)
pymoos.set_moos_timewarp($(WARP))
self.comms.set_comms_control_timewarp_scale_factor(0.4)
self.comms.run('localhost', 9000, 'uSonarSimulator')
# Initialize stored variables
self.messages = dict()
self.messages['NAV_X'] = 0
self.messages['NAV_Y'] = 0
self.messages['NAV_LAT'] = 0
self.messages['NAV_LONG'] = 0
self.messages['NAV_HEADING'] = 0
self.messages['NEXT_SWATH_SIDE'] = 'stbd'
self.post_ready = False
self.post_message = ""
self.post_stbd = ""
self.post_port = ""
self.swath_angle = SWATH_ANGLE
self.last_port_depth = 0
self.last_stbd_depth = 0
self.msg_count = 0
def __init__(self):
self.moos_app_name = 'PySpectrogram'
self.time_warp = 1
self.server_host = 'localhost'
self.server_port = 9000
self.freq_sampling = 37500.0
self.num_samples = 16000
self.num_hydrophones = 5
self.aco_data = np.zeros(
(self.num_samples, self.num_hydrophones
)) #array that holds the acoustic data currently being processed
self.aco_data_latest = None #array that holds new acoustic data while it waits to be processed
self.aco_new = False #flag to indicate that new acoustic data has been read
self.extents = None
''' Initialize Python-MOOS Communications '''
self.mooscomms = pymoos.comms()
self.mooscomms.set_on_connect_callback(self.moos_on_connect)
self.mooscomms.add_active_queue(
'cbf_queue', self.moos_on_new_acoustic_binary_data
) #let's use a queue callback instead to handle distinct messages
self.mooscomms.add_message_route_to_active_queue(
'cbf_queue', 'DAQ_BINARY_DATA'
) #route 'DAQ_BINARY_DATA' messages to moos_on_new_acoustic_binary_data function
self.mooscomms.run(self.server_host, self.server_port,
self.moos_app_name)
pymoos.set_moos_timewarp(self.time_warp)
def Set_time_warp(self, timewarp):
if timewarp > 1:
# Time Warp and Scaling factor constant
time_warp = timewarp
scaling_factor = 0.04 * time_warp
# Set the timewarp and scale factor
pymoos.set_moos_timewarp(time_warp)
self.comms.set_comms_control_timewarp_scale_factor(scaling_factor)
def __init__(self, params):
"""Initiates MOOSComms, sets the callbacks and runs the loop"""
super(Ship, self).__init__()
self.server = 'localhost'
self.port = int(params['ServerPort'])
self.name = 'iSMH'
self.set_on_connect_callback(self.__on_connect)
self.set_on_mail_callback(self.__on_new_mail)
self.add_active_queue('desired_queue', self.on_desired_message)
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_ROTATION_1')
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_ROTATION_2')
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_ROTATION_3')
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_RUDDER_1')
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_RUDDER_2')
self.desired_rotation_1 = 0
self.desired_rotation_2 = 0
self.desired_rotation_3 = 0
self.desired_rudder_1 = 0
self.desired_rudder_2 = 0
self.real_x = params['START_X']
self.real_y = params['START_Y']
self.real_heading = params['START_HEADING']
self.real_speed = 0
self.real_v = 0
self.real_r = 0
self.run(self.server, self.port, self.name)
pymoos.set_moos_timewarp(params['MOOSTimeWarp'])
self.dt = 0.1
self.pid = '911'
self.server_addr = '172.16.11.38'
self.db_conn_str = 'mongodb://172.16.11.10:27017'
self.db_name = 'smh'
self.ds = pybuzz.create_bson_data_source(self.db_conn_str,
self.db_name)
self.session = pybuzz.join_simco_session(
self.pid, pybuzz.create_bson_serializer(self.ds))
self.session.initialize()
self.session.is_publisher(pybuzz.rudder_tag(),
pybuzz.rudder_tag.SMH_DEMANDED_ANGLE)
self.session.is_publisher(pybuzz.thruster_tag(),
pybuzz.thruster_tag.SMH_DEMANDED_ROTATION)
self.session.connect(self.server_addr)
self.navio = []
def main():
comms.set_on_connect_callback(c)
comms.set_on_mail_callback(m)
pymoos.set_moos_timewarp(10)
comms.set_comms_control_timewarp_scale_factor(0.4)
comms.run('localhost', 9000, 'pymoos')
while True:
time.sleep(1.0)
comms.notify('simple_var', 'a string', pymoos.time())
def main():
comms.set_on_connect_callback(c)
comms.set_on_mail_callback(m)
pymoos.set_moos_timewarp(10)
comms.set_comms_control_timewarp_scale_factor(0.4)
comms.run('localhost',9000,'pymoos')
while True:
time.sleep(1.0)
comms.notify('simple_var','a string',pymoos.time())
def __init__(self, params):
"""Initiates MOOSComms, sets the callbacks and runs the loop"""
super(pTrajectPID, self).__init__()
self.server = 'localhost'
self.port = int(params['ServerPort'])
self.name = 'pTrajectPID'
self.set_on_connect_callback(self.__on_connect)
self.set_on_mail_callback(self.__on_new_mail)
self.add_active_queue('desired_queue', self.on_desired_message)
self.add_message_route_to_active_queue('desired_queue', 'DESIRED_SPEED')
self.add_message_route_to_active_queue('desired_queue', 'DESIRED_HEADING')
self.add_active_queue('sensor_queue', self.on_sensor_message)
self.add_message_route_to_active_queue('sensor_queue', 'SENSOR_SPEED')
self.add_message_route_to_active_queue('sensor_queue', 'SENSOR_HEADING')
self.add_active_queue('ivphelm_queue', self.on_ivphelm_message)
self.add_message_route_to_active_queue('ivphelm_queue', 'IVPHELM_ALLSTOP')
self.manual="false"
self.add_active_queue('dp_queue', self.on_dp_message)
self.add_message_route_to_active_queue('dp_queue', 'DP_MODE')
self.dp="off"
self.desired_speed = 0
self.desired_heading = 0
self.sensor_speed = 0
self.sensor_heading = 0
self.desired_rudder = 0
self.desired_rotation = 0
self.run(self.server, self.port, self.name)
pymoos.set_moos_timewarp(params['MOOSTimeWarp'])
self.dt=0.5
dt=self.dt
self.coursePID = myPID(Kp=params['yaw_kp']/5, Ki=params['yaw_ki']/5, Kd=params['yaw_kd']/5, dt=dt, max_output=params['max_rudder'])
self.speedPID = myPID(Kp=params['spd_kp'], Ki=params['spd_ki'], Kd=params['spd_kd'], dt=dt, max_output=params['max_rotation'])
def __init__(self):
self.moos_app_name = 'PySpectrogram'
self.time_warp = 1
self.server_host = 'localhost'
self.server_port = 9000
self.freq_sampling = 37500.0
self.num_samples = 16000
self.num_hydrophones = 5
self.aco_data = np.zeros((self.num_samples, self.num_hydrophones)) #array that holds the acoustic data currently being processed
self.aco_data_latest = None #array that holds new acoustic data while it waits to be processed
self.aco_new = False #flag to indicate that new acoustic data has been read
self.extents = None
''' Initialize Python-MOOS Communications '''
self.mooscomms = pymoos.comms()
self.mooscomms.set_on_connect_callback(self.moos_on_connect)
self.mooscomms.add_active_queue('cbf_queue', self.moos_on_new_acoustic_binary_data) #let's use a queue callback instead to handle distinct messages
self.mooscomms.add_message_route_to_active_queue('cbf_queue', 'DAQ_BINARY_DATA') #route 'DAQ_BINARY_DATA' messages to moos_on_new_acoustic_binary_data function
self.mooscomms.run(self.server_host, self.server_port, self.moos_app_name)
pymoos.set_moos_timewarp(self.time_warp)
def __init__(self, params):
"""Initiates MOOSComms, sets the callbacks and runs the loop"""
super(Ship, self).__init__()
self.server = 'localhost'
self.port = int(params['ServerPort'])
self.name = 'iPydyna'
self.set_on_connect_callback(self.__on_connect)
self.set_on_mail_callback(self.__on_new_mail)
self.add_active_queue('desired_queue', self.on_desired_message)
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_ROTATION_1')
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_ROTATION_2')
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_ROTATION_3')
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_RUDDER_1')
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_RUDDER_2')
self.desired_rotation_1 = 0
self.desired_rotation_2 = 0
self.desired_rotation_3 = 0
self.desired_rudder_1 = 0
self.desired_rudder_2 = 0
self.real_x = params['START_X']
self.real_y = params['START_Y']
self.real_heading = params['START_HEADING']
self.real_speed = 0
self.sim = pydyna.create_simulation("NACMM_2021.p3d")
self.ship = self.sim.vessels['292']
self.rudder1 = self.ship.rudders['0']
self.rudder2 = self.ship.rudders['1']
self.propeller1 = self.ship.thrusters['0']
self.propeller2 = self.ship.thrusters['1']
self.propeller3 = self.ship.thrusters['4']
self.ship._set_linear_position([self.real_x, self.real_y, 0])
if self.real_heading > 270:
self.ship._set_angular_position(
[0, 0, np.deg2rad(450 - self.real_heading)])
elif self.real_heading > 180:
self.ship._set_angular_position(
[0, 0, np.deg2rad(360 - self.real_heading)])
else:
self.ship._set_angular_position(
[0, 0, np.deg2rad(90 - self.real_heading)])
self.ship._set_linear_velocity([0, 0, 0])
self.rudder1.dem_angle = -self.desired_rudder_1
self.rudder2.dem_angle = -self.desired_rudder_2
self.propeller1.dem_rotation = self.desired_rotation_1
self.propeller2.dem_rotation = self.desired_rotation_2
self.propeller3.dem_rotation = self.desired_rotation_3
self.run(self.server, self.port, self.name)
pymoos.set_moos_timewarp(params['MOOSTimeWarp'])
def __init__(self, params):
"""Initiates MOOSComms, sets the callbacks and runs the loop"""
super(pEKF, self).__init__()
self.server = 'localhost'
self.port = int(params['ServerPort'])
self.name = 'pEKF'
self.set_on_connect_callback(self.__on_connect)
self.set_on_mail_callback(self.__on_new_mail)
self.add_active_queue('dvl_queue', self.on_dvl_message)
self.add_message_route_to_active_queue('dvl_queue', 'DVL_SPEED')
self.add_active_queue('gps_queue', self.on_gps_message)
self.add_message_route_to_active_queue('gps_queue', 'GPS_SPEED')
self.add_message_route_to_active_queue('gps_queue', 'GPS_X')
self.add_message_route_to_active_queue('gps_queue', 'GPS_Y')
self.add_active_queue('gyro_queue', self.on_gyro_message)
self.add_message_route_to_active_queue('gyro_queue', 'GYRO_HEADING')
self.add_active_queue('imu_queue', self.on_imu_message)
self.add_message_route_to_active_queue('imu_queue', 'IMU_SPEED')
self.add_message_route_to_active_queue('imu_queue', 'IMU_X')
self.add_message_route_to_active_queue('imu_queue', 'IMU_Y')
self.add_message_route_to_active_queue('imu_queue', 'IMU_HEADING')
self.add_message_route_to_active_queue('imu_queue', 'IMU_V')
self.add_message_route_to_active_queue('imu_queue', 'IMU_R')
self.add_active_queue('desired_queue', self.on_desired_message)
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_ROTATION_1')
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_ROTATION_2')
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_ROTATION_3')
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_RUDDER_1')
self.add_message_route_to_active_queue('desired_queue',
'DESIRED_RUDDER_2')
self.dvl_speed = 0
self.gps_speed = 0
self.gps_x = 0
self.gps_y = 0
self.gyro_heading = 0
self.imu_speed = 0
self.imu_x = 0
self.imu_y = 0
self.imu_heading = 0
self.imu_v = 0
self.imu_r = 0
self.ekf_speed = 0
self.ekf_x = params['START_X']
self.ekf_y = params['START_Y']
self.ekf_heading = params['START_HEADING']
self.n1 = 0
self.n2 = 0
self.n3 = 0
self.beta1 = 0
self.beta2 = 0
self.eta_hat = [
self.ekf_speed, 0, 0, self.ekf_x, self.ekf_y,
hdg2yaw(self.ekf_heading)
]
self.P = 1e6 * np.eye(6)
self.Q = 1e6 * np.diagflat([1, 1, 1, 1, 1, 1])
e_IMU_spd = 0.12**2
e_IMU_pos = 1.2**2
e_IMU_hdg = np.deg2rad(0.08)**2
e_IMU_rot = np.deg2rad(0.03)**2
e_GPS_pos = 1.2**2
e_GPS_spd = 0.03**2
e_gyro_hdg = np.deg2rad(0.25)**2
e_DVL_spd = 0.002**2
self.R = np.diagflat([
e_GPS_spd, e_DVL_spd, e_IMU_spd, e_IMU_spd, e_IMU_rot, e_GPS_pos,
e_IMU_pos, e_GPS_pos, e_IMU_pos, e_gyro_hdg, e_IMU_hdg
])
self.run(self.server, self.port, self.name)
pymoos.set_moos_timewarp(params['MOOSTimeWarp'])
self.dt = 0.1
def __init__(self):
self.comms = pymoos.comms()
self.comms.set_on_connect_callback(self.connect_callback)
self.comms.set_on_mail_callback(self.message_received)
pymoos.set_moos_timewarp($(WARP))
self.comms.set_comms_control_timewarp_scale_factor(0.4)
self.comms.run('localhost', 9000, 'pPathPlan')
# Initialize stored variables
self.messages = dict()
self.messages['SWATH_EDGE'] = ''
self.messages['SWATH_WIDTH_RECORD'] = ''
self.messages['NEXT_SWATH_SIDE'] = 'stbd'
self.messages['TURN_REACHED'] = 'false'
self.post_ready = False
self.post_message = ''
self.turn_pt_message = ''
self.start_line_message = ''
self.need_to_process = False
self.post_next_turn = False
self.post_end = False
self.turn_count = 0
# self.op_poly = [(16,-45), (50,-150), \
# (-85, -195), (-122, -70)]
# SH 15 North Survey area:
# self.op_poly = [(4075.0, -650.0), (3293, -2464), (2405, -2259), \
# (3180, -387)]
# SH15 South Survey Area:
# self.op_poly = [(2497.0, -4374.0), (1727, -6077), (588, -5468), \
# (1272, -3864)]
# Small region for half step testing, SH 2015
# self.op_poly = [(655, -4429), (550, -4813), (198, -4725), (300, -4353)]
self.op_poly = $(OP_POLY)
# Newport, RI
#self.op_poly = [ (-229,47), (-279,217), (-41,264), (0,100)]
# South of Pier
#self.op_poly = [ (-167, -194), (-136, -342), (199, -255), (142, -107) ]
# Smaller South of Pier
# self.op_poly = [ (-210, -192), (-191,-307), (10,-254), (-16,-144)]
# VIEW_SEGLIST = "pts={10,-26:16,-45},label=emily_waypt_line_start,
# label_color=white,edge_color=white,vertex_color=dodger_blue,
# vertex_size=3,edge_size=1"
pts_list = [str(pt[0]) + ',' + str(pt[1]) + ':' for pt in self.op_poly]
# Back to the beginning
pts_list.append(str(self.op_poly[0][0]) + ',' + str(self.op_poly[0][1]))
pts_string = ''.join(pts_list)
region_message = 'pts={' + pts_string + '},label=op_region,' + \
'label_color=red,edge_color=red,vertex_color=red,edge_size=2'
# Make sure we can send the message
if self.comms.wait_until_connected(2000):
print 'Posting op region: ' + pts_string
self.comms.notify('VIEW_SEGLIST', region_message, pymoos.time())
print('Updating lines')
# Put these in a function
point_list = [str(pt[0]) + ',' + str(pt[1]) + ':' for pt in self.op_poly[0:2]]
points_message = ''.join(point_list)
points_message = 'points=' + points_message[:-1]
self.comms.notify('SURVEY_UPDATE', points_message, pymoos.time())
start_heading = (self.op_poly[0][0] - self.op_poly[1][0], \
self.op_poly[0][1] - self.op_poly[1][1])
start_heading = pathplan.unit_vector(start_heading)
start_line_message = 'points=' + \
str(self.op_poly[0][0] + start_heading[0] * ALIGNMENT_LINE_LEN) + ',' \
+ str(self.op_poly[0][1] + start_heading[1] * ALIGNMENT_LINE_LEN) + \
':' + str(self.op_poly[0][0]) + ',' + str(self.op_poly[0][1])
self.comms.notify('START_UPDATE', start_line_message, pymoos.time())
end_heading = (self.op_poly[1][0] - self.op_poly[0][0], \
self.op_poly[1][1] - self.op_poly[0][1])
end_heading = pathplan.unit_vector(end_heading)
turn_pt_message = 'point=' + \
str(self.op_poly[1][0] + end_heading[0] * TURN_PT_OFFSET) + ',' \
+ str(self.op_poly[1][1] + end_heading[1] * TURN_PT_OFFSET)
self.comms.notify('TURN_UPDATE', turn_pt_message, pymoos.time())
home_message = 'station_pt = ' + str(self.op_poly[0][0] + \
start_heading[0] * 30) + ',' + str(self.op_poly[0][1] + \
start_heading[1] * 30)
self.comms.notify('HOME_UPDATE', home_message, pymoos.time())
# move boat to start
# self.comms.notify('NAV_X', str(self.op_poly[0][0] + \
# start_heading[0] * 30), pymoos.time())
# self.comms.notify('NAV_Y', str(self.op_poly[0][1] + \
# start_heading[1] * 30), pymoos.time())
# pdb.set_trace()
else:
print 'Timed out connecting before sending op region'
