def __init__(self, main_behavior_class=None, control_reactor=True, startRightNow=True):
super(TwistedMainLoop, self).__init__(main_behavior_class = main_behavior_class)
self._do_cleanup = True
self._control_reactor = control_reactor
from twisted.internet import reactor
orig_sigInt = reactor.sigInt
self._ctrl_c_presses = 0
def my_int_handler(reactor, *args):
self._ctrl_c_presses += 1
info( "TwistedMainLoop: SIGBREAK caught")
if self._ctrl_c_presses == 1:
if self.quitting:
info( "TwistedMainLoop: SIGBREAK: quitting already in progress")
else:
self._startShutdown(naoqi_ok=True, ctrl_c_pressed=True)
else:
info( "Two ctrl-c - shutting down uncleanly")
orig_sigInt(*args)
reactor.sigInt = my_int_handler
import pynaoqi
self.con = pynaoqi.getDefaultConnection()
self.started = False # True when setStartCallback has been called
self.quitting = False
if startRightNow:
self.start()
def main():
from twisted.internet import gtk2reactor
try:
gtk2reactor.install()
except:
pass
from twisted.internet import reactor
con = pynaoqi.getDefaultConnection()
notes = NotesMain(con)
notes._w.show_all()
reactor.run()
def __init__(self):
self._con = con = pynaoqi.getDefaultConnection()
options = pynaoqi.getDefaultOptions()
broker_info = dict(con.getBrokerInfo())
con.getInfo(broker_info["name"])
con.registerBroker()
con.exploreToGetModuleByName("NaoCam")
con.registerToCamera()
self._c = 0
self._image = None
self._initGui()
def __init__(self, main_behavior_class=None, reactor=True, startRightNow=True):
super(TwistedMainLoop, self).__init__(main_behavior_class = main_behavior_class)
self._do_cleanup = True
self._reactor = reactor
if reactor:
self._install_sigint_handler(reactor)
import pynaoqi
self.con = pynaoqi.getDefaultConnection()
self.started = False # True when setStartCallback has been called
self.quitting = False
if startRightNow:
self.start()
def gotoAngle(ind, val):
con = pynaoqi.getDefaultConnection()
#print "joint %s, ind %s, value %s %s" % (
# self.name, ind, s.get_value(), val)
if ind == self._waiting_callbacks:
# gotoAngleWithSpeed
d = con.ALMotion.angleInterpolationWithSpeed(self.name, clip(-pi, pi, val), 0.2) # 50, 1
else:
#print "not moving, %s != %s" % (ind, self._waiting_callbacks)
d = succeed(0)
self._last.append(d)
return d
def main():
using_gtk = installgtkreactor()
# If you are looking for command line parsing, it happens
# in pynaoqi.getDefaultConnection (actually in pynaoqi.getDefaultOptions)
import urllib2
import pynaoqi
options = pynaoqi.getDefaultOptions()
try:
con = pynaoqi.getDefaultConnection()
except urllib2.URLError, e:
print "error connecting: %s" % e
raise SystemExit
def main(clazz=Joints):
global con
con = pynaoqi.getDefaultConnection(with_twisted=True)
class Main(clazz):
def _onDestroy(self, *args):
super(Main, self)._onDestroy(*args)
print "quitting.."
reactor.stop()
from twisted.internet import gtk2reactor
try:
gtk2reactor.install()
except:
pass
window = Main(con)
reactor.run()
def getDCMProxy(deferred=False):
con = pynaoqi.getDefaultConnection()
return con.DCM
for propname, value in zip(['center-x', 'center-y'],
get_many('Ball', ['centerX', 'centerY'])):
if value == 'None':
self.notifyUserManModuleIsntLoaded()
return
else:
self.vision_ball.set_property(propname, value)
ball_bearing, ball_dist = get_many('Ball', ['bearing', 'dist'])
# ball dist is in cm, need to multiply by 10 to get mm
if ball_dist > 0.0:
ball_dist = ball_dist * 10 * F
x0, y0 = AREA_WIDTH / 2, 0
x, y = x0 + ball_dist * sin(ball_bearing), y0 + ball_dist * cos(ball_bearing)
print x, y
self.ball.set_property('center-x', x)
self.ball.set_property('center-y', y)
for post in ['BGRP', 'BGLP', 'YGLP', 'YGRP']:
obj = getattr(self, post.lower())
bearing_deg, distance = get_many(post, ['BearingDeg', 'Distance'])
x, y = get_many(post, ['CenterX', 'CenterY'])
if distance > 0.0:
obj.set_property('x', x)
obj.set_property('y', y)
# compute bearing my self, then set myball to that position
if __name__ == '__main__':
main = Main(pynaoqi.getDefaultConnection(with_twisted=True))
reactor.run()
def getMemoryProxy(deferred=False):
con = pynaoqi.getDefaultConnection()
return con.ALMemory
def __init__(self, con=None):
super(Joints, self).__init__()
global start_time
start_time = time()
if con is None:
con = pynaoqi.getDefaultConnection()
self.scales = scales = {}
self.con = con
self.updater = task.LoopingCall(self.getAngles)
self.battery_level_task = task.LoopingCall(self.getBatteryLevel)
self.vision = None
self.inertial = None
self.localization = None
if pynaoqi.options.localization_on_start:
self.toggleLocalization(None)
# Create the joint controlling and displaying slides (called by onJointData)
def onBodyAngles(cur_angles):
self._joint_panes = []
self._joints_table = table = gtk.Table(
rows=Scale.NUM_ROWS, columns=len(self.joint_names), homogeneous=False)
# put all the toggle buttons on the top
togglebox = gtk.HBox()
for name, start, end in [('Head', 0,2), ('LArm', 2, 8), ('LLeg', 8, 14),
('RLeg', 14, 20), ('RArm', 20, 26)]:
joints, angles = self.joint_names[start:end], cur_angles[start:end]
pane = ScalePane(parent=self, name=name, table=table, start_joint_num=start,
joints=joints, cur_angles=angles)
self._joint_panes.append(pane)
# update visibility toggling lists
togglebox.add(pane.toggle())
self._joints_container.pack_start(togglebox, False, False, 0)
self._joints_container.add(table)
self._joints_widgets.add(table)
self._all_widgets.add(table)
self.updater.start(DT_CHECK_FOR_NEW_ANGLES)
# we added a bunch of widgets, show them (this is async to __init__)
w.show_all()
def onJointData(results):
# called when we have the joint number, we can do this parallel but it's not
# that time consuming. Besides, joint data is cached, should only happen once on the
# machine (unless you delete ~/.burst_joint_data.pickle
self.joint_names, self.joint_limits = results
self.con.ALMotion.getAngles('Body', False).addCallback(onBodyAngles) # getAngles - second param is useSensors (use True?)
# initiate network request that will lead to slides creation.
# do everything based on a initDeferred, otherwise methods will not be available.
self.con.modulesDeferred.addCallback(lambda result:
self.con.ALMotion.initDeferred.addCallback(lambda _: getJointData(self.con).addCallback(onJointData))
)
# initiate battery task when pynaoqi finishes loading
self.con.modulesDeferred.addCallback(lambda _: self.battery_level_task.start(
DT_CHECK_BATTERY_LEVEL))
w = self._w
w.set_title('naojoints - %s (%s)' % (self.con.host, host_to_ip(self.con.host)))
self.c = c = gtk.VBox()
self._joints_container = gtk.VBox() # top - buttons, bottom - joints sliders table
w.add(c)
# Create Many Buttons on Top
bat_stat_eventbox = gtk.EventBox()
self._battery_level_button = battery_status_label = gtk.Label()
bat_stat_eventbox.connect('button-press-event', self._toggleAllButtonsExceptBattery)
bat_stat_eventbox.add(battery_status_label)
self._mirror_moves = mirror_moves = gtk.CheckButton('mirror moves')
top_buttons_data = [
(bat_stat_eventbox, None),
(mirror_moves, None),
('print angles', self.printAngles),
('stiffness on', self.setStiffnessOn),
('stiffness off', self.setStiffnessOff),
('vision', self.toggleVision),
('notes', self.toggleNotes),
('inertial', self.toggleInertial),
('localization', self.toggleLocalization),
]
chains = ['Head', 'LLeg', 'LArm', 'RArm', 'RLeg']
stiffness_off_buttons_data = [
('%s OFF' % chain,
lambda _, chain=chain: self.con.ALMotion.setChainStiffness(chain, 0.0))
for chain in chains]
stiffness_on_buttons_data = [
('%s on' % chain,
lambda _, chain=chain: self.con.ALMotion.setChainStiffness(chain, 1.0))
for chain in chains]
# XXX - import burst here so it doesn't parse sys.argv
import burst.moves as moves
import burst.moves.poses as poses
import burst.moves.walks as walks
import burst
moves_buttons_data = [(move_name, lambda _, move=getattr(poses, move_name):
self.con.ALMotion.executeMove(move))
for move_name in moves.NAOJOINTS_EXECUTE_MOVE_MOVES]
self._walkconfig = walks.STRAIGHT_WALK.walkParameters
self._default_walk_speed = walks.STRAIGHT_WALK.defaultSpeed
def updateWalkConfig(_):
self.con.ALMotion.getWalkConfig().addCallback(self.setWalkConfig)
self._start_walk_count = 1
def startWalkTest(result=None):
print "start walk req %s" % self._start_walk_count
self._start_walk_count += 1
self.con.ALMotion.getRemainingFootStepCount().addCallback(startWalkCb)
def startWalkCb(steps):
if steps > 0:
print "remaining footsteps, not calling walk"
else:
self.con.ALMotion.walk()
def doWalk(steps):
# distance [m], # 20ms cycles per step
distance_per_step = self._walkconfig[WalkParameters.StepLength]
import pdb; pdb.set_trace()
return setWalkConfig(self.con, self._walkconfig).addCallback(
lambda result: self.con.ALMotion.addWalkStraight(
steps * distance_per_step, self._default_walk_speed).addCallback(startWalkTest)
)
def doArc(angle, radius=0.5, cycles_per_step=60):
# angle [rad], radius [m], # 20ms cycles per step
return self.con.ALMotion.addWalkArc(
angle, radius, cycles_per_step).addCallback(startWalkTest)
def doTurn(angle, cycles_per_step=60):
# angle [rad], # 20ms cycles per step
return self.con.ALMotion.addTurn(
angle, cycles_per_step).addCallback(startWalkTest)
toggle_buttons_data = [
('all', self.onShowAll),
('joints only', self.onShowJointsOnly),
('stiffness toggle', None),
('buttons only', self.onShowButtonsOnly),
]
stiffness_toggle_index = 2
walk_steps = 4
walk_buttons_data = [
('Walk: get config', updateWalkConfig),
('fw %s' % walk_steps, lambda _, steps=walk_steps: doWalk(walk_steps)),
('rev %s' % walk_steps, lambda _, steps=-walk_steps: doWalk(-walk_steps)),
('rt 45', lambda _, steps=1: doTurn(-pi / 4)),
('lt 45', lambda _, steps=-1: doTurn(pi / 4)),
('arc right 1', lambda _, steps=1: doArc( -1 )),
('arc left 1', lambda _, steps=-1: doArc( 1 )),
]
top_strip, top_buttons = create_button_strip(top_buttons_data)
stiffness_off, self.stiffness_off_buttons = (
create_button_strip(stiffness_off_buttons_data))
stiffness_on, self.stiffness_on_buttons = (
create_button_strip(stiffness_on_buttons_data))
moves_strip, moves_buttons = create_button_strip(moves_buttons_data)
walk_strip, walk_buttons = create_button_strip(walk_buttons_data)
toggle_strip, toggle_buttons = create_button_strip(toggle_buttons_data)
# Python bug? if I don't set the result of ToggleButton to something
# it is lost to the garbage collector.. very hard to debug, since you just
# have a missing dictionary, but no actual error..
self._stiffness_toggle = ToggleButton(button=toggle_buttons[stiffness_toggle_index],
widgets = (stiffness_on, stiffness_off))
for button_strip in [top_strip, toggle_strip, stiffness_off, stiffness_on, moves_strip, walk_strip]:
c.pack_start(button_strip, False, False, 0)
# add the joints container after all the buttons
c.add(self._joints_container)
# lists for toggling visibility with appropriate callbacks
self._buttons_widgets = set([stiffness_off, stiffness_on, moves_strip, walk_strip])
self._joints_widgets = set([])
self._all_widgets = self._buttons_widgets.union(self._joints_widgets)
self._all_but_bat_label = self._all_widgets.union(set(top_buttons[1:]))
w.resize(700, 400)
w.show_all()
def getSonarProxy(deferred=False):
con = pynaoqi.getDefaultConnection()
return getattr(con, SONAR_MODULE)
def simulateButtonsPress(con, vars):
setIt(con, vars, ON)
reactor.callLater(0.5, setIt, con, vars, OFF)
reactor.callLater(1.0, reactor.stop)
def getvar(name):
global s, oldstdout
for k, v in vars.items():
if k.startswith(name):
ret = v
break
else:
k, ret = 'chest', vars['chest']
sys.stdout = oldstdout
print "Pressing %s" % k
return ret
if __name__ == '__main__':
import StringIO
s=StringIO.StringIO()
oldstdout = sys.stdout
sys.stdout = s
import player_init
import pynaoqi
con = pynaoqi.getDefaultConnection()
con.modulesDeferred.addCallback(
lambda _: simulateButtonsPress(con, getvar(sys.argv[-1].lower()))).addErrback(log.err)
reactor.run()
def getSentinelProxy(deferred=False):
con = pynaoqi.getDefaultConnection()
return con.ALSentinel if hasattr(con, "ALSentinel") else None
def getImopsProxy(deferred=False):
con = pynaoqi.getDefaultConnection()
return hasattr(con, "imops") and con.imops or None
def getBurstMemProxy(deferred=False):
con = pynaoqi.getDefaultConnection()
return getattr(con, BURST_SHARED_MEMORY_PROXY_NAME)
def getALVideoDeviceProxy(deferred=False):
con = pynaoqi.getDefaultConnection()
return getattr(con, VIDEO_MODULE)
def getSpeechProxy(deferred=False):
""" return None if nothing there """
con = pynaoqi.getDefaultConnection()
return hasattr(con, "ALTextToSpeech") and con.ALTextToSpeech or None