def stop():
try:
toolkit.verbose("Halting the system.")
robot.stop()
server.stop()
except:
toolkit.verbose("Uh-oh. An error occured when halting.")
def start():
"""
Connect with the NAO robot.
You can set to which NaoQi server to connect in settings.ini.
"""
global robot
# TODO: make a NAO_model
toolkit.verbose("Connecting with the Nao at %s:%s" %
(toolkit.settings["NAOQI_HOST"], toolkit.settings["NAOQI_PORT"]))
robot = NAO(toolkit.settings["NAOQI_HOST"], toolkit.settings["NAOQI_PORT"])
robot.connect()
robot.init_position()
toolkit.verbose("Connected!")
return robot
def compute_target_position(kchain_id, motion):
if robot is None or not robot.is_connected():
toolkit.verbose("It seems that the system is not setup properly. " \
+ "Did you run all start() sequences?")
return False
target = robot.kinematic_chain[kchain_id] + motion
laser_plane = robot.get_laser_plane()
if not in_plane(target, laser_plane):
new_target = orthogonal_projection(target, laser_plane)
difference = dist(new_target, target)
if abs(difference) > toolkit.settings['SCANNER_AREA_STICKINESS']:
details = {"off plane": target,\
"on plane": new_target,\
"difference": difference}
raise OutOfScannerRangeException, details
target = new_target
return target
def start():
try:
toolkit.verbose("Starting the system.")
robot.start()
server.start()
toolkit.verbose("Succesfully started the system!")
except:
toolkit.verbose("Uh-oh. An error occured at startup.")
def stop():
"""
Disconnect from the NAO robot.
"""
global robot
toolkit.verbose("Closing connection with Nao at %s:%s" %
(toolkit.settings["NAOQI_HOST"], toolkit.settings["NAOQI_PORT"]))
if not robot.proxies:
toolkit.verbose("Already disconnected.")
return
robot.close_connections()
robot = None
toolkit.verbose("Disconnecting succesful!")
def stop():
"""
Stop the joystick server.
It also registers that they are stopped.
"""
global server
toolkit.verbose("Stopping the joystick servers...")
if not server:
toolkit.verbose("Joystick server is already halted.")
return
server.shutdown()
server = None
toolkit.verbose("Joystick server has halted.")
def start():
"""
Start the joystick server.
It also registers that they are started.
"""
global server
toolkit.verbose("Starting the joystick server...")
if server:
toolkit.verbose("Joystick server was already up and running.")
return
server = ThreadedTCPServer((toolkit.settings["ANDROID_SERVER_HOST"],
toolkit.settings["ANDROID_SERVER_PORT"]), AndroidRequestHandler)
server_thread = threading.Thread(target=server.serve_forever)
server_thread.setDaemon(True)
server_thread.start()
toolkit.verbose("Joystick server has started.")
def main(*args, **kwargs):
options = getopts(args, kwargs)
process_options(options)
robot = Robot.get_instance()
limb_controllers = []
for index, limb in enumerate(robot.get_limbs()):
controller = None
toolkit.verbose("Put the controller in connectivity mode.")
while controller is None:
try:
controller = Controller.get_instance(index)
limb_controllers.append(LimbControlThread(robot, limb, controller))
except RuntimeError, e:
if "wiimote connection" in e.message:
toolkit.verbose("! Connection failed:. Please try again.")
else:
raise
toolkit.verbose("Paired controller %d with limb '%s'" % (index, limb))
for index, limb in enumerate(robot.get_limbs()):
controller = None
toolkit.verbose("Put the controller in connectivity mode.")
while controller is None:
try:
controller = Controller.get_instance(index)
limb_controllers.append(LimbControlThread(robot, limb, controller))
except RuntimeError, e:
if "wiimote connection" in e.message:
toolkit.verbose("! Connection failed:. Please try again.")
else:
raise
toolkit.verbose("Paired controller %d with limb '%s'" % (index, limb))
for control_thread in limb_controllers:
control_thread.start()
toolkit.verbose("Press enter to quit the session.")
while True:
if not raw_input(''):
break
for control_thread in limb_controllers:
control_thread.terminate()
robot.terminate()
toolkit.verbose("Quitting the system. Goodbye!")
@toolkit.dummy
def process_options(options):
pass
def LimbControlThread(threading.Thread):
