def setUp(self): path_a = '../config/node_desc_a.json' path_b = '../config/node_desc_b.json' try: f = open(path_a, 'r') node_descriptor_a = json.load(f) f.close() except Exception as e: print(repr(e)) print('Could not open given node file {}'.format(path_a)) return -1 try: f = open(path_b, 'r') node_descriptor_b = json.load(f) f.close() except Exception as e: print(repr(e)) print('Could not open given node file {}'.format(path_b)) return -1 self.node_a = node.Node('localhost', 1883, node_descriptor_a) self.node_a.start() self.node_b = node.Node('localhost', 1883, node_descriptor_b) self.node_b.start() self.vizier = vizier.Vizier('localhost', 1883, ['b']) self.vizier.start()
def main(): # Parse Command Line Arguments parser = argparse.ArgumentParser() parser.add_argument("node_descriptor", help=".json file node information") parser.add_argument("-port", type=int, help="MQTT Port", default=8080) parser.add_argument("-host", help="MQTT Host IP", default="localhost") args = parser.parse_args() # Ensure that Node Descriptor File can be Opened node_descriptor = None try: f = open(args.node_descriptor, 'r') node_descriptor = json.load(f) f.close() except Exception as e: print(repr(e)) print("Couldn't open given node file " + args.node_descriptor) return -1 # Start the Node node = vizier_node.Node(args.host, args.port, node_descriptor) node.start() time.sleep(1) # Get the links for Publishing/Subscribing publishable_link = list(node.publishable_links)[0] tick = time.time() while time.time()-tick<=10: output = 'Seconds since start: ' + str(time.time()-tick) node.publish(publishable_link,output) node.stop()
def main(): # Parse Command Line Arguments parser = argparse.ArgumentParser() parser.add_argument("node_descriptor", help=".json file node information") parser.add_argument("-port", type=int, help="MQTT Port", default=8080) parser.add_argument("-host", help="MQTT Host IP", default="localhost") parser.add_argument("-val", type=int, help="The initial condition for this test", default=1) args = parser.parse_args() # Ensure that Node Descriptor File can be Opened node_descriptor = None try: f = open(args.node_descriptor, 'r') node_descriptor = json.load(f) f.close() except Exception as e: print(repr(e)) print("Couldn't open given node file " + args.node_descriptor) return -1 # Start the Node node = vizier_node.Node(args.host, args.port, node_descriptor) node.start() # Get the links for Publishing/Subscribing subscribable_link = list(node.subscribable_links)[0] msg_queue = node.subscribe(subscribable_link) publishable_link = list(node.publishable_links)[0] # This will be printed from paragraph = "The quick brown fox jumps over the lazy dog.".split(' ') # Keep Track of the Initial Conditions so that Output is Ordered init_val = args.val val = init_val if init_val == 0: node.publish(publishable_link, str(val)) # Loop until the string is printed, then terminate while True: try: val = int( msg_queue.get(timeout=10).payload.decode(encoding='UTF-8')) except Exception as e: if init_val == 0: node.publish(publishable_link, str(val)) continue if (val >= len(paragraph)): node.publish(publishable_link, str(val + 1)) break print(paragraph[val]) node.publish(publishable_link, str(val + 1)) node.stop()
def main(): # Parse Command Line Arguments parser = argparse.ArgumentParser() parser.add_argument("node_descriptor", help=".json file node information") parser.add_argument("-port", type=int, help="MQTT Port", default=8080) parser.add_argument("-host", help="MQTT Host IP", default="localhost") args = parser.parse_args() # Ensure that Node Descriptor File can be Opened node_descriptor = None try: f = open(args.node_descriptor, 'r') node_descriptor = json.load(f) f.close() except Exception as e: print(repr(e)) print("Couldn't open given node file " + args.node_descriptor) return -1 # Start the Node node = vizier_node.Node(args.host, args.port, node_descriptor) node.start() # Get the links for Publishing/Subscribing publishable_link = list(node.publishable_links)[0] subscribable_link = list(node.subscribable_links)[0] msg_queue = node.subscribe(subscribable_link) # Set the initial condition state = 10 * random.random() - 5 dt = 0.0 node.publish(publishable_link, str(state)) print('\n') while True: tick = time.time() try: message = msg_queue.get(timeout=0.1).payload.decode( encoding='UTF-8') input = float(message) except KeyboardInterrupt: break except: input = 0.0 tock = time.time() dt = tock - tick state = state + dt * input print('State = {}'.format(state), end='\r') node.publish(publishable_link, str(state)) print('\n') node.stop()
def setUp(self): # Ensure that we can open the nodes file filepath = '../config/node_desc_a.json' try: f = open(filepath, 'r') node_descriptor = json.load(f) f.close() except Exception as e: print(repr(e)) print('Could not open given node file {}'.format(filepath)) return -1 self.node = node.Node('localhost', 1883, node_descriptor) self.node.start()
def main(): # Parse Command Line Arguments parser = argparse.ArgumentParser() parser.add_argument("node_descriptor", help=".json file node information") parser.add_argument("-port", type=int, help="MQTT Port", default=8080) parser.add_argument("-host", help="MQTT Host IP", default="localhost") args = parser.parse_args() # Ensure that Node Descriptor File can be Opened node_descriptor = None try: f = open(args.node_descriptor, 'r') node_descriptor = json.load(f) f.close() except Exception as e: print(repr(e)) print("Couldn't open given node file " + args.node_descriptor) return -1 # Start the Node node = vizier_node.Node(args.host, args.port, node_descriptor) node.start() # Get the links for Publishing/Subscribing publishable_link = list(node.publishable_links)[0] subscribable_link = list(node.subscribable_links)[0] msg_queue = node.subscribe(subscribable_link) # Control stuffs ref = 5.0 state = np.inf print('\n') while abs(ref - state) >= 0.001: try: message = msg_queue.get(timeout=1).payload.decode(encoding='UTF-8') state = float(message) except KeyboardInterrupt: break except: continue print('Control input = {}'.format(ref - state), end='\r') node.publish(publishable_link, str(ref - state)) print('\n') node.stop()
def main(): # Parse Command Line Arguments parser = argparse.ArgumentParser() parser.add_argument("node_descriptor", help=".json file node information") parser.add_argument("-port", type=int, help="MQTT Port", default=8080) parser.add_argument("-host", help="MQTT Host IP", default="localhost") args = parser.parse_args() # Ensure that Node Descriptor File can be Opened node_descriptor = None try: f = open(args.node_descriptor, 'r') node_descriptor = json.load(f) f.close() except Exception as e: print(repr(e)) print("Couldn't open given node file " + args.node_descriptor) return -1 # Start the Node node = vizier_node.Node(args.host, args.port, node_descriptor) node.start() # Get the links for Publishing/Subscribing subscribable_link = list(node.subscribable_links)[0] msg_queue = node.subscribe(subscribable_link) # Loop for 9ish seconds, then terminate val = 0 while val <= 9: try: message = msg_queue.get(timeout=10).payload.decode( encoding='UTF-8') val = float(message.replace(' ', '').split(':')[1]) print(message) except KeyboardInterrupt: break except: pass node.stop()
def main(): parser = argparse.ArgumentParser() # Optional arguments parser.add_argument('--params', help='path to detector AruCo parameters (YAML file)', default='config/detector_params.yml') parser.add_argument('--calib', help='path to camera calibration (YAML file)', default='config/camera_calib.yml') parser.add_argument('--dev', type=int, help='Input video device number', default=0) parser.add_argument('--output', help='Output path for homography (YAML file)', default='./output.yaml') parser.add_argument('--width', type=int, help='Width of camera frame (pixels)', default=1920) parser.add_argument('--height', type=int, help='Height of camera frame (pixels)', default=1080) parser.add_argument('--host', help='IP of MQTT broker', default='localhost') parser.add_argument('--port', type=int, help='Port of MQTT broker', default=1884) parser.add_argument('--query', help='How often to query robots for status data (e.g., battery voltage)', type=float, default=2) # Required arguments parser.add_argument('desc', help='Path to Vizier node descriptor for tracker (JSON file)') parser.add_argument('ref', help='Path to reference marker (YAML file)') args = parser.parse_args() try: f = open(args.desc, 'r') node_descriptor = json.load(f) f.close() except Exception as e: print(repr(e)) print("Could not open given node file " + args.node_descriptor) return -1 tracker_node = node.Node(args.host, args.port, node_descriptor) # Set up capture device. Should be a webcam! cap = cv.VideoCapture(args.dev) if not cap.isOpened(): print("Could not open video camera. Exiting") return # Set width and height of frames in pixels cap.set(3, args.width) cap.set(4, args.height) # HAVE to change codec for frame rate codec = cv.VideoWriter_fourcc('M', 'J', 'P', 'G') cap.set(cv.CAP_PROP_FOURCC, codec) # Apparently, we NEED to set FPS here... cap.set(cv.CAP_PROP_FPS, 30) # Load aruco parameters from file aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_100) parameters = aruco.DetectorParameters_create() utils.load_detector_params(args.params, parameters) # Load camera calibration from file cam_matrix, dist_coeffs, proj_matrix = utils.load_camera_calib(args.calib) # Load reference markers so we can ignore them reference_markers, ref_markers_world = utils.load_ref_markers(args.ref) # Start camera capture thread in backgroun read_from_camera_running = [True] frame_queue = queue.Queue() read_from_camera_thread = threading.Thread(target=read_from_camera, args=(cap, frame_queue, read_from_camera_running)) read_from_camera_thread.start() possible_ids = set({x.split('/')[0] for x in tracker_node.gettable_links}) poses = dict({x: {'x': 0, 'y': 0, 'theta': 0, 'batt_volt': -1, 'charge_status': False} for x in possible_ids}) getting_data = dict({x: {'viz': False, 'image': False} for x in possible_ids}) print('Tracking robots:', possible_ids) # This is a list containing a bool so we can pass by reference get_data_thread_running = [True] get_data_thread = threading.Thread(target=get_data, args=(tracker_node, list(possible_ids), poses, getting_data, get_data_thread_running), kwargs={'query_every': args.query}) get_data_thread.start() # Start MQTT client tracker_node.start() publish_topic = list(tracker_node.publishable_links)[0] # Initialize exponential filter for FPS avg_proc_time = 0.033 # Gain for exponential filter alpha = 0.1 # For homography H = None # STATE VARIABLE state = States.HOMOGRAPHY # Main loop of program while(True): start = time.time() ret, frame = frame_queue.get(timeout=TIMEOUT) # ret, frame are now set and the queue is empty after this block while True: try: ret, frame = frame_queue.get_nowait() except queue.Empty: break if(not ret): print('Could not get frame') # Skip this iteration if there is not a valid frame continue # convert to grayscale and find markers with aruco gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY) corners, ids, _ = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) # I can do what I want with corners now aruco.drawDetectedMarkers(gray, corners, ids, _VISIBLE_COLOR) # Find the homography from image data if(state == States.HOMOGRAPHY): if(len(corners) > 0): for i in range(ids.shape[0]): cv.undistortPoints(corners[i], cam_matrix, dist_coeffs, dst=corners[i], P=proj_matrix) ref_markers_image = np.zeros((len(reference_markers), 2)) found = 0 if ids is not None: for i in range(ids.shape[0]): if ids[i][0] in reference_markers: found += 1 ref_markers_image[reference_markers[ids[i][0]], :] = np.mean(corners[i][0], axis=0) # Compute mean along columns H = None if found == len(reference_markers): print('Found', '(' + repr(found) + ')', 'all', '(' + repr(len(reference_markers)) + ')', 'reference markers. Calculating homography') H = cv.findHomography(ref_markers_image, ref_markers_world)[0] # Make sure that homography is valid if H is not None: if(abs(np.linalg.det(H)) <= 0.001): print('Warning: H is close to losing invertibility') print('Homography found. Going to tracking state') state = States.TRACKING # GRAPHICS cv.putText(gray, 'Searching for reference_markers: ' + repr([x for x in reference_markers]), (10, 30), cv.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), thickness=1) # Tracking state. Estimate poses of robots using homography elif(state == States.TRACKING): to_send = {} if(len(corners) > 0): for i in range(ids.shape[0]): tag_id = ids[i][0] tag_id_str = repr(tag_id) if(tag_id in reference_markers): continue if tag_id_str not in possible_ids: print('Got id:', tag_id, 'not in possible ids (see node descriptor)') continue cv.undistortPoints(corners[i], cam_matrix, dist_coeffs, dst=corners[i], P=proj_matrix) # Convert points to homogeneous -> homography -> convert back hom = cv.convertPointsToHomogeneous(np.array([np.mean(corners[i][0], axis=0)])) cv.transform(hom, H, dst=hom) tag_pos = cv.convertPointsFromHomogeneous(hom) hom = cv.convertPointsToHomogeneous(corners[i][0]) # DON'T use dst=hom here. Seems to mess with the calculations hom = cv.transform(hom, H) tag_pos = cv.convertPointsFromHomogeneous(hom) # Compute position as the average of the positions of the four corners position = 0.25 * (tag_pos[0][0] + tag_pos[1][0] + tag_pos[2][0] + tag_pos[3][0]) # TODO: What is this? forward_vector = 0.5 * (tag_pos[1][0] + tag_pos[2][0] - tag_pos[0][0] - tag_pos[3][0]) # Assemble poses JSON in the assumed format # TODO: If I want to share this between threads, I can't re-init the dict. It blows away the other changes poses[tag_id_str]['x'] = round(float(position[0]), 2) poses[tag_id_str]['y'] = round(float(position[1]), 2) poses[tag_id_str]['theta'] = round(float(np.math.atan2(forward_vector[1], forward_vector[0])), 2) getting_data[tag_id_str]['image'] = True # Only send pose of active robots if(getting_data[tag_id_str]['image'] and getting_data[tag_id_str]['viz']): to_send[tag_id_str] = poses[tag_id_str] # Send poses on if(len(to_send) > 0): tracker_node.publish(publish_topic, json.dumps(to_send).encode()) print(to_send) # Update exponential filter for FPS elapsed = time.time() - start avg_proc_time = (1 - alpha) * avg_proc_time + alpha * elapsed # GRAPHICS STUFF cv.putText(gray, 'FPS: ' + repr(int(1 / avg_proc_time)) + ' <- should be > 30', (10, 30), cv.FONT_HERSHEY_PLAIN, 1, _VISIBLE_COLOR, thickness=1) cv.putText(gray, 'Q: ' + repr(frame_queue.qsize()) + ' <- should be small', (10, 50), cv.FONT_HERSHEY_PLAIN, 1, _VISIBLE_COLOR, thickness=1) # for i in range(len(rvecs)): # aruco.drawAxis(gray, cam_matrix, dist_coeffs, rvecs[i], tvecs[i], 0.1) cv.putText(gray, 'Press "h" to re-find homography', (10, 70), cv.FONT_HERSHEY_PLAIN, 1, _VISIBLE_COLOR, thickness=1) else: # This should NEVER happen print('Critical error in state. State undefined') state = States.HOMOGRAPHY cv.imshow('Tag Tracker', gray) key = cv.waitKey(1) # If 'q' is pressed, quit the main loop if(key == ord('q')): break # If 'h' is pressed, re-find the homography if(key == ord('h')): state = States.HOMOGRAPHY # Stop MQTT client tracker_node.stop() # Terminate the thread that's running in the background get_data_thread_running[0] = False read_from_camera_running[0] = False read_from_camera_thread.join() get_data_thread.join() cap.release() cv.destroyAllWindows()
connection_string = "/dev/serial/by-id/usb-3D_Robotics_PX4_FMU_v2.x_0-if00" vehicle = connect(connection_string, wait_ready=True) #connect over mavlink vehicle.armed = True # Ensure that Node Descriptor File can be Opened node_descriptor = None try: f = open("node_desc_robot.json", 'r') node_descriptor = json.load(f) f.close() except Exception as e: print(repr(e)) print("Couldn't open given node file node_desc_robot.json") # Start the Node node = vizier_node.Node("192.168.0.2", 1884, node_descriptor) node.start() # Get the links for Publishing/Subscribing publishable_link = list(node.publishable_links)[0] subscribable_link = list(node.subscribable_links)[0] msg_queue = node.subscribe(subscribable_link) #Initialize Connection print("Connecting to Controller") node.publish(publishable_link, "1") message = msg_queue.get(timeout=10).decode(encoding='UTF-8') recieved = False
def __init__(self, broker_ip: str, broker_port: int, node_descriptor: Dict): self.vizier_node = node.Node(broker_ip, broker_port, node_descriptor) self.active = False self.subscribables = {}
def main(): # Parse Command Line Arguments parser = argparse.ArgumentParser() # parser.add_argument("node_descriptor", help=".json file node information") parser.add_argument("-port", type=int, help="MQTT Port", default=8080) parser.add_argument("-host", help="MQTT Host IP", default="localhost") args = parser.parse_args() #endpoint = "matlab_api" endpoint = "monitor" api_link = 'matlab_api/inputs' # TODO: Get the ids somehow all_ids = list(range(50)) tracker_link = 'overhead_tracker/all_robot_pose_data' # Ensure that Node Descriptor File can be Opened node_descriptor = create_node_descriptor(endpoint, api_link, tracker_link, all_ids) # try: # f = open(args.node_descriptor, 'r') # node_descriptor = json.load(f) # f.close() # except Exception as e: # print(repr(e)) # print("Couldn't open given node file " + args.node_descriptor) # return -1 # Start the Node started = False robot_node = None while (not started): node = vizier_node.Node(args.host, args.port, node_descriptor) try: node.start() started = True except Exception as e: node.stop() time.sleep(1) # Get the links for Publishing/Subscribing # publishable_link = list(node.publishable_links)[0] # Queues for STREAM links inputs = node.subscribe(api_link) poses = node.subscribe(tracker_link) vel_commands = np.zeros([2, len(all_ids)]) last_input_msg = None message_timestamp = 0 robust_barriers = create_robust_barriers() # Main Loop while True: # Process input commands input_msg = None poses_msg = None # Make sure that the queue has few enough messages # if(inputs.qsize() > MAX_QUEUE_SIZE): # logger.critical('Queue of motor messages is too large.') # TODO: Add Timeout in poses.get() try: poses_msg = poses.get(timeout=0.1) except: poses_msg = None try: # Clear out the queue while True: poses_msg = poses.get_nowait() except queue.Empty: pass try: # Clear out the queue while True: input_msg = inputs.get_nowait() except queue.Empty: pass if (poses_msg is not None): try: poses_msg = json.loads(poses_msg.decode(encoding='UTF-8')) except Exception as e: # logger.warning('Got malformed JSON motor message ({})'.format(poses_msg)) # logger.warning(e) # Set this to None for the next checks poses_msg = None if (input_msg is not None): try: input_msg = json.loads(input_msg.decode(encoding='UTF-8')) except Exception as e: # logger.warning('Got malformed JSON motor message ({})'.format(input_msg)) # logger.warning(e) # Set this to None for the next checks input_msg = None # If we got a valid JSON input msg, look for appropriate commands if (input_msg is not None): last_input_msg = input_msg message_timestamp = time.time() else: input_msg = last_input_msg if (poses_msg is not None) and (time.time() - message_timestamp) < 0.1: active_ids = input_msg['inputs'].keys() # IDs that got inputs last_poses_msg = poses_msg tracked_ids = poses_msg.keys() # TODO: Watchout for instances when an active_id isn't being tracked! active_poses = np.zeros([3, len(tracked_ids)]) pre_inputs = np.zeros([2, len(tracked_ids)]) # Pre Barrier Inputs left_leds = np.zeros([3, len(tracked_ids)]) # Pass the LED Inputs right_leds = np.zeros([3, len(tracked_ids)]) # Pass the LED Inputs inactive_poses = np.zeros([2, len(tracked_ids)]) active_ids_2 = [] # IDs that are tracked and active inactive_ids = [] # IDs that are tracked and did not get inputs counters = [0, 0] print('Input Message:') print(input_msg['inputs']) for id in tracked_ids: if id in active_ids and input_msg['inputs'][id] != [0, 0]: active_ids_2.append(id) active_poses[:, counters[0]] = [ poses_msg[id]['x'], poses_msg[id]['y'], poses_msg[id]['theta'] ] pre_inputs[:, counters[0]] = input_msg['inputs'][id] left_leds[:, counters[0]] = input_msg['leds'][id][0] right_leds[:, counters[0]] = input_msg['leds'][id][1] counters[0] += 1 else: inactive_ids.append(id) inactive_poses[:, counters[1]] = [ poses_msg[id]['x'], poses_msg[id]['y'] ] counters[1] += 1 error_ids = [] # Active ids that are not being tracked for id in active_ids: if id not in active_ids_2: error_ids.append(id) # Reshapes & Calls the Barrier Function # Truncates Back To Right-Size pre_inputs = pre_inputs[:, :counters[0]] active_poses = active_poses[:, :counters[0]] if len(inactive_ids) == 0: inactive_poses = np.array([]) else: inactive_poses = inactive_poses[:, :counters[1]] # Post-Barrier Inputs start = time.time() vel_commands = robust_barriers(pre_inputs, active_poses, inactive_poses) print('Time for BC: {}'.format(time.time() - start)) # Iterate & Publish for i in range(len(active_ids_2)): id = active_ids_2[i] input_dict = { 'v': vel_commands[0, i], 'w': vel_commands[1, i], 'left_led': left_leds[:, i].tolist(), 'right_led': right_leds[:, i].tolist() } node.publish(endpoint + '/{}'.format(id), json.dumps(input_dict)) else: # Didn't get Poses, publish 0 velocities as a safety measures. # Iterate & Publish for i in range(len(all_ids)): id = all_ids[i] input_dict = { 'v': 0, 'w': 0, 'left_led': [0, 0, 0], 'right_led': [0, 0, 0] } node.publish(endpoint + '/{}'.format(id), json.dumps(input_dict)) node.stop()
def main(): logger = log.get_logger() parser = argparse.ArgumentParser() parser.add_argument("mac_list", help="JSON file containing MAC to id mapping") parser.add_argument("-port", type=int, help="MQTT Port", default=8080) parser.add_argument("-host", help="MQTT Host IP", default="localhost") # Retrieve the MAC address for the robot mac_address = get_mac() # Parser and set CLI arguments args = parser.parse_args() # Retrieve the MAC list file, containing a mapping from MAC address to robot ID try: f = open(args.mac_list, 'r') mac_list = json.load(f) except Exception as e: print(repr(e)) print('Could not open file ({})'.format(args.node_descriptor)) if (mac_address in mac_list): robot_id = mac_list[mac_address] else: print( 'MAC address {} not in supplied MAC list file'.format(mac_address)) raise ValueError() node_descriptor = create_node_descriptor(robot_id) started = False pd_node = None while (not started): pd_node = node.Node(args.host, args.port, node_descriptor) try: pd_node.start() started = True except Exception as e: logger.critical('Could not start robot node.') logger.critical(repr(e)) pd_node.stop() # Don't try to make nodes too quickly time.sleep(1) status_link = list(pd_node.puttable_links)[0] input_link = list(pd_node.subscribable_links)[0] status_data = {} input_q = pd_node.subscribe(input_link) # Initially set GPIO pins high for x in PINS: logger.info('Setting pin {0} as output'.format(x)) GPIO.setup(x, GPIO.OUT) for x in PINS: logger.info('Setting pin {0} high'.format(x)) GPIO.output(x, GPIO.HIGH) while True: print(input_q.qsize()) msg = input_q.get() # Clear out other messages while True: try: msg = input_q.get_nowait() except queue.Empty: break # msg now contains latest msg try: msg = json.loads(msg.decode(encoding='UTF-8')) except Exception as e: logger.warning('Malformed json message.') logger.warning(repr(e)) continue status_data = msg pd_node.put(status_link, json.dumps(status_data)) # At this point, msg contains a valid JSON message # structure is {'state': 0/1} if ('state' in msg): states = msg['state'] if len(states) == len(PINS): for i in range(len(states)): if (states[i] is 1): logger.info('Setting GPIO pin {} high.'.format( PINS[i])) GPIO.output(PINS[i], GPIO.HIGH) else: logger.info('Setting GPIO pin {} low.'.format(PINS[i])) GPIO.output(PINS[i], GPIO.LOW) # We cannot switch GPIO too quickly! time.sleep(0.1) else: logger.warning( 'Expected a message of length {0} but got {1}. Message: {2}' .format(len(PINS), len(states), states))
def main(): parser = argparse.ArgumentParser() parser.add_argument("mac_list", help="JSON file containing MAC to id mapping") parser.add_argument("-port", type=int, help="MQTT Port", default=8080) parser.add_argument("-host", help="MQTT Host IP", default="localhost") parser.add_argument('-update_rate', type=float, help='Update rate for robot main loop', default=0.016) parser.add_argument('-status_update_rate', type=float, help='How often to check status info', default=1) # Retrieve the MAC address for the robot mac_address = get_mac() # Parser and set CLI arguments args = parser.parse_args() update_rate = args.update_rate status_update_rate = args.status_update_rate # Retrieve the MAC list file, containing a mapping from MAC address to robot ID try: f = open(args.mac_list, 'r') mac_list = json.load(f) except Exception as e: print(repr(e)) print('Could not open file ({})'.format(args.node_descriptor)) if (mac_address in mac_list): robot_id = mac_list[mac_address] else: print( 'MAC address {} not in supplied MAC list file'.format(mac_address)) raise ValueError() logger.info('This is robot: ({0}) with MAC address: ({1})'.format( robot_id, mac_address)) # Create node descriptor for robot and set up links node_descriptor = create_node_descriptor(mac_list[mac_address]) status_link = robot_id + '/status' input_link = 'matlab_api/' + robot_id started = False robot_node = None while (not started): robot_node = node.Node(args.host, args.port, node_descriptor) try: robot_node.start() started = True except Exception as e: logger.critical('Could not start robot node.') logger.critical(repr(e)) robot_node.stop() # Don't try to make nodes too quickly time.sleep(1) logger.info('Started robot node.') started = False serial = None while (not started): serial = gritsbotserial.GritsbotSerial(serial_dev='/dev/ttyACM0', baud_rate=500000) try: serial.start() started = True except Exception as e: # This class stops itself if the device cannot be initially acquired, so we don't need to stop it. logger.critical('Could not acquire serial device.') logger.critical(repr(e)) # Don't try to acquire the serial device too quickly time.sleep(1) logger.info('Acquired serial device.') # Queues for STREAM links inputs = robot_node.subscribe(input_link) # Initialize times for various activities start_time = time.time() print_time = time.time() status_update_time = time.time() # Initialize data status_data = {'batt_volt': -1, 'charge_status': False} last_input_msg = {} # Main loop for the robot while True: start_time = time.time() # Serial requests request = Request() handlers = [] # Retrieve status data: battery voltage and charging status if ((start_time - status_update_time) >= status_update_rate): request.add_read_request('batt_volt').add_read_request( 'charge_status') handlers.append(lambda status, body: handle_read_response( 'batt_volt', status, body)) handlers.append(lambda status, body: handle_read_response( 'charge_status', status, body)) status_update_time = start_time # Process input commands input_msg = None # Make sure that the queue has few enough messages if (inputs.qsize() > MAX_QUEUE_SIZE): logger.critical('Queue of motor messages is too large.') try: # Clear out the queue while True: input_msg = inputs.get_nowait() except queue.Empty: pass if (input_msg is not None): try: input_msg = json.loads(input_msg.decode(encoding='UTF-8')) except Exception as e: logger.warning( 'Got malformed JSON motor message ({})'.format(input_msg)) logger.warning(e) # Set this to None for the next checks input_msg = None # If we got a valid JSON input msg, look for appropriate commands if (input_msg is not None): last_input_msg = input_msg if ('v' in input_msg and 'w' in input_msg): # Handle response? request.add_write_request('motor', { 'v': input_msg['v'], 'w': input_msg['w'] }) handlers.append(handle_write_response) if ('left_led' in input_msg): request.add_write_request('left_led', {'rgb': input_msg['left_led']}) handlers.append(handle_write_response) if ('right_led' in input_msg): request.add_write_request('right_led', {'rgb': input_msg['right_led']}) handlers.append(handle_write_response) # Write to serial port response = None if (len(handlers) > 0): try: response = serial.serial_request(request.to_json_encodable()) except Exception as e: logger.critical('Serial exception.') logger.critical(e) # Call handlers # We'll have a status and body for each request if (response is not None and 'status' in response and 'body' in response and len(response['status']) == len(handlers) and len(response['body']) == len(handlers)): status = response['status'] body = response['body'] # Ensure the appropriate handler gets each response for i, handler in enumerate(handlers): status_data.update(handler(status[i], body[i])) else: # If we should have responses, but we don't if (len(handlers) > 0): logger.critical('Malformed response ({})'.format(response)) robot_node.put(status_link, json.dumps(status_data)) # Print out status data if ((start_time - print_time) >= status_update_rate): logger.info('Status data ({})'.format(status_data)) logger.info( 'Last input message received ({})'.format(last_input_msg)) print_time = time.time() # Sleep for whatever time is left at the end of the loop time.sleep(max(0, update_rate - (time.time() - start_time)))
tx = int(width * value) pygame.draw.rect(screen, BLUE, [x + width, y, tx, 20]) # Ensure that Node Descriptor File can be Opened node_descriptor = None try: f = open("node_desc_controller.json", 'r') node_descriptor = json.load(f) f.close() except Exception as e: print(repr(e)) print("Couldn't open given node file node_desc_controller.json") # Start the Node node = vizier_node.Node("localhost", 1884, node_descriptor) node.start() # Get the links for Publishing/Subscribing publishable_link = list(node.publishable_links)[0] subscribable_link = list(node.subscribable_links)[0] msg_queue = node.subscribe(subscribable_link) pygame.init() size = (640, 480) screen = pygame.display.set_mode(size) # Used to manage how fast the screen updates clock = pygame.time.Clock() done = False