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')
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, **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 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("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("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("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("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"
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('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 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):
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 __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')
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):
cv2.circle(mask, (x, y), 5, 0, -1)
p = cv2.goodFeaturesToTrack(frame_gray,
mask=mask,
**self.feature_params)
if p is not None:
for x, y in np.float32(p).reshape(-1, 2):
self.tracks.append([(x, y)])
self.frame_count += 1
self.prev_gray = frame_gray
k = cv2.waitKey(1) & 0xFF
if k == 27:
break
if __name__ == "__main__":
try:
video = sys.argv[1]
port = sys.argv[2]
except:
video = "cropped.mp4"
port = "/dev/ttyUSB0"
drawingDemon = threading.Thread(target=plot, daemon=True)
drawingDemon.start()
serialcomms = Comms(port, 9600)
BirdTracker(video, rawPaths, True).track()
cv2.destroyAllWindows()
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")
async def step_3(robot1, robot2, destPos):
res = False
# Class instances of 'communication' needed for each robot
comms1 = Comms(robot1)
comms2 = Comms(robot2)
await comms1.load()
await comms2.load()
# Converts the actual coordinate to the limited range of pictures
# that can be displayed on the Cozmos
y = destPos[0]
x = destPos[1]
if y == 0:
y = 0
elif y == 2:
y = 1
elif y == 4:
y = 2
elif y == 6:
y = 3
elif y == 8:
y = 4
if x == 0:
x = 0
elif x == 2:
x = 1
elif x == 4:
x = 2
elif x == 6:
x = 3
elif x == 8:
x = 4
destPos = [0, 0]
print('y', y, 'x', x)
# Need to break out of the centre-grid positioning, because they must be closer
# together when exchanging the coordinates
# await robot1.drive_straight(distance_mm(125), speed_mmps(40)).wait_for_completed()
# Robot 1 communicates, for the benefit of humans listening, that it will now
# begin exchanging coordinates
await robot1.say_text(
"Hey, I found the cube, but I need your help to lift it, I'm too tired."
).wait_for_completed()
say = "Here is where I saw it. X equals:" + str(4)
await robot1.say_text(say).wait_for_completed()
#robot 2 communitcation
await robot2.say_text("Ok, let me grab a pen...").wait_for_completed()
# Since there is a chance of bad alignment of the robots, a few attempts are
# made at slightly different angles to increase chances of success.
#
# The 'display' and 'read' functions are used to show a marker corresponding to
# a coordinate and read those markers, respectively.
for i in range(0, 3):
comms1.display(y)
res = await comms2.read()
# await asyncio.sleep(2)
# res = 6
destPos[0] = res
comms1.clear()
if res != -1:
break
else:
await robot1.turn_in_place(degrees(5)).wait_for_completed()
if res == -1:
print(
'[ERROR][CONTROL] Robot 2 was unable to detect Robot 1 coordinate Y. '
)
await asyncio.sleep(2)
# As above, but now for the y-axis coordinate.
say = "and Y equals:" + str(6)
await robot1.say_text(say).wait_for_completed()
comms1.display(x)
res = await comms2.read()
# res = 4
await asyncio.sleep(2)
destPos[1] = res
comms1.clear()
if res == -1:
print(
'[ERROR][CONTROL] Robot 2 was unable to detect Robot 1 coordinate X. '
)
await asyncio.sleep(2)
await robot2.say_text("or... not").wait_for_completed()
await robot1.drive_straight(distance_mm(-125),
speed_mmps(40)).wait_for_completed()
return True, destPos
def main():
rospy.init_node('pegs_node', anonymous=False)
rosRate = rospy.Rate(20)
myCom = Comms()
rospy.loginfo("Pegs Loaded")
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add("DISENGAGE",
Disengage(myCom),
transitions={
'start_complete': "SEARCHPEGS",
'killed': 'killed'
})
smach.StateMachine.add("FOLLOWSONAR",
FollowSonar(myCom),
transitions={
'following_sonar': "FOLLOWSONAR",
'sonar_complete': "SEARCHPEGS",
'aborted': 'aborted',
'killed': 'killed'
})
smach.StateMachine.add("SEARCHPEGS",
SearchPegs(myCom),
transitions={
'searchPeg_complete': "MOVEFORWARD",
'aborted': 'aborted',
'killed': 'killed'
})
smach.StateMachine.add("MOVEFORWARD",
MoveForward(myCom),
transitions={
'forwarding': "MOVEFORWARD",
'forward_complete': "CENTERING",
'lost': "SEARCHPEGS",
'aborted': 'aborted',
'killed': 'killed'
})
smach.StateMachine.add("CENTERING",
Centering(myCom),
transitions={
'centering': "CENTERING",
'centering_complete': "OFFSET",
'lost': "SEARCHPEGS",
'aborted': 'aborted',
'killed': 'killed'
})
smach.StateMachine.add("OFFSET",
Offset(myCom),
transitions={
'offset_complete': "MOVEPEG",
'aborted': 'aborted',
'killed': 'killed'
})
smach.StateMachine.add("MOVEPEG",
MovePeg(myCom),
transitions={
'move_complete': "SEARCHPEGS",
'task_complete': 'succeeded',
'aborted': 'aborted',
'killed': 'killed'
})
#set up introspection Server
introServer = smach_ros.IntrospectionServer('mission_server', sm,
'/MISSION/PEGS')
introServer.start()
sm.execute()
################################ 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
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")
def build(self):
lt = self.config.get('UI', 'display_type')
dtlut = {
"RPI Touch": 0,
"Small Desktop": 1,
"Large Desktop": 2,
"Wide Desktop": 3,
"RPI Full Screen": 4
}
self.is_desktop = dtlut.get(lt, 0)
# load the layouts for the desktop screen
if self.is_desktop == 1:
Builder.load_file('desktop.kv')
Window.size = (1024, 768)
elif self.is_desktop == 2 or self.is_desktop == 3 or self.is_desktop == 4:
Builder.load_file('desktop_large.kv' if self.is_desktop ==
2 else 'desktop_wide.kv')
if self.is_desktop != 4:
# because rpi_egl does not like to be told the size
s = self.config.get('UI', 'screen_size')
if s == 'auto':
Window.size = (1280,
1024) if self.is_desktop == 2 else (1280,
800)
elif 'x' in s:
(w, h) = s.split('x')
Window.size = (int(w), int(h))
p = self.config.get('UI', 'screen_pos')
if p != 'auto' and ',' in p:
(t, l) = p.split(',')
Window.top = int(t)
Window.left = int(l)
Window.bind(on_request_close=self.window_request_close)
else:
self.is_desktop = 0
# load the layouts for rpi 7" touch screen
Builder.load_file('rpi.kv')
self.is_cnc = self.config.getboolean('UI', 'cnc')
self.tab_top = self.config.getboolean('UI', 'tab_top')
self.is_webserver = self.config.getboolean('Web', 'webserver')
self.is_show_camera = self.config.getboolean('Web', 'show_video')
self.is_spindle_camera = self.config.getboolean(
'General', 'is_spindle_camera')
self.manual_tool_change = self.config.getboolean(
'General', 'manual_tool_change')
self.wait_on_m0 = self.config.getboolean('General', 'wait_on_m0')
self.is_v2 = self.config.getboolean('General', 'v2')
self.comms = Comms(App.get_running_app(),
self.config.getfloat('General', 'report_rate'))
self.gcode_file = self.config.get('General', 'last_print_file')
self.sm = ScreenManager()
ms = MainScreen(name='main')
self.main_window = ms.ids.main_window
self.sm.add_widget(ms)
self.sm.add_widget(GcodeViewerScreen(name='viewer', comms=self.comms))
self.config_editor = ConfigEditor(name='config_editor')
self.sm.add_widget(self.config_editor)
self.gcode_help = GcodeHelp(name='gcode_help')
self.sm.add_widget(self.gcode_help)
if self.is_desktop == 0:
self.text_editor = TextEditor(name='text_editor')
self.sm.add_widget(self.text_editor)
self.blank_timeout = self.config.getint('General', 'blank_timeout')
Logger.info("SmoothieHost: screen blank set for {} seconds".format(
self.blank_timeout))
self.sm.bind(on_touch_down=self._on_touch)
Clock.schedule_interval(self._every_second, 1)
# select the file chooser to use
# select which one we want from config
filechooser = self.config.get('UI', 'filechooser')
if self.is_desktop > 0:
if filechooser != 'default':
NativeFileChooser.type_name = filechooser
Factory.register('filechooser', cls=NativeFileChooser)
try:
f = Factory.filechooser()
except Exception:
Logger.error(
"SmoothieHost: can't use selected file chooser: {}".
format(filechooser))
Factory.unregister('filechooser')
Factory.register('filechooser', cls=FileDialog)
else:
# use Kivy filechooser
Factory.register('filechooser', cls=FileDialog)
# we want to capture arrow keys
Window.bind(on_key_down=self._on_keyboard_down)
else:
# use Kivy filechooser
Factory.register('filechooser', cls=FileDialog)
# setup for cnc or 3d printer
if self.is_cnc:
if self.is_desktop < 3:
# remove Extruder panel from tabpanel and tab
self.main_window.ids.tabs.remove_widget(
self.main_window.ids.tabs.extruder_tab)
# if not CNC mode then do not show the ZABC buttons in jogrose
if not self.is_cnc:
self.main_window.ids.tabs.jog_rose.jogrosemain.remove_widget(
self.main_window.ids.tabs.jog_rose.abc_panel)
if self.is_webserver:
self.webserver = ProgressServer()
self.webserver.start(self, 8000)
if self.is_show_camera:
self.camera_url = self.config.get('Web', 'camera_url')
self.sm.add_widget(CameraScreen(name='web cam'))
self.main_window.tools_menu.add_widget(
ActionButton(text='Web Cam',
on_press=lambda x: self._show_web_cam()))
if self.is_spindle_camera:
if self.is_desktop in [0, 4]:
try:
self.sm.add_widget(SpindleCamera(name='spindle camera'))
except Exception as err:
self.main_window.display(
'ERROR: failed to load spindle camera. Check logs')
Logger.error(
'Main: spindle camera exception: {}'.format(err))
self.main_window.tools_menu.add_widget(
ActionButton(text='Spindle Cam',
on_press=lambda x: self._show_spindle_cam()))
# load any modules specified in config
self._load_modules()
if self.blank_timeout > 0:
# unblank if blanked
self.unblank_screen()
return self.sm
def main():
rospy.init_node('torpedo_lynnette_awesomeness', anonymous=False)
rosRate = rospy.Rate(30)
myCom = Comms()
rospy.loginfo("Torpedo Loaded")
sm = smach.StateMachine(outcomes=['succeeded', 'killed', 'failed'])
with sm:
smach.StateMachine.add("DISENGAGE",
Disengage(myCom),
transitions={
'start_complete': "ALIGNBOARD",
'killed': 'failed'
})
smach.StateMachine.add("FOLLOWSONAR",
FollowSonar(myCom),
transitions={
'following_sonar': "FOLLOWSONAR",
'sonar_complete': "ALIGNBOARD",
'aborted': 'killed',
'killed': 'failed'
})
smach.StateMachine.add("ALIGNBOARD",
AlignBoard(myCom),
transitions={
'aligning_board': "ALIGNBOARD",
'alignBoard_complete': "MOVEFORWARD",
'aborted': 'failed',
'killed': 'failed'
})
smach.StateMachine.add("SEARCHCIRCLES",
SearchCircles(myCom),
transitions={
'searchCircles_complete': "ALIGNBOARD",
'lost': 'failed',
'aborted': 'killed',
'killed': 'failed'
})
smach.StateMachine.add("MOVEFORWARD",
MoveForward(myCom),
transitions={
'forwarding': "MOVEFORWARD",
'forward_complete': "CENTERING",
'lost': 'failed',
'aborted': 'killed',
'killed': 'failed'
})
smach.StateMachine.add("CENTERING",
Centering(myCom),
transitions={
'centering': "CENTERING",
'centering_complete': "SHOOTING",
'lost': 'failed',
'aborted': 'killed',
'killed': 'failed'
})
smach.StateMachine.add("SHOOTING",
ShootTorpedo(myCom),
transitions={
'shoot_again': "SEARCHCIRCLES",
'shoot_complete': 'succeeded',
'aborted': 'killed',
'killed': 'failed'
})
#set up introspection Server
introServer = smach_ros.IntrospectionServer('mission_server', sm,
'/MISSION/TORPEDO')
introServer.start()
sm.execute()
def main():
rospy.init_node('lane_acoustic')
myCom = Comms(True)
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add('DISENGAGE',
Disengage(myCom),
transitions={
'started': 'MAINCENTERBOX',
'killed': 'killed'
})
smach.StateMachine.add('MAINCENTERBOX',
MainCenterBox(myCom),
transitions={
'centered': 'ALIGNBOXLANE',
'centering': 'MAINCENTERBOX',
'lost': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('CENTERBOX',
CenterBox(myCom),
transitions={
'centered': 'ALIGNBOXLANE',
'centering': 'CENTERBOX',
'lost': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('ALIGNBOXLANE',
AlignBoxLane(myCom),
transitions={
'aligned': 'SEARCH',
'aligning': 'ALIGNBOXLANE',
'next_corner': 'CENTERBOX',
'lost': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
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")
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
