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)