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
