def send_msg(self, var, val):
# heading command = yaw_com
# 'north_com'
# 'east_com',
# 'depth_com'
# 'yaw_com',
# vel_com
if isinstance(val, str) or isinstance(val, float):
self.comms.notify(var, val, pm.time())
else:
self.comms.notify(var, str(val), pm.time())
def run(self):
while True:
time.sleep(0.1)
if self.post_ready:
print 'Notifying MOOSDB with swath width'
if OUTPUT_MODE == "Depth":
self.comms.notify('SONAR_DEPTH_PORT_M', float(self.post_port), pymoos.time())
self.comms.notify('SONAR_DEPTH_STBD_M', float(self.post_stbd), pymoos.time())
self.post_ready = False
else:
self.comms.notify('SWATH_WIDTH', self.get_post_message(), \
pymoos.time())
def main():
# my code h e r e
comms.set_on_connect_callback(on_connect)
comms.add_active_queue('the_queue', m)
comms.add_message_route_to_active_queue('the_queue', 'simple_var')
comms.add_active_queue('the_queue2', m2)
comms.add_message_route_to_active_queue('the_queue2', 'test')
comms.run('localhost', 9000, 'pymoos')
i = 0
while True:
time.sleep(1)
comms.notify('simple_var', 'hello world%i' % i, pymoos.time())
comms.notify('test', 'held%i' % i, pymoos.time())
i = i + 1
def run(self):
while True:
time.sleep(0.1)
if self.post_ready:
print 'Notifying MOOSDB with new path\n'
self.comms.notify('NEW_PATH', self.get_post_message(), \
pymoos.time())
if self.post_next_turn and self.turn_count == 1:
print 'Posting turn update'
self.comms.notify('TURN_UPDATE', self.turn_pt_message, pymoos.time())
self.post_next_turn = False
if self.post_end:
self.comms.notify('FAULT', 'true', pymoos.time())
self.post_end = False
def test_10_notify_msg(self):
c = pymoos.comms()
c.set_on_connect_callback(
lambda: self.assertTrue(c.register('TEST_NOTIFY_MSG')))
c.run('localhost', 9000, 'test_notify_msg')
c.wait_until_connected(5000)
time.sleep(1)
self.assertTrue(c.is_registered_for('TEST_NOTIFY_MSG'))
t = pymoos.time()
self.assertTrue(c.notify('TEST_NOTIFY_MSG', 1., t))
time.sleep(1)
msgs = c.fetch()
self.assertNotEqual(len(msgs), 0)
for msg in msgs:
self.assertEqual(msg.key(), 'TEST_NOTIFY_MSG')
self.assertEqual(msg.name(), 'TEST_NOTIFY_MSG')
self.assertTrue(msg.is_name('TEST_NOTIFY_MSG'))
self.assertEqual(msg.double(), 1)
self.assertTrue(msg.is_double())
self.assertEqual(msg.source(), 'test_notify_msg')
self.assertFalse(msg.is_string())
self.assertFalse(msg.is_binary())
self.assertEqual(msg.time(), t)
c.close(True)
def main():
comms.set_on_connect_callback(c);
comms.set_on_mail_callback(m);
comms.run('localhost',9000,'pymoos')
while True:
time.sleep(1)
comms.notify('simple_var','a string',pymoos.time());
def main():
# my code here
comms.set_on_connect_callback(on_connect);
comms.run('localhost',9000,'pymoos')
while True:
time.sleep(1)
comms.notify('simple_var','hello world',pymoos.time());
map(lambda msg: msg.trace(), comms.fetch())
def main():
comms.set_on_connect_callback(c)
comms.set_on_mail_callback(m)
comms.run('localhost', 9000, 'pymoos')
while True:
time.sleep(1)
comms.notify('simple_var', 'a string', pymoos.time())
def main():
# my code here
comms.set_on_connect_callback(on_connect)
comms.run('localhost', 9000, 'pymoos')
while True:
time.sleep(1)
comms.notify('simple_var', 'hello world', pymoos.time())
map(lambda msg: msg.trace(), comms.fetch())
def main():
comms.set_on_connect_callback(c);
comms.add_active_queue('the_queue',queue_callback)
comms.add_message_route_to_active_queue('the_queue','binary_var')
comms.run('localhost',9000,'pymoos')
while True:
time.sleep(1)
x = bytearray( [0, 3, 0x15, 2, 6, 0xAA] )
comms.notify_binary('binary_var',bytes(x),pymoos.time());
def main():
comms.set_on_connect_callback(c);
comms.add_active_queue('the_queue',queue_callback)
comms.add_message_route_to_active_queue('the_queue','simple_var')
comms.run('localhost',9000,'pymoos')
while True:
time.sleep(1)
comms.notify('simple_var','hello world',pymoos.time());
def main():
comms.set_on_connect_callback(c)
comms.add_active_queue('the_queue', queue_callback)
comms.add_message_route_to_active_queue('the_queue', 'binary_var')
comms.run('localhost', 9000, 'pymoos')
while True:
time.sleep(1)
x = bytearray([0, 3, 0x15, 2, 6, 0xAA])
comms.notify_binary('binary_var', bytes(x), pymoos.time())
def main():
comms.set_on_connect_callback(c)
comms.add_active_queue('the_queue', queue_callback)
comms.add_message_route_to_active_queue('the_queue', 'simple_var')
comms.run('localhost', 9000, 'pymoos')
while True:
time.sleep(1)
comms.notify('simple_var', 'hello world', 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 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 run(self, host='localhost', port=9000, name='logger'):
self.comms.run(host, port, name)
first_row = True
for row in self.reader:
time = timedelta(seconds=float(row['time']))
if first_row:
self.now = datetime.now()
self.first_time = time
first_row = False
while datetime.now() < self.now + time - self.first_time:
sleep(0.000001)
if row['type'] == 'binary':
data = np.fromstring(row['data'][1:-1],
dtype=np.uint8,
sep=',')
msg = struct.pack('>dddH{}B'.format(len(data)),
float(row['bearing']), float(row['step']),
float(row['range_scale']), len(data), *data)
self.comms.notify_binary(row['key'], msg.decode('latin-1'),
pm.time())
else:
self.comms.notify(row['key'], float(row['data']), pm.time())
def run(self):
''' main program loop '''
self._moos.run('localhost',9000,'iBathy')
while True:
if self._update:
t = pymoos.time()
line = self.sim.mb_sample(self._x, self._y, self._h,depth=self._z,sensor=False)
num = len(line[:,0])
x_array = ",".join([f'{d:.2f}' for d in line[:,0]])
y_array = ",".join([f'{d:.2f}' for d in line[:,1]])
z_array = ",".join([f'{d:.2f}' for d in line[:,2]])
self._moos.notify('BATHY_X', f"t={t:.2f},n={num},x={x_array}")
self._moos.notify('BATHY_Y', f"t={t:.2f},n={num},y={y_array}")
self._moos.notify('BATHY_Z', f"t={t:.2f},n={num},z={z_array}")
self._logfile.write(f"iBATHY,{t:.2f},{num}\n")
self._logfile.write(f"{x_array}\n{y_array}\n{z_array}\n")
self._update = False
time.sleep(.1)
def main(argv):
comms.set_on_connect_callback(on_connect);
# comms.run('192.168.2.3',9000,'pHypackPath')
filename_hypack = argv[0]
# comms.run('10.42.0.15',9000,'pHypackPath')
print('Connecting to MOOSDB')
post_ip = '192.168.1.105'
if len(argv) > 1:
post_ip = argv[1]
comms.run(post_ip, 9000, 'pHypackPath')
if comms.wait_until_connected(2000):
line_reader = HypackLineReader(filename_hypack)
line_reader.read_file()
msg = line_reader.get_lines_msg()
print('Posting Message to MOOS')
print(msg)
comms.notify('UTM_SURVEYLINE', msg, pymoos.time())
else:
print('Timed out trying to send to survey system.')
comms.close(True)
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'
def notify(self,var,val,time=pymoos.time()):
self.comms.notify(var,val,time)
def build_filter(self, x, y, ASV_head, sensor_type, comms):
""" This function builds the OGR polygon that corresponds to the field
of view of the sensor and prints it to pMarnineViewer
Inputs:
x - Current x position of the ASV
y - Current y position of the ASV
ASV_head - Current heading of the ASV
sensor_type - Type of obstacles that the sensor can "see"
(Landmarks, Nav_aids, Underwater)
comms - Communication link to MOOS
Ouputs:
poly_filter - polygon that corresponds to the field of view of the
sensor
"""
# Get both Lat/long
lon, lat = self.MOOSxy2LonLat(x, y)
# Make sure that the heading is from 0-360
ASV_head = np.mod(self.ang4MOOS(ASV_head), 360)
# Build polygon for pMarineViewer
hypot = self.sensor_max_dist / np.cos(self.sensor_FoV_ang)
ang1 = self.sensor_head + ASV_head + self.sensor_FoV_ang
ang2 = self.sensor_head + ASV_head - self.sensor_FoV_ang
# Find the sensor FoV polygon as it rotates
A_x = x
A_y = y
B_x = x + hypot * np.cos(ang1 * np.pi / 180)
B_y = y + hypot * np.sin(ang1 * np.pi / 180)
C_x = x + hypot * np.cos(ang2 * np.pi / 180)
C_y = y + hypot * np.sin(ang2 * np.pi / 180)
# Print it to pMarnineViewer
if sensor_type == 'Underwater':
color = 'edge_color=blue'
elif sensor_type == 'Landmark':
color = 'edge_color=salmon'
elif sensor_type == 'Nav_aid':
color = 'edge_color=red'
FoV_Filter = 'pts={'+str(A_x)+','+str(A_y)+':'+str(B_x)+','+str(B_y)+\
':'+str(C_x)+','+str(C_y)+'},label=Sensor_FoV_'+sensor_type+','+color
comms.notify('VIEW_POLYGON', FoV_Filter, pymoos.time())
# Initialize filers
ring_filter = ogr.Geometry(ogr.wkbLinearRing)
poly_filter = ogr.Geometry(ogr.wkbPolygon)
# Build Lat/Long filter
B_lon, B_lat = self.MOOSxy2LonLat(B_x, B_y)
C_lon, C_lat = self.MOOSxy2LonLat(C_x, C_y)
ring_filter.AddPoint(lon, lat)
ring_filter.AddPoint(B_lon, B_lat)
ring_filter.AddPoint(C_lon, C_lat)
ring_filter.CloseRings()
poly_filter.AddGeometry(ring_filter)
return poly_filter
def test_20_zmsg_types(self):
x = bytearray([0, 3, 0x15, 2, 6, 0xAA])
s = 'hello'
d = 384.653
c = pymoos.comms()
def on_connect():
self.assertTrue(c.register('TEST_STRING_VAR', 0))
self.assertTrue(c.register('TEST_DOUBLE_VAR', 0))
self.assertTrue(c.register('TEST_BINARY_VAR', 0))
return True
c.set_on_connect_callback(on_connect)
c.run('localhost', 9000, 'test_zmsg_types')
c.wait_until_connected(2000)
self.assertTrue(c.is_registered_for('TEST_STRING_VAR'))
self.assertTrue(c.is_registered_for('TEST_DOUBLE_VAR'))
self.assertTrue(c.is_registered_for('TEST_BINARY_VAR'))
t = pymoos.time()
self.assertTrue(c.notify('TEST_STRING_VAR', s, t))
self.assertTrue(c.notify('TEST_DOUBLE_VAR', d, t))
self.assertTrue(c.notify_binary('TEST_BINARY_VAR', str(x), t))
time.sleep(1)
idx = 0
logger.debug('idx hit')
msgs = c.fetch()
logger.debug('fetch hit')
self.assertNotEqual(len(msgs), 0)
self.assertEqual(len(msgs), 3)
for msg in msgs:
logger.debug('iter hit')
self.assertEqual(msg.time(), t)
logger.debug('time hit')
if msg.key() == 'TEST_STRING_VAR':
logger.debug('string hit')
self.assertTrue(msg.is_string())
self.assertEqual(msg.string(), s)
self.assertFalse(msg.is_double())
self.assertFalse(msg.is_binary())
elif msg.key() == 'TEST_DOUBLE_VAR':
logger.debug('double hit')
self.assertTrue(msg.is_double())
self.assertEqual(msg.double(), d)
self.assertFalse(msg.is_string())
self.assertFalse(msg.is_binary())
elif msg.key() == 'TEST_BINARY_VAR':
logger.debug('binary hit')
self.assertTrue(msg.is_binary())
self.assertEqual(str(msg.binary_data()), str(x))
self.assertTrue(msg.is_string())
self.assertFalse(msg.is_double())
idx += 1
logger.debug('idx++ hit')
self.assertEqual(idx, 3)
c.close(True)
