def __init__(self, qiapp):
# generic activity boilerplate
self.qiapp = qiapp
self.session = qiapp.session
self.events = stk.events.EventHelper(qiapp.session)
self.s = stk.services.ServiceCache(qiapp.session)
self.logger = stk.logging.get_logger(qiapp.session, self.APP_ID)
# Wait some services
self.session.waitForService("ALMotion")
self.session.waitForService("DXHomeFinder") # NB it may not be there
self.session.waitForService("ALBasicAwareness")
self.session.waitForService("ALFaceDetection")
# Internal variable
self.last_time_at_home = time.time()
self.started_solitary_wandering_time = time.time()
self.state = StrollState.WANDERING
self.life_state = "unknown"
self.time_last_define_home = 0
self.time_last_failed_go_gome = 0
self.is_proactive_activated = False
self.research_360_activated = False
self.last_obstacle = None
self.notifications = []
# subscribe face detection
self.s.ALFaceDetection.subscribe(self.APP_ID, 500, 0)
# periodic task
self.update_task = qi.PeriodicTask()
self.update_task.setCallback(self.update)
self.update_task.setUsPeriod(ONE_SECOND_IN_US)
# periodic task
self.reset_body_task = qi.PeriodicTask()
self.reset_body_task.setCallback(self.reset_angle)
self.reset_body_task.setUsPeriod(HUNDRED_SECOND_IN_US)
# State transitions
self.changing_state_lock = threading.Lock()
self.changing_state = False
self.queued_state = None
# configuration
self.prefs = None
# Sub objects
self.go_engage_state = GoEngageState(qiapp.session, self.s,
self.logger, self)
self.tablet_helper = tablet.TabletHelper(self.session, self.s)
self.configuration_reader = preferences.ConfigurationReader(\
self.session, self.s, self.on_prefs)
def set_routines(self):
sayMotivation = functools.partial(self.get_motivation)
self.sayMotivationTask = qi.PeriodicTask()
self.sayMotivationTask.setCallback(sayMotivation)
self.sayMotivationTask.setUsPeriod(self.settings['MotivationTime'])
self.sayMotivationTask.start(True)
getBorg = functools.partial(self.get_borg_scale)
self.askBorgTask = qi.PeriodicTask()
self.askBorgTask.setCallback(getBorg)
self.askBorgTask.setUsPeriod(100000000)
self.askBorgTask.start(True)
def set_routines(self):
print("settings memory routines")
motivate = functools.partial(self.motivation)
self.motivationTask = qi.PeriodicTask()
self.motivationTask.setCallback(motivate)
self.motivationTask.setUsPeriod(self.settings['MotivationTime'] *
self.micro)
borg = functools.partial(self.ask_borg)
self.borgTask = qi.PeriodicTask()
self.borgTask.setCallback(borg)
self.borgTask.setUsPeriod(self.settings['BorgTime'] * self.micro)
def __init__(self, session, logger, distance_to_do, slow_down_distance):
# services declaration
self.services = ["ALMotion", "ALMemory"]
self.session = session
self.logger = logger
# fetching mandatory services
for service_name in self.services:
self.session.waitForService(service_name)
setattr(self, service_name, self.session.service(service_name))
self.distance_to_do = float(distance_to_do)
self.move_failed_subscriber = self.ALMemory.subscriber(
"ALMotion/MoveFailed")
self.future = None
self.promise = None
self.robot_position_begin = None
self.error_odom = 0.3
self.last_distance = 0
self.cpt = 0
self.slow_down_distance = slow_down_distance
self.mode = "full_speed"
self.task = qi.PeriodicTask()
self.task.setCallback(self.move_routine)
self.task.setUsPeriod(int(0.01 * ONE_SEC_TIME)) # check every 0.1s
self.check_variables()
def _connect_services(self):
"""
Connect to all services required by BarcodeReader.
"""
self.logger.info('Connecting services...')
self.services_connected = qi.Promise()
services_connected_fut = self.services_connected.future()
def get_services():
"""Attempt to get all services"""
try:
self.mem = self.session.service('ALMemory')
self.vid = self.session.service("ALVideoDevice")
self.logger.info('All services are now connected')
self.services_connected.setValue(True)
except RuntimeError as e:
self.logger.warning('Missing service:\n {}'.format(e))
get_services_task = qi.PeriodicTask()
get_services_task.setCallback(get_services)
get_services_task.setUsPeriod(int(2 * 1000000)) # check every 2s
get_services_task.start(True)
try:
services_connected_fut.value(30 * 1000) # timeout = 30s
get_services_task.stop()
except RuntimeError:
get_services_task.stop()
self.logger.error('Failed to reach all services after 30 seconds.')
raise RuntimeError
def _connect_services(self):
"""Connect to all services required by ALTactileGesture"""
self.services_connected = qi.Promise()
services_connected_fut = self.services_connected.future()
def get_services():
"""Attempt to get all services"""
try:
self.memory = self.session.service('ALMemory')
self.prefs = self.session.service('ALPreferenceManager')
self.logger.info('got references to all services')
self.services_connected.setValue(True)
except RuntimeError as e:
self.logger.warning('missing service:\n {}'.format(e))
get_services_task = qi.PeriodicTask()
get_services_task.setCallback(get_services)
get_services_task.setUsPeriod(int(2 * 1000000)) # check every 2s
get_services_task.start(True)
try:
services_connected_fut.value(30 * 1000) # timeout = 30s
get_services_task.stop()
except RuntimeError:
get_services_task.stop()
self.logger.info('Failed to reach all services after 30 seconds')
raise RuntimeError
def __init__(self, app):
self.qiapp = app
self.session = app.session
self.session.waitForService('ALMemory')
self.session.waitForService('ALAutonomousLife')
self.session.waitForService('ALDialog')
self.memory = self.session.service('ALMemory')
self.life = self.session.service('ALAutonomousLife')
self.light = self.session.service('ALDarknessDetection')
self.audio = self.session.service('ALAudioPlayer')
self.tablet = self.session.service('ALTabletService')
self.dialog = self.session.service('ALDialog')
self.loop_task = qi.PeriodicTask()
self.loop_task.setCallback(self.check_if_can_sleep)
self.loop_task.setUsPeriod(long(1 * 1000 * 1000))
self.previousDarkness = 0
self.sleep_count = 0
self.memory.subscribeToEvent("go_to_sleep", "ALGoToSleepALD",
"check_start_sleep")
self.memory.subscribeToEvent("go_to_sleep_now", "ALGoToSleepALD",
"sleep_mode")
#put initialization code here
pass
def connect_services(self):
# connect all services required by your module
# done in async way over 30s,
# so it works even if other services are not yet ready when you start your module
# this is required when the service is autorun as it may start before other modules...
self.logger.info('Connecting services...')
self.services_connected = qi.Promise()
services_connected_fut = self.services_connected.future()
def get_services():
try:
self.memory = self.session.service('ALMemory')
self.dialog = self.session.service('ALDialog')
# connect other services if needed...
self.logger.info('All services are now connected')
self.services_connected.setValue(True)
except RuntimeError as e:
self.logger.warning(
'Still missing some service:\n {}'.format(e))
get_services_task = qi.PeriodicTask()
get_services_task.setCallback(get_services)
get_services_task.setUsPeriod(int(2 * 1000000)) # check every 2s
get_services_task.start(True)
try:
services_connected_fut.value(30 * 1000) # timeout = 30s
get_services_task.stop()
except RuntimeError:
get_services_task.stop()
self.logger.error('Failed to reach all services after 30 seconds')
raise RuntimeError
def __init__(self, robot_ip):
self.position = [[(0,0)]*15, [(0,0)]*15, [(0,0)]*15]
self.laser_callback = qi.PeriodicTask()
self.laser_callback.setCallback(self.get_laser_values)
self.laser_callback.setUsPeriod(200 * 1000)
self.connect(robot_ip)
def start_loop(self):
self.beat = 0
self.loop = qi.PeriodicTask()
self.loop.setCallback(self.loop_update)
self.loop.setUsPeriod(5 * TENTH_OF_SECOND)
self.loop.start(True)
#print dir(self.loop)
time.sleep(40)
print "Finished"
def _start_watch_face_characteristics(self):
self.logger.info('Start watch face characteristics')
self._face_characteristics = {
'age': None,
'gender': None,
'smile': None,
'expression': None
}
self._detection_task = qi.PeriodicTask()
self._detection_task.setCallback(self._update_face_characteristics_info)
self._detection_task.setUsPeriod(1*2000000)
self._detection_task.start(True)
def __init__(self, application):
self.application = application
self.session = application.session
self.service_name = self.__class__.__name__
# Getting a logger. Logs will be in /var/log/naoqi/servicemanager/{application id}.{service name}
self.logger = qi.Logger(self.service_name)
self.logger.info("Initializing...")
self.handDetect = False
def awaitServiceStartup(p):
try:
self.motion = self.session.service("ALMotion")
self.posture = self.session.service("ALRobotPosture")
self.speech = self.session.service("ALTextToSpeech")
self.memory = self.session.service("ALMemory")
self.speakingMovement = self.session.service(
"ALSpeakingMovement")
self.listeningMovement = self.session.service(
"ALListeningMovement")
self.backgroundMovement = self.session.service(
"ALBackgroundMovement")
except RuntimeError:
self.logger.info("Services not found")
return
self.logger.info("Services found")
p.setValue("SERVICES_READY")
p = qi.Promise()
f = p.future()
pt = qi.PeriodicTask()
pt.setCallback(functools.partial(awaitServiceStartup, p))
pt.setUsPeriod(2000000)
pt.start(True)
if f.value(30000) == "SERVICES_READY":
self.service_id = self.session.registerService(
self.service_name, self)
self.logger.info("Initialized!")
else:
self.logger.info("Initialization failed!")
pt.stop()
def __init__(self, session):
self.session = session
self.events = stk.events.EventHelper(self.session)
self.services = stk.services.ServiceCache(self.session)
self.logger = stk.logging.get_logger(self.session, self.APP_ID)
self.pod_pos_in_world = None # We don't know it at start
self.look_for_station_future = None
self.pod_future = None
self.search_future = None
self.future_sleep = None
self.search_future_track = None
self.task = qi.PeriodicTask()
self.task.setCallback(self.pod_detected)
self.task.setUsPeriod(100000) # 100ms
self.pod_position_from_robot = None
def __init__(self, qiapp):
self.qiapp = qiapp
self.events = stk.events.EventHelper(qiapp.session)
self.s = stk.services.ServiceCache(qiapp.session)
self.logger = get_logger(qiapp.session, self.APP_ID)
self.wait_for_dependency_services()
self.behavior_running = set([])
self.behavior_list = [
"deep_nao/behavior",
]
self.robot_posture_check_task = qi.PeriodicTask()
self.robot_posture_check_task.setCallback(
self.check_robot_posture_after_fall)
self.robot_posture_check_task.compensateCallbackTime(False)
self.robot_posture_check_task.setUsPeriod(500)
self.s.ALMemory.raiseEvent("DeepNao/RobotFallen", "False")
def subscribe(self, _params):
# self.logger.info("subscribe...")
self.last_timestamp = 0
self.image_nb = 0
params = dict(DEFAULT_PARAMS) # copy default params
params.update(_params)
subscriber_id = self._subscribe(params)
self.periodic_tasks[subscriber_id] = qi.PeriodicTask()
self.periodic_tasks[subscriber_id].setCallback(
functools.partial(self._task, subscriber_id, params))
resolution = params["resolution"]
fps = CAMERA_DATAS_AT_RESOLUTION[resolution]["fps"]
self.periodic_tasks[subscriber_id].setUsPeriod(1000000 / fps)
self.periodic_tasks[subscriber_id].start(True)
# self.logger.info("subscribe done")
return subscriber_id
def test_periodic_task():
t = qi.PeriodicTask()
def add():
global result
result += 1
t.setCallback(add)
t.setUsPeriod(1000)
t.start(True)
time.sleep(1)
t.stop()
assert result > 5 #how to find 5: plouf plouf plouf
cur = result
time.sleep(1)
assert cur == result
def zero_head():
global ses,per,motion,posture,tracker,video,tts,landmark,memory,movement
robotIP = "192.168.1.13"
try:
motion.angleInterpolationWithSpeed("HeadYaw", 0, 0.1)
except:
ses = qi.Session()
ses.connect(robotIP)
per = qi.PeriodicTask()
motion = ses.service('ALMotion')
posture = ses.service('ALRobotPosture')
tracker = ses.service('ALTracker')
video = ses.service('ALVideoDevice')
tts = ses.service('ALTextToSpeech')
landmark = ses.service('ALLandMarkDetection')
memory = ses.service('ALMemory')
movement=ses.service('MovementGraph')
def run(self):
"""
Main application loop. Waits for manual interruption.
"""
print "Starting RUN Loop..."
get_frame_task = qi.PeriodicTask()
get_frame_task.setCallback(self.get_frame)
get_frame_task.compensateCallbackTime(True)
get_frame_task.setUsPeriod(60000)
get_frame_task.start(True)
try:
while True:
time.sleep(1)
except KeyboardInterrupt:
self.logger.info("Interrupted by user, stopping Scheduler")
get_frame_task.stop()
self.video_device.unsubscribe(self.camera_handle)
self.face_detection.unsubscribe('SandBox')
sys.exit(0)
def start(self):
"""
Starts the main loop of the service.
It will get an image from the front camera every 0.5 second and
analyze it, looking for a barcode.
If a barcode is detected, it will trigger the signal onBarcodeDetected
with 2 values: the type of the code and the code itself.
"""
if not self.scanning:
self.logger.info("Starting...")
try:
getImageCallable = functools.partial(self._getImage)
self.getImageTask = qi.PeriodicTask()
self.getImageTask.setCallback(getImageCallable)
self.getImageTask.setUsPeriod(int(0.5 * 1000000)) # 0.5 sec
self.getImageTask.start(True)
self.logger.info("Started.")
self.scanning = True
except RuntimeError as err:
self.logger.info("Can't start {}".format(self.serviceName))
self.logger.warning(err)
def test_all_strand():
obj = Stranded(81)
sig = qi.Signal("m")
prop = qi.Property("m")
prom = qi.Promise()
per = qi.PeriodicTask()
per.setCallback(partial(obj.cb, None))
per.setUsPeriod(10000)
for i in range(20):
prom.future().addCallback(obj.cb)
for i in range(20):
sig.connect(obj.cb)
for i in range(20):
prop.connect(obj.cb)
per.start(True)
sig(None)
prop.setValue(42)
prom.setValue(None)
for i in range(20):
qi. async (partial(obj.cb, None))
obj.end.future().value()
per.stop()
def connect_services(self, timeout):
"""Connect to all services required.
:param timeout: time in seconds after wich acquiring services is
abandonned."""
self.services_connected = qi.Promise()
services_connected_fut = self.services_connected.future()
timeout = int(max([1, timeout]) * 1000)
period = int(min([2, timeout / 2]) * 1000000)
self.logger.info('Timeout: {} ms'.format(timeout))
self.logger.info('Period: {} us'.format(period))
get_services_task = qi.PeriodicTask()
get_services_task.setCallback(self.get_services)
get_services_task.setUsPeriod(period)
get_services_task.start(True)
try:
services_connected_fut.value(timeout)
get_services_task.stop()
except RuntimeError:
get_services_task.stop()
raise RuntimeError(
'Failed to reach all services after {} ms'.format(timeout))
def __init__(self, session):
self.session = session
self.memory = session.ALMemory
self.task = qi.PeriodicTask()
import math
from functools import partial
import numpy as np
import motion as mot
from operator import itemgetter
# Python Image Library
from PIL import Image
# OpenCV Libraries
import cv2
import cv2.cv as cv
''' ROBOT CONFIGURATION '''
robotIP = "192.168.137.109"
ses = qi.Session()
ses.connect(robotIP)
per = qi.PeriodicTask()
motion = ses.service('ALMotion')
posture = ses.service('ALRobotPosture')
tracker = ses.service('ALTracker')
video = ses.service('ALVideoDevice')
tts = ses.service('ALTextToSpeech')
landmark = ses.service('ALLandMarkDetection')
memory = ses.service('ALMemory')
resolution = 2 # VGA
colorSpace = 11 # RGB
trial_number = 12
path = ''
def take_pics(angleScan, CameraIndex):
