def startMeshListener(self):
# make sure we find an unused port
while Comms.test_port(self.meshPort):
self.meshPort = random.randint(40000, 65000)
self.meshListener = Comms.create_server_socket("0.0.0.0",
self.meshPort)
class ExampleCameraPub():
def __init__(self):
self.name = "examplecamera"
self.cameraTurret = CameraTurret()
self.Comms = Comms()
self.Comms.add_publisher_port('127.0.0.1', '3002', 'cameraPath')
def sendPath(self, path):
self.Comms.define_and_send(self.name, 'cameraPath', path)
def run(self, is_sim):
"""
Check subscriber bus for path matrix. If path is found, clear cameraPath and replace with new path.
If no path is found,
"""
# Testing required to find q1_max and q2_max
while (True):
q_mat = self.cameraTurret.sweepPath(5,
q1_range=(-np.pi / 4,
np.pi / 4),
q2_range=(-np.pi / 4,
np.pi / 4))
self.sendPath(q_mat)
time.sleep(5)
class SmoothieHost(App):
#Factory.register('Comms', cls=Comms)
def __init__(self, **kwargs):
super(SmoothieHost, self).__init__(**kwargs)
self.comms = Comms(self)
if len(sys.argv) > 1:
# override com port
self.use_com_port = sys.argv[1]
else:
self.use_com_port = None
def build_config(self, config):
config.setdefaults(
'General', {
'last_gcode_path': os.path.expanduser("~"),
'last_print_file': '',
'serial_port': '/dev/ttyACM0'
})
def on_stop(self):
# The Kivy event loop is about to stop, stop the async main loop
self.comms.stop()
# stop the aysnc loop
def build(self):
return MainWindow()
def __init__(self):
self.name = "examplecamera"
self.cameraTurret = CameraTurret()
self.Comms = Comms()
self.Comms.add_publisher_port('127.0.0.1', '3002', 'cameraPath')
def __init__(self):
self.name = "gunmotion"
# Current configuration of gun turret as reported by hardware
self.actual_configuration = np.array([[0, 0]]).T
self.dest_configuration = np.array([[0, 0]]).T
# Configuration comparison tolerance (rad)
self.tol = .025
# First target indicator to skip delay
self.first_targ = True
# Time to wait for gun to fire
self.delay = 2
# queue of P0Ts to process
self.targets = []
self.gTurret = GunTurret(0, 0, 500, [-100, 50, 100])
self.Comms = Comms()
self.Comms.add_subscriber_port('127.0.0.1', '3001', 'gState')
self.Comms.add_subscriber_port('127.0.0.1', '3004', 'targetPos')
self.Comms.add_publisher_port('127.0.0.1', '3003', 'gunPath')
def _connect_callback(self):
if self.connected:
self.imu.stop()
self.motors.stop()
self.receiver.stop()
if self.comms is not None:
self.comms.stop()
self._clear()
self._disable_buttons()
self.button_connect['text'] = 'Connect'
self.hide(self.error_label)
self._show_splash()
else:
self.comms = Comms(self)
self.comms.start()
self.button_connect['text'] = 'Connecting ...'
self._disable_button(self.button_connect)
self._hide_splash()
self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)
self.connected = not self.connected
def __init__(self):
self.name = "examplepub"
self.gunTurret = GunTurret(0, 0, 40, [-100, 50, 100])
self.Comms = Comms()
self.Comms.add_publisher_port('127.0.0.1', '3003', 'gunPath')
class ExamplePub():
def __init__(self):
self.name = "examplepub"
self.gunTurret = GunTurret(0, 0, 40, [-100, 50, 100])
self.Comms = Comms()
self.Comms.add_publisher_port('127.0.0.1', '3003', 'gunPath')
def sendPath(self, path):
self.Comms.define_and_send(self.name, 'gunPath', path)
def run(self, is_sim):
"""
Check subscriber bus for path matrix. If path is found, clear cameraPath and replace with new path.
If no path is found,
"""
while (True):
q1, q2, toa, f = self.gunTurret.inverseKin()
q_mat = self.gunTurret.scurvePath(
np.array([0, 0]).reshape(2, 1),
np.array([q1, q2]).reshape(2, 1), 6, 1.5, .05)
self.sendPath(q_mat)
time.sleep(3)
def __init__(self, **kwargs):
super(SmoothieHost, self).__init__(**kwargs)
self.comms = Comms(self)
if len(sys.argv) > 1:
# override com port
self.use_com_port = sys.argv[1]
else:
self.use_com_port = None
def __init__(self):
#
# instantiate all our classes
self.comms = Comms(config.ORIGIN_NUM, config.TARGET_NUMS)
self.light = Light(config.CITY_NAME, config.LATITUDE, config.LONGITUDE,
config.SUNRISE_DELAY, config.SUNSET_DELAY)
self.door = Door(config.REVS)
self.camera = Camera(config.MAX_HORZ, config.MAX_VERT)
def process(self, message, tags, author='rabot32'):
if self.tag_types['message'] in tags:
comms = Comms()
if comms.direct_message(self.message_recipient, message) is True:
# If the message failed to send, then no 'messaged' tag will be added
tags.append(self.tag_types['messaged'])
if self.tag_types['store'] in tags:
vault = Vault()
vault.store(message, tags=['store'], author=author)
def __init__(self):
self.name = "camera"
self.cameraTurret = CameraTurret()
self.currentPos = np.array([0, 0]).reshape(2, 1)
self.currentTarget = np.array([])
self.gunPath = np.array([])
self.Comms = Comms()
self.Comms.add_subscriber_port('127.0.0.1', '3000', 'cState')
self.Comms.add_publisher_port('127.0.0.1', '3002', 'cameraPath')
self.Comms.add_subscriber_port('127.0.0.1', '3004', 'targetPos')
def main():
rospy.init_node('drop')
myCom = Comms(taskMode='drop')
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add('DISENGAGE',
Disengage(myCom),
transitions={
'started': 'SEARCHBOX',
'killed': 'killed'
})
smach.StateMachine.add('SEARCHBOX',
SearchBox(myCom),
transitions={
'timeout': 'DISENGAGE',
'aborted': 'DISENGAGE',
'foundBox': 'CENTERBOX'
})
smach.StateMachine.add('CENTERBOX',
CenterBox(myCom),
transitions={
'centered': 'CENTERBOX2',
'centering': 'CENTERBOX',
'lost': 'SEARCHBOX',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('CENTERBOX2',
CenterBox2(myCom),
transitions={
'centered': 'DROP',
'centering': 'CENTERBOX2',
'lost': 'DROP',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('DROP',
Drop(myCom),
transitions={
'completed': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
introServer = smach_ros.IntrospectionServer('mission_server', sm,
'/MISSION/DROP')
introServer.start()
try:
sm.execute()
except Exception as e:
rospy.logerr(str(e))
myCom.failTask()
finally:
rospy.signal_shutdown("bins task ended")
def main():
rospy.init_node('lane')
myCom = Comms(False)
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add('DISENGAGE',
Disengage(myCom),
transitions={'started':'SEARCH',
'killed':'killed'})
smach.StateMachine.add('SEARCH',
Search(myCom),
transitions={'foundLanes':'STABLIZE',
'timeout':'DISENGAGE',
'aborted':'DISENGAGE'})
smach.StateMachine.add('STABLIZE',
Stablize(myCom),
transitions={'stablized':'ALIGN',
'stablizing':'STABLIZE',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('ALIGN',
Align(myCom),
transitions={'aligned':'CENTER',
'aligning':'ALIGN',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('CENTER',
Center(myCom),
transitions={'centered':'FORWARD',
'centering':'CENTER',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('FORWARD',
Forward(myCom),
transitions={'completed':'DISENGAGE',
'aborted':'DISENGAGE'})
introServer = smach_ros.IntrospectionServer('mission_server',
sm,
'/MISSION/LANE')
introServer.start()
try:
sm.execute()
except Exception as e:
rospy.logerr(str(e))
myCom.failTask()
finally:
rospy.signal_shutdown("bins task ended")
def setUpThreads(self):
#Set up threads. Initialise objects and move them to threads
self.controlThread = QThread()
self.commsThread = QThread()
self.controlThreadObject = Control()
self.commsThreadObject = Comms()
self.controlThreadObject.moveToThread(self.controlThread)
self.commsThreadObject.moveToThread(self.commsThread)
self.controlThread.start()
self.commsThread.start()
return
def __init__(self, namespace, node_name, launch_params):
self.node_name = node_name
self.params = launch_params
self.comms = Comms(namespace)
self.comms._connect_to_master_socket()
self.message_bus = self.comms.get_message_bus()
def rmtsh(self, tmp_sock, slist, server_sock):
prompt = "rmtsh (EXIT to quit) "
cwd = "" # used to keep track of current working dir
# attempt to get the pwd/cwd so we can use in in our commands
Comms.sendMsg(tmp_sock, "EXEC pwd")
Comms.sendMsg(tmp_sock, "GETBUFFER")
while (1):
displayPrompt = False
ready_to_read, ready_to_write, in_error = select.select(
slist, [], [], 0)
for sock in ready_to_read:
displayPrompt = False
if (sock == sys.stdin): # server sending message
msg = sys.stdin.readline()
msg = msg.lstrip(
'\r\n '
) # clean up line removing any starting spaces and CRLF
if 'EXIT' in msg: # did we enter EXIT?
return
else: #must have entered some other command
msg = msg.rstrip('\r\n')
if len(msg
) > 0: # is this a blank line? just a return?
if (cwd): # do we have a stored cwd?
msg = "cd " + cwd + " ; " + msg # if so, change command to prepend a "cd <cwd> ; "
Comms.sendMsg(
tmp_sock, "EXEC " + msg + " ; pwd"
) # append a "; pwd" to the command so we can find out the ending working directory
Comms.sendMsg(tmp_sock, "GETBUFFER")
else:
displayPrompt = True
elif (sock != server_sock) and (sock != sys.stdin):
msg = Comms.readMsg(sock, 4096)
msg = msg.rstrip('\r\n')
indx = msg.rfind('\n') # what is the ending line break?
if indx == -1:
indx = 0
cwd = msg[indx:].lstrip('\r\n').rstrip('\r\n')
msg = msg[:indx]
sys.stdout.write("\r\n")
sys.stdout.write(msg)
sys.stdout.write("\r\n")
displayPrompt = True
else:
displayPrompt = False
if (displayPrompt):
sys.stdout.write(prompt + cwd + "> ")
sys.stdout.flush()
def main():
rospy.init_node("PipeMaster")
#rospy.loginfo("Init")
pipe = Comms()
sm = smach.StateMachine(outcomes=['completed', 'failed'])
Inserver = smach_ros.IntrospectionServer("/pole", sm, "/pole")
Inserver.start()
with sm:
smach.StateMachine.add("DISENGAGE",
Disengage(pipe),
transitions={
'init': 'SEARCH',
'aborted': 'failed',
'test': 'TEST'
})
smach.StateMachine.add("SEARCH",
Search(pipe),
transitions={
'lost': 'DISENGAGE',
'detected': 'POSITION',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add("POSITION",
Position(pipe),
transitions={
'locked': 'MOVING',
'check': 'POSITION',
'find': 'SEARCH',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add("MOVING",
Moving(pipe),
transitions={
'success': 'STRIKE',
'return': 'POSITION',
'confirm': 'MOVING',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add("STRIKE",
Strike(pipe),
transitions={
'completed': 'DISENGAGE',
'back': 'STRIKE',
'aborted': 'DISENGAGE',
'circle': 'CIRCLE'
})
smach.StateMachine.add("TEST",
Test(pipe),
transitions={'done': 'TEST'})
smach.StateMachine.add("CIRCLE",
Circle(pipe),
transitions={'completed': 'DISENGAGE'})
outcomes = sm.execute()
rospy.spin()
def main():
rospy.init_node("bins_vision")
cv2.namedWindow("output")
img = cv2.imread("bins/res/std_bin.png")
from comms import Comms
visionFilter = BinsVision(comms=Comms())
_, outImg = visionFilter.gotFrame(img)
if outImg is not None: cv2.imshow("output", outImg)
cv2.waitKey()
def main():
rospy.init_node("lane_marker_vision")
cv2.namedWindow("test")
from comms import Comms
inImg = cv2.imread("lane_marker/test1.jpg")
detector = LaneMarkerVision(Comms())
_, outImg = detector.gotFrame(inImg)
if outImg is not None: cv2.imshow("test", outImg)
cv2.waitKey()
def main():
cv2.namedWindow("Peg Test")
inImg = cv2.imread("pegs/pegs3.png")
from comms import Comms
detector = PegsVision(comms=Comms())
outImg = detector.gotFrame(inImg)
if outImg is not None: cv2.imshow("Peg Test", outImg)
cv2.waitKey()
def main():
rospy.init_node('drop')
myCom = Comms(taskMode='drop')
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add('DISENGAGE',
Disengage(myCom),
transitions={'started':'SEARCHBOX',
'killed':'killed'})
smach.StateMachine.add('SEARCHBOX',
SearchBox(myCom),
transitions={'timeout': 'DISENGAGE',
'aborted': 'DISENGAGE',
'foundBox': 'CENTERBOX'})
smach.StateMachine.add('CENTERBOX',
CenterBox(myCom),
transitions={'centered': 'CENTERBOX2',
'centering': 'CENTERBOX',
'lost': 'SEARCHBOX',
'aborted': 'DISENGAGE'})
smach.StateMachine.add('CENTERBOX2',
CenterBox2(myCom),
transitions={'centered': 'DROP',
'centering': 'CENTERBOX2',
'lost': 'DROP',
'aborted': 'DISENGAGE'})
smach.StateMachine.add('DROP',
Drop(myCom),
transitions={'completed': 'DISENGAGE',
'aborted': 'DISENGAGE'})
introServer = smach_ros.IntrospectionServer('mission_server',
sm,
'/MISSION/DROP')
introServer.start()
try:
sm.execute()
except Exception as e:
rospy.logerr(str(e))
myCom.failTask()
finally:
rospy.signal_shutdown("bins task ended")
def main():
cv2.namedWindow("RGB")
inImg = cv2.imread("rgb_buoy/rgb1.png")
from comms import Comms
detector = RgbBuoyVision(comms=Comms())
outImg = detector.gotFrame(inImg)
if outImg is not None: cv2.imshow("RGB", outImg)
cv2.waitKey()
def main():
import rospy
rospy.init_node("pickup_vision")
cv2.namedWindow("output")
image = cv2.imread("green_cheese.png")
from comms import Comms
visionFilter = PickupVision(Comms())
_, outImg = visionFilter.gotFrame(image)
cv2.imshow("output", outImg)
cv2.waitKey()
def main():
cv2.namedWindow("Torpedo")
inImg = cv2.imread("torpedo/torpedo1.png")
from comms import Comms
detector = TorpedoVision(comms=Comms())
outImg = detector.gotFrame(inImg)
if outImg is not None: cv2.imshow("Torpedo", outImg)
cv2.waitKey()
def __init__(self):
self.name = "camera"
# Keep track of all targets seen to ensure only one fwdkin is pushed downstream for each
self.target_log = set()
# Targets defined here for simulation only
self.cTurret = CameraTurret([
Target(np.array([0, 100, 100]).reshape(3, 1), "t0"),
Target(np.array([0, 100, 150]).reshape(3, 1), "t1"),
Target(np.array([0, 150, 150]).reshape(3, 1), "t2")
])
self.Comms = Comms()
self.Comms.add_subscriber_port('127.0.0.1', '3000', 'cState')
self.Comms.add_publisher_port('127.0.0.1', '3004', 'targetPos')
# twenty frames per second
self.frame_delay = .05
def forwardTraffic(self, srcip, srcport, dstip, dstport, msg):
for uid in self.neighbors:
if (self.neighbors[uid].location == "Mesh"):
if (self.neighbors[uid].ip
== dstip) and (self.neighbors[uid].port == dstport):
# connect and send
remote_sock = Comms.create_direct_socket(
self.neighbors[uid].ip, self.neighbors[uid].port)
if (remote_sock):
Comms.sendMsg(
remote_sock, srcip + ":" + str(srcport) + ":" +
dstip + ":" + str(dstport) + ":" + msg)
msg = srcip + ":" + str(
srcport) + ":" + dstip + ":" + str(
dstport) + ":" + msg
remote_sock.close()
else:
print("FAILED TO SEND")
return
for uid in self.neighbors:
if (self.neighbors[uid].location == "Mesh"):
if not ((self.neighbors[uid].ip == srcip) and
(self.neighbors[uid].port == srcport)):
# connect and send
remote_sock = Comms.create_direct_socket(
self.neighbors[uid].ip, self.neighbors[uid].port)
if (remote_sock):
Comms.sendMsg(
remote_sock, srcip + ":" + str(srcport) + ":" +
dstip + ":" + str(dstport) + ":" + msg)
remote_sock.close()
else:
print("FAILED TO SEND")
return
class BaseNode(object):
def __init__(self, namespace, node_name, launch_params):
self.node_name = node_name
self.params = launch_params
self.comms = Comms(namespace)
self.comms._connect_to_master_socket()
self.message_bus = self.comms.get_message_bus()
def get_param(self, param_name, default=None):
if param_name in self.params:
return self.params[param_name]
else:
return default
def setup(self):
pass
def run(self):
self.comms.process_messages()
def __init__(self):
self.name = "hardware"
self.gun_config = np.array([[0, 0]]).T
self.camera_config = np.array([[0, 0]]).T
self.gun_ready = 0
self.gunReady = True
self.gunTurretReady = False
# When a target has been detected and
self.cameraPath = None
self.gunPath = None
# Simulation graphics engine
self.sim_out = Engine(
np.array([150, 150, 150]).reshape(3, 1),
np.array([np.pi / 2 - .4, -1, .3]).reshape(3, 1))
self.Comms = Comms()
self.Comms.add_publisher_port('127.0.0.1', '3000', 'cState')
self.Comms.add_publisher_port('127.0.0.1', '3001', 'gState')
self.Comms.add_subscriber_port('127.0.0.1', '3002', 'cameraPath')
self.Comms.add_subscriber_port('127.0.0.1', '3003', 'gunPath')
def main():
rospy.init_node('rgb_buoy_node', anonymous=False)
rosRate = rospy.Rate(20)
myCom = Comms()
rospy.loginfo("RGB Loaded")
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'failed'])
with sm:
smach.StateMachine.add("DISENGAGE",
Disengage(myCom),
transitions={
'start_complete': "SEARCH",
'start': "DISENGAGE",
'killed': 'failed'
})
smach.StateMachine.add("SEARCH",
Search(myCom),
transitions={
'search_complete': "BANGBUOY",
'aborted': 'aborted',
'killed': 'failed'
})
smach.StateMachine.add("CENTERING",
Centering(myCom),
transitions={
'centering': "CENTERING",
'centering_complete': 'succeeded',
'aborted': 'aborted',
'killed': 'failed'
})
smach.StateMachine.add("BANGBUOY",
bangBuoy(myCom),
transitions={
'banging': "BANGBUOY",
'bang_to_center': "CENTERING",
'aborted': 'aborted',
'killed': 'failed'
})
#set up introspection Server
introServer = smach_ros.IntrospectionServer('mission_server', sm,
'/MISSION/RGB_BUOY')
introServer.start()
sm.execute()
def _start_print(self, file_path=None, directory=None):
# start comms thread to stream the file
# set comms.ping_pong to False for fast stream mode
if file_path is None:
file_path = self.app.gcode_file
if directory is None:
directory = self.last_path
Logger.info('MainWindow: printing file: {}'.format(file_path))
try:
self.nlines = Comms.file_len(
file_path) # get number of lines so we can do progress and ETA
Logger.debug('MainWindow: number of lines: {}'.format(self.nlines))
except Exception:
Logger.warning('MainWindow: exception in file_len: {}'.format(
traceback.format_exc()))
self.nlines = None
self.start_print_time = datetime.datetime.now()
self.display('>>> Running file: {}, {} lines'.format(
file_path, self.nlines))
if self.app.comms.stream_gcode(
file_path, progress=lambda x: self.display_progress(x)):
self.display('>>> Run started at: {}'.format(
self.start_print_time.strftime('%x %X')))
else:
self.display('WARNING Unable to start print')
return
self.set_last_file(directory, file_path)
self.ids.print_but.text = 'Pause'
self.is_printing = True
self.paused = False
def main():
rospy.init_node('pickup')
myCom = Comms()
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add('DISENGAGE',
Disengage(myCom),
transitions={'started':'SEARCHSITE',
'killed':'killed'})
smach.StateMachine.add('SEARCHSITE',
SearchSite(myCom),
transitions={'foundSite':'CENTERSITE',
'timeout':'DISENGAGE',
'aborted':'DISENGAGE'})
smach.StateMachine.add('CENTERSITE',
CenterSite(myCom),
transitions={'centered':'SEARCH',
'centering':'CENTERSITE',
'lost':'SEARCHSITE',
'aborted':'DISENGAGE'})
smach.StateMachine.add('SEARCH',
Search(myCom),
transitions={'foundSamples':'CENTER',
'searching':'SEARCH',
'timeout':'DISENGAGE',
'aborted':'DISENGAGE'})
smach.StateMachine.add('CENTER',
Center(myCom),
transitions={'centered':'ALIGN',
'centering':'CENTER',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('ALIGN',
Align(myCom),
transitions={'aligned': 'CENTER2',
'aligning': 'ALIGN',
'lost': 'SEARCH',
'aborted': 'DISENGAGE'})
smach.StateMachine.add('CENTER2',
Center2(myCom),
transitions={'centered':'APPROACH',
'centering':'CENTER2',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('APPROACH',
Approach(myCom),
transitions={'completed':'CENTER3',
'approaching':'APPROACH',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('CENTER3',
Center3(myCom),
transitions={'centered':'GRAB',
'centering':'CENTER3',
'lost':'GRAB',
'aborted':'DISENGAGE'})
smach.StateMachine.add('GRAB',
Grab(myCom),
transitions={'grabbed':'SURFACE',
'aborted':'DISENGAGE'})
smach.StateMachine.add('SURFACE',
Surface(myCom),
transitions={'completed':'DISENGAGE',
'aborted':'DISENGAGE'})
introServer = smach_ros.IntrospectionServer('mission_server',
sm,
'/MISSION/PICKUP')
introServer.start()
try:
sm.execute()
except Exception as e:
rospy.logerr(str(e))
myCom.failTask()
finally:
rospy.signal_shutdown("bins task ended")
print('Open cutelog separately to see logging!')
# Initialize providers and pass to coordinator
providers = []
if IS_SIMULATION:
NO_RADIO = True
providers += [Simulator(SIMULATOR_SETUP)]
else:
providers += [SSLVisionDataProvider()]
if not NO_REFBOX:
providers += [RefboxDataProvider()]
if not NO_RADIO:
providers += [Comms(HOME_TEAM)]
if CONTROL_BOTH_TEAMS:
providers += [Comms(AWAY_TEAM, True)]
providers += [Strategy(HOME_TEAM, HOME_STRATEGY)]
if CONTROL_BOTH_TEAMS:
providers += [Strategy(AWAY_TEAM, AWAY_STRATEGY)]
providers += [Visualizer()]
# Pass the providers to the coordinator
c = Coordinator(providers)
# Setup the exit handler
def stop_it(signum, frame):
def main():
rospy.init_node('bins')
myCom = Comms()
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add('DISENGAGE',
Disengage(myCom),
transitions={'started':'SEARCH',
'killed':'killed'})
smach.StateMachine.add('SEARCH',
Search(myCom),
transitions={'foundBins':'CENTER',
'timeout':'DISENGAGE',
'aborted':'DISENGAGE'})
smach.StateMachine.add('CENTER',
Center(myCom),
transitions={'centered':'ALIGN',
'centering':'CENTER',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('ALIGN',
Align(myCom),
transitions={'aligned':'CENTERAGAIN',
'aligning':'ALIGN',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('CENTERAGAIN',
CenterAgain(myCom),
transitions={'centered':'FIRE',
'centering':'CENTERAGAIN',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('FIRE',
Fire(myCom),
transitions={'completed':'succeeded',
'next':'SEARCH2',
'aborted':'DISENGAGE'})
smach.StateMachine.add('SEARCH2',
Search2(myCom),
transitions={'foundBins':'CENTER2',
'lost':'DISENGAGE',
'aborted':'DISENGAGE'})
smach.StateMachine.add('CENTER2',
Center2(myCom),
transitions={'centered':'ALIGN',
'centering':'CENTER2',
'lost':'DISENGAGE',
'aborted':'DISENGAGE'})
introServer = smach_ros.IntrospectionServer('mission_server',
sm,
'/MISSION/BINS')
introServer.start()
try:
sm.execute()
except Exception as e:
rospy.logerr(str(e))
myCom.failTask()
finally:
rospy.signal_shutdown("bins task ended")
class GCS(msppg.Parser):
def __init__(self):
msppg.Parser.__init__(self)
# No communications or arming yet
self.comms = None
self.armed = False
self.gotimu = False
# Do basic Tk initialization
self.root = tk.Tk()
self.root.configure(bg=BACKGROUND_COLOR)
self.root.resizable(False, False)
self.root.title('Hackflight Ground Control Station')
left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
self.root.geometry('%dx%d+%d+%d' %
(DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
self.frame = tk.Frame(self.root)
# Too much hassle on Windows
if 'nt' != os.name:
self.root.tk.call('wm', 'iconphoto', self.root._w,
tk.PhotoImage('icon.xbm'))
self.root.protocol('WM_DELETE_WINDOW', self.quit)
# Create panes for two rows of widgets
self.pane1 = self._add_pane()
self.pane2 = self._add_pane()
# Add a buttons
self.button_connect = self._add_button('Connect', self.pane1,
self._connect_callback)
self.button_imu = self._add_button('IMU', self.pane2,
self._imu_callback)
self.button_motors = self._add_button('Motors', self.pane2,
self._motors_button_callback)
self.button_receiver = self._add_button('Receiver', self.pane2,
self._receiver_button_callback)
#self.button_messages = self._add_button('Messages', self.pane2, self._messages_button_callback)
#self.button_maps = self._add_button('Maps', self.pane2, self._maps_button_callback, disabled=False)
# Prepare for adding ports as they are detected by our timer task
self.portsvar = tk.StringVar(self.root)
self.portsmenu = None
self.connected = False
self.ports = []
# Finalize Tk stuff
self.frame.pack()
self.canvas = tk.Canvas(self.root,
width=DISPLAY_WIDTH,
height=DISPLAY_HEIGHT,
background='black')
self.canvas.pack()
# Set up a text label for reporting errors
errmsg = 'No response from board. Possible reasons:\n\n' + \
' * You connected to the wrong port.\n\n' + \
' * Firmware uses serial receiver\n' + \
' (DSMX, SBUS), but receiver is\n' + \
' not connected.'
self.error_label = tk.Label(self.canvas,
text=errmsg,
bg='black',
fg='red',
font=(None, 24),
justify=tk.LEFT)
self.hide(self.error_label)
# Add widgets for motor-testing dialog; hide them immediately
self.motors = Motors(self)
self.motors.stop()
# Create receiver dialog
self.receiver = Receiver(self)
# Create messages dialog
#self.messages = Messages(self)
# Create IMU dialog
self.imu = IMU(self)
self._schedule_connection_task()
# Create a maps dialog
#self.maps = Maps(self, yoffset=-30)
# Create a splash image
self.splashimage = tk.PhotoImage(file=resource_path('splash.gif'))
self._show_splash()
# Create a message parser
#self.parser = msppg.Parser()
# Set up parser's request strings
self.attitude_request = msppg.serialize_ATTITUDE_RADIANS_Request()
self.rc_request = msppg.serialize_RC_NORMAL_Request()
# No messages yet
self.roll_pitch_yaw = [0] * 3
self.rxchannels = [0] * 6
# A hack to support display in IMU dialog
self.active_axis = 0
def quit(self):
self.motors.stop()
self.root.destroy()
def hide(self, widget):
widget.place(x=-9999)
def getChannels(self):
return self.rxchannels
def getRollPitchYaw(self):
# configure button to show connected
self._enable_buttons()
self.button_connect['text'] = 'Disconnect'
self._enable_button(self.button_connect)
return self.roll_pitch_yaw
def checkArmed(self):
if self.armed:
self._show_armed(self.root)
self._show_armed(self.pane1)
self._show_armed(self.pane2)
self._disable_button(self.button_motors)
else:
self._show_disarmed(self.root)
self._show_disarmed(self.pane1)
self._show_disarmed(self.pane2)
def scheduleTask(self, delay_msec, task):
self.root.after(delay_msec, task)
def handle_RC_NORMAL(self, c1, c2, c3, c4, c5, c6):
# Display throttle as [0,1], other channels as [-1,+1]
self.rxchannels = c1 / 2. + .5, c2, c3, c4, c5, c6
# As soon as we handle the callback from one request, send another request, if receiver dialog is running
if self.receiver.running:
self._send_rc_request()
#self.messages.setCurrentMessage('Receiver: %04d %04d %04d %04d %04d' % (c1, c2, c3, c4, c5))
def handle_ATTITUDE_RADIANS(self, x, y, z):
self.roll_pitch_yaw = x, -y, z
self.gotimu = True
#self.messages.setCurrentMessage('Roll/Pitch/Yaw: %+3.3f %+3.3f %+3.3f' % self.roll_pitch_yaw)
# As soon as we handle the callback from one request, send another request, if IMU dialog is running
if self.imu.running:
self._send_attitude_request()
def _add_pane(self):
pane = tk.PanedWindow(self.frame, bg=BACKGROUND_COLOR)
pane.pack(fill=tk.BOTH, expand=1)
return pane
def _add_button(self, label, parent, callback, disabled=True):
button = tk.Button(parent, text=label, command=callback)
button.pack(side=tk.LEFT)
button.config(state='disabled' if disabled else 'normal')
return button
# Callback for IMU button
def _imu_callback(self):
self._clear()
self.motors.stop()
self.receiver.stop()
#self.messages.stop()
#self.maps.stop()
self._send_attitude_request()
self.imu.start()
def _start(self):
self._send_attitude_request()
self.imu.start()
self.gotimu = False
self.hide(self.error_label)
self.scheduleTask(CONNECTION_DELAY_MSEC, self._checkimu)
def _checkimu(self):
if not self.gotimu:
self.imu.stop()
self.error_label.place(x=50, y=50)
self._disable_button(self.button_imu)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
# Sends Attitude request to FC
def _send_attitude_request(self):
self.comms.send_request(self.attitude_request)
# Sends RC request to FC
def _send_rc_request(self):
self.comms.send_request(self.rc_request)
# Callback for Motors button
def _motors_button_callback(self):
self._clear()
self.imu.stop()
self.receiver.stop()
#self.messages.stop()
#self.maps.stop()
self.motors.start()
def _clear(self):
self.canvas.delete(tk.ALL)
# Callback for Receiver button
def _receiver_button_callback(self):
self._clear()
self.imu.stop()
self.motors.stop()
#self.messages.stop()
#self.maps.stop()
self._send_rc_request()
self.receiver.start()
# Callback for Messages button
def _messages_button_callback(self):
self._clear()
self.imu.stop()
self.motors.stop()
#self.maps.stop()
self.receiver.stop()
self.messages.start()
def _getting_messages(self):
return self.button_connect['text'] == 'Disconnect'
# Callback for Maps button
def _maps_button_callback(self):
self._clear()
if self._getting_messages():
self.receiver.stop()
self.messages.stop()
self.imu.stop()
self.motors.stop()
#self.maps.start()
# Callback for Connect / Disconnect button
def _connect_callback(self):
if self.connected:
self.imu.stop()
#self.maps.stop()
self.motors.stop()
#self.messages.stop()
self.receiver.stop()
if not self.comms is None:
self.comms.stop()
self._clear()
self._disable_buttons()
self.button_connect['text'] = 'Connect'
self.hide(self.error_label)
self._show_splash()
else:
#self.maps.stop()
self.comms = Comms(self)
self.comms.start()
self.button_connect['text'] = 'Connecting ...'
self._disable_button(self.button_connect)
self._hide_splash()
self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)
self.connected = not self.connected
# Gets available ports
def _getports(self):
allports = comports()
ports = []
for port in allports:
portname = port[0]
if 'usbmodem' in portname or 'ttyACM' in portname or 'ttyUSB' in portname or 'COM' in portname:
if not portname in ['COM1', 'COM2']:
ports.append(portname)
return ports
# Checks for changes in port status (hot-plugging USB cables)
def _connection_task(self):
ports = self._getports()
if ports != self.ports:
if self.portsmenu is None:
self.portsmenu = tk.OptionMenu(self.pane1, self.portsvar,
*ports)
else:
for port in ports:
self.portsmenu['menu'].add_command(label=port)
self.portsmenu.pack(side=tk.LEFT)
# Disconnected
if ports == []:
self.portsmenu['menu'].delete(0, 'end')
self.portsvar.set('')
self._disable_button(self.button_connect)
self._disable_buttons()
# Connected
else:
self.portsvar.set(ports[0]) # default value
self._enable_button(self.button_connect)
self.ports = ports
self._schedule_connection_task()
# Mutually recursive with above
def _schedule_connection_task(self):
self.root.after(USB_UPDATE_MSEC, self._connection_task)
def _disable_buttons(self):
self._disable_button(self.button_imu)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
#self._disable_button(self.button_messages)
def _enable_buttons(self):
self._enable_button(self.button_imu)
self._enable_button(self.button_motors)
self._enable_button(self.button_receiver)
#self._enable_button(self.button_messages)
def _enable_button(self, button):
button['state'] = 'normal'
def _disable_button(self, button):
button['state'] = 'disabled'
def sendMotorMessage(self, index, percent):
values = [0] * 4
values[index - 1] = percent / 100.
self.comms.send_message(msppg.serialize_SET_MOTOR_NORMAL, values)
def _show_splash(self):
self.splash = self.canvas.create_image(SPLASH_LOCATION,
image=self.splashimage)
def _hide_splash(self):
self.canvas.delete(self.splash)
def _show_armed(self, widget):
widget.configure(bg='red')
def _show_disarmed(self, widget):
widget.configure(bg=BACKGROUND_COLOR)
if self._getting_messages():
self._enable_button(self.button_motors)
def _handle_calibrate_response(self):
self.imu.showCalibrated()
def _handle_params_response(self, pitchroll_kp_percent, yaw_kp_percent):
# Only handle parms from firmware on a fresh connection
if self.newconnect:
self.imu.setParams(pitchroll_kp_percent, yaw_kp_percent)
self.newconnect = False
def _handle_arm_status(self, armed):
self.armed = armed
#self.messages.setCurrentMessage('ArmStatus: %s' % ('ARMED' if armed else 'Disarmed'))
def _handle_battery_status(self, volts, amps):
return
elif msg["type"] == NAVI_END:
comms.NaviEnd()
elif msg["type"] == OBSTACLE_DETECTED:
comms.ObstacleDetected(msg.val.dir, msg.val.str)
def receive(q_xbee, comms):
while True:
msg = comms.Receive(q_xbee)
# if msg != None:
# q_xbee.put(msg.status)
if __name__ == "__main__":
# # Always instatiate a Comms object
comms = Comms();
# # Shows the all the different information that Pi
# # sends to Arduino. This methods runs until an
# # it receives an acknowledgement from Arduino
# # comms.DeviceReady()
# # comms.NaviReady()
# # comms.NaviEnd()
# # comms.ObstacleDetected("L",3)
# # The following code represents
# # the process that always run
def __init__(self):
Comms.__init__(self, "/dev/cu.usbmodem14171", baudrate=9600, timeout=0.25)
VISION_LOOP_SLEEP = .02
COMMS_SEND_LOOP_SLEEP = .1
COMMS_RECEIVE_LOOP_SLEEP = .1
CONTROL_LOOP_SLEEP = .1
SIMULATION_LOOP_SLEEP = .05
VISUALIZATION_LOOP_SLEEP = .05
GAME_LOOP_SLEEP = .1
if __name__ == '__main__':
VERBOSE = False
# initialize gamestate + all other modules
gamestate = GameState()
vision = SSLVisionDataProvider(gamestate)
refbox = RefboxDataProvider(gamestate)
home_comms = Comms(gamestate, HOME_TEAM)
away_comms = Comms(gamestate, AWAY_TEAM, True)
simulator = Simulator(gamestate)
home_strategy = Strategy(gamestate, HOME_TEAM)
away_strategy = Strategy(gamestate, AWAY_TEAM)
# choose which modules to run based on run conditions
print('Spinning up Threads...')
if IS_SIMULATION:
# spin up simulator to replace actual vision data + comms
simulator.start_simulating(SIMULATION_SETUP, SIMULATION_LOOP_SLEEP)
else:
# spin up ssl-vision data polling to update gamestate
vision.start_updating(VISION_LOOP_SLEEP)
if not VISION_ONLY:
# spin up comms to send commands to robots
################################ MAIN LOOP ##########################################
printMenu()
# Make an object to represent the Stellaris and reset device.
ESTR = LoadTest()
ESTR.resetESTR()
# Create two log files. One for raw COM port data, the other
# for interpreted test results.
dataLog = FileManager("logs/datalog{}.txt")
resultLog = FileManager("logs/resultlog{}.txt")
#try to open the COM port to connect to ESTR
try:
COM = Comms("COM39", 115200*2) #57600 #203400
except:
quit_event.set()
print("COM port could not be opened! Exiting...")
# Create a queue to communicate between COM producer thread
# and COM worker thread.
COMQueue = Queue.Queue()
# Create three daemom threads, one to read the COM port, one to
# work on the COM data and the last to read the command line interface
readCOMThread = threading.Thread(target=readCOM, name="readCOM")
readCOMThread.daemon = True
readCOMThread.start()
COMWorkerThread = threading.Thread(target=COMWorker, name="COMWorker")
COMWorkerThread.daemon = True