def fillLinksMap(address):
global docURLRoot
global naoqiVersion
session = Session()
session.connect('tcp://' + address + ':9559')
system = session.service("ALSystem")
naoqiVersion = system.systemVersion()
if naoqiVersion[:3] == "2.3":
docURLRoot = URL_2_3
elif naoqiVersion[:3] == "2.1":
docURLRoot = URL_2_1
else:
docURLRoot = URL_OTHER
url = docURLRoot + "index.html"
print "Parse : " + url
html = urlopen(url).read()
soup = BeautifulSoup.BeautifulSoup(html)
elements = soup.findAll("li")
for element in elements:
key = element.text.split(' ', 1)[0]
links = element.findAll("a")
if len(links) > 1:
# linksDicApi[key] = links[0]["href"]
linksDicOverview[key] = links[1]["href"]
def test_compile_with_life(set_no_life, force_compilation, restart_naoqi, cpu_server):
logs = log_viewer(ssh_tools.get_ip())
cpu = RemoteCPUWatch(ssh_tools.get_ip())
cpu.start_store('test_compile_with_life')
cpu.start_display()
s = Session()
s.connect(ssh_tools.get_ip())
dialog = s.service('ALDialog')
print "======================================="
print " Compiling ..."
print "=======================================\n"
logs.dump_logs('test_compile_with_life')
dialog._preloadMain()
cpu.stop_store()
cpu.stop_display()
logs.dump_logs('test_compile_with_life')
print "======================================="
print " Log Results:"
print " | Load time = {} s".format(logs.get_load_time())
print " | Main Dialog total compile time = {} s".format(logs.get_bundle_compile_time('welcome'))
print " | Main Dialog model compile time = {} s".format(logs.get_model_compile_time('welcome'))
print " | Main Dialog Japanese reco compile time = {} s".format(logs.get_reco_compile_time('welcome', 'Japanese'))
print " | Main Dialog English reco compile time = {} s".format(logs.get_reco_compile_time('welcome', 'English'))
print "=======================================\n"
print "======================================="
print " CPU Results:"
print " | Virtual Memory RSS difference = {}".format(cpu.get_naoqiservice_VmRSS_diff())
print " | Virtual Memory Size difference = {}".format(cpu.get_naoqiservice_VmSize_diff())
print " | Maximum 1 minute load average = {}".format(cpu.get_cpu_lavg1_max())
print " | Average IO time = {}".format( cpu.get_cpu_iotime_mean())
print " | Total Majflt = {}".format(cpu.get_cpu_majflt_sum())
print " | Average system time = {}".format(cpu.get_naoqiservice_stime_mean())
print " | Average user time = {}".format(cpu.get_naoqiservice_utime_mean())
print "=======================================\n"
def fillLinksMap(address):
global docURLRoot
global naoqiVersion
session = Session()
session.connect('tcp://'+address+':9559')
system = session.service("ALSystem")
naoqiVersion = system.systemVersion()
if naoqiVersion[:3] == "2.3":
docURLRoot = URL_2_3
elif naoqiVersion[:3] == "2.1" :
docURLRoot = URL_2_1
else:
docURLRoot = URL_OTHER
url = docURLRoot+"index.html"
print "Parse : "+url
html = urlopen(url).read()
soup = BeautifulSoup.BeautifulSoup(html)
elements = soup.findAll("li")
for element in elements:
key = element.text.split(' ', 1)[0]
links = element.findAll("a")
if len(links) > 1:
# linksDicApi[key] = links[0]["href"]
linksDicOverview[key] = links[1]["href"]
def _iter_services(address):
session = Session()
session.connect('tcp://' + address + ':9559')
for servicesDesc in session.services():
module_name = servicesDesc["name"]
if not module_name.startswith('_'):
yield module_name, session.service(module_name)
def force_compilation_old():
s = Session()
s.connect(ssh_tools.get_ip())
dialog = s.service('ALDialog')
#autonomous_life.setState("disabled")
dialog.deleteSerializationFiles()
dialog.resetAll()
dialog._resetPreload()
def _iter_services(address):
session = Session()
session.connect('tcp://'+address+':9559')
print "Number of services : "+str(len(session.services()))
for servicesDesc in session.services():
module_name = servicesDesc["name"]
if not module_name.startswith('_') and module_name not in BLACKLIST_MODULES:
yield module_name, session.service(module_name)
def _iter_services(address):
session = Session()
session.connect('tcp://' + address + ':9559')
print "Number of services : " + str(len(session.services()))
for servicesDesc in session.services():
module_name = servicesDesc["name"]
if not module_name.startswith(
'_') and module_name not in BLACKLIST_MODULES:
yield module_name, session.service(module_name)
def check_cloud_env():
ses = Session()
ses.connect(ssh_tools.get_ip())
system = ses.service("ALSystem")
version = system.version().split(".")
if len(version) < 2:
print version
sys.exit("wrong version format")
else:
if int(version[0]) == 2:
if int(version[1]) < 4:
print """Check cloud environment fixture:
This script is not made for versions inferior to 2.4.
You need to manually set the environment to preproduction. Use Simon's script to do so
Once you've done that, remove check_cloud_env fixture from all tests"""
sys.exit("Exiting program")
if int(version[1]) > 4:
alcloud = ses.service("_ALCloud")
if int(version[1]) == 4:
alcloud = ses.service("ALCloud")
if int(version[0]) != 2:
print """Check cloud environment fixture:
This script is made for naoqi version 2.0. You probably need to update it in order to make it work
for the version you are using"""
sys.exit("Exiting program")
if alcloud.getEnvironment() != "ppd":
print "Your robot cloud configuration needs to be changed to a preproduction environment."
#clean prod settings
#ssh_tools.runcommand('rm -r .local')
alcloud.setEnvironment("ppd")
print "Please wait for naoqi to restart..."
ssh_tools.nao_restart(life=False)
ses = Session()
ses.connect(ssh_tools.get_ip())
if int(version[1]) > 4:
alcloud = ses.service("_ALCloud")
if int(version[1]) == 4:
alcloud = ses.service("ALCloud")
if alcloud.getEnvironment() == "ppd":
print "Your configuration has been successfully changed"
else:
print "cloud configuration seems to have failed"
sys.exit("Exiting program")
return True
else:
return False
def NON_VALID_test_VALID_DIA_003_002(dialog_compile, reboot_no_life):
s = Session()
s.connect(ssh_tools.get_ip())
autonomous_life = s.service("ALAutonomousLife")
dialog = s.service("ALDialog")
logs = log_viewer(ssh_tools.get_ip())
logs.watch_precompile()
autonomous_life.setState("solitary")
while logs.get_precompile_result() is None:
time.sleep(3)
print "======================================="
print " Result = " + str(logs.get_precompile_result())
print "======================================="
def check_cloud_env():
ses = Session()
ses.connect(ssh_tools.get_ip())
system = ses.service("ALSystem")
if float(sy.version()) > 2.4:
alcloud = ses.service("_ALCloud")
else
alcloud = ses.service("ALCloud")
if alcloud.getEnvironment() != "ppd":
alcloud.setEnvironment("ppd")
print "Your robot cloud configuration needs to be changed to a preproduction environment."
print "Please wait for naoqi to restart..."
ssh_tools.nao_restart(life=False)
print "Your configuration has been successfully changed"
return True
else:
return False
def non_test_VALID_DIA_003_001(dialog_compile, restart_naoqi_no_life): # Missing associate dummy profile fixture
s = Session()
s.connect(ssh_tools.get_ip())
autonomous_life = s.service("ALAutonomousLife")
dialog = s.service("ALDialog")
logs = log_viewer(ssh_tools.get_ip())
logs.watch_precompile()
autonomous_life.setState("solitary")
while logs.get_precompile_result() is None:
time.sleep(3)
#print "sleep"
print "======================================="
print " Result = " + str(logs.get_precompile_result())
print "======================================="
def non_compile_with_life_2(set_no_life, force_compilation, restart_naoqi, cpu_server):
logs = log_viewer(ssh_tools.get_ip())
logs.watch_compile()
cpu = RemoteCPUWatch(ssh_tools.get_ip())
cpu.start_store()
cpu.start_display()
print cpu.files
s = Session()
s.connect(ssh_tools.get_ip())
dialog = s. service('ALDialog')
dialog.runDialog()
print 'Has run dialog'
print "======================================="
print " Result = " + str(logs.get_compile_result())
print "======================================="
cpu.stop_store()
cpu.stop_display()
dialog.stopDialog()
assert True
def set_robot_version(self):
try:
s = Session()
s.connect(self.ip)
system = s.service('ALSystem')
version = system.systemVersion()
split_version = version.split('.')
if len(split_version) < 2:
raise Exception('Wrong version format returned: {}'.format(version))
if split_version[0] < 2:
raise Exception('Unhandled version: {}'.format(version))
if split_version[1] < 4:
### SHOULD test on 2.3 and predecessors
raise Exception('Unhandled version: {}'.format(version))
self.version = int(split_version[1])
self.exact_version = version
except Exception as e:
print 'Version number processing went wrong: '
print e.message
print 'Considering we run on 2.4'
self.version = 4
class NaoqiWatcher(object):
def __init__(self, ip):
self.s = Session()
self.s.connect(ip)
self.a_life = self.s.service('ALAutonomousLife')
self.behavior_manager = self.s.service('ALBehaviorManager')
self.is_recording_activity = False
self.file = None
def start_activity_record(self, dirname='naoqi_watcher'):
if not self.is_recording_activity:
self.is_recording_activity = True
directory = os.path.join(DATA_PATH, dirname)
if not os.path.isdir(directory):
os.makedirs(directory)
file_path = os.path.join(directory, 'activity')
self.file = open(file_path, 'w')
self.file.write(CSV_SEP.join(HEADERS))
thr = threading.Thread(target=self.process_loop, args=())
thr.start()
return file_path
def stop_activity_record(self):
self.is_recording_activity = False
def process_loop(self):
while self.is_recording_activity:
time_stamp = str(time.time())
life_state = self.a_life.getState()
focused_activity = self.a_life.focusedActivity()
runniung_behaviors = BEHAVIOR_SEP.join(
self.behavior_manager.getRunningBehaviors())
to_write = [
time_stamp, life_state, focused_activity, runniung_behaviors,
'\n'
]
self.file.write(CSV_SEP.join(to_write))
time.sleep(TIME_STEP)
self.file.close()
self.file = None
def set_robot_version(self):
try:
s = Session()
s.connect(self.ip)
system = s.service('ALSystem')
version = system.systemVersion()
split_version = version.split('.')
if len(split_version) < 2:
raise Exception(
'Wrong version format returned: {}'.format(version))
if split_version[0] < 2:
raise Exception('Unhandled version: {}'.format(version))
if split_version[1] < 4:
### SHOULD test on 2.3 and predecessors
raise Exception('Unhandled version: {}'.format(version))
self.version = int(split_version[1])
self.exact_version = version
except Exception as e:
print 'Version number processing went wrong: '
print e.message
print 'Considering we run on 2.4'
self.version = 4
class NaoqiWatcher(object):
def __init__(self, ip):
self.s = Session()
self.s.connect(ip)
self.a_life = self.s.service('ALAutonomousLife')
self.behavior_manager = self.s.service('ALBehaviorManager')
self.is_recording_activity = False
self.file = None
def start_activity_record(self, dirname = 'naoqi_watcher'):
if not self.is_recording_activity:
self.is_recording_activity = True
directory = os.path.join(DATA_PATH, dirname)
if not os.path.isdir(directory):
os.makedirs(directory)
file_path = os.path.join(directory, 'activity')
self.file = open(file_path, 'w')
self.file.write(CSV_SEP.join(HEADERS))
thr = threading.Thread(target = self.process_loop, args = ())
thr.start()
return file_path
def stop_activity_record(self):
self.is_recording_activity = False
def process_loop(self):
while self.is_recording_activity:
time_stamp = str(time.time())
life_state = self.a_life.getState()
focused_activity = self.a_life.focusedActivity()
runniung_behaviors = BEHAVIOR_SEP.join(self.behavior_manager.getRunningBehaviors())
to_write = [time_stamp, life_state, focused_activity, runniung_behaviors, '\n']
self.file.write(CSV_SEP.join(to_write))
time.sleep(TIME_STEP)
self.file.close()
self.file = None
class log_viewer(object):
message_start = ''
message_stop = ''
is_logging = False
log_records = []
def __init__(self, ip, message_start = 'Enable wav logging', message_stop = 'Preload finished'):
log_viewer.message_start = message_start
log_viewer.message_stop = message_stop
self.s = Session()
self.s.connect(ip)
self.log_manager = self.s.service("LogManager")
self.listener = self.log_manager.getListener()
self.watch()
def watch(self):
self.listener.clearFilters()
self.listener.setLevel(logging.DEBUG)
self.listener.onLogMessage.connect(dialog_preload_timestamp_message)
###
### Helpers
###
def get_log_subset(self, substring = '', begin = '', end = '', logs = False):
if not logs:
logs = self.log_records
subset = []
appending = False
for log in logs:
if begin in log[1]:
appending = True
if substring in log[1] and appending:
subset.append(log)
if end in log[1]:
appending = False
return subset
def dump_logs(self, file_name):
if not os.path.isdir(RECORD_DIRECTORY):
os.makedirs(RECORD_DIRECTORY)
file_path = os.path.join(RECORD_DIRECTORY, file_name)
with open(file_path, 'w') as f:
f.write(CSV_SEP.join([x for x in HEADERS]))
for line in self.log_records:
f.write(CSV_SEP.join([str(x) for x in line]))
###
### Access functions
###
def get_load_time(self):
logs = self.get_log_subset(substring = 'Load topic')
timestamps = [x[0] for x in logs]
return np.max(timestamps) - np.min(timestamps)
def get_bundle_compile_time(self, bundle):
begin = 'Compile bundle: {}'.format(bundle)
print begin
log = self.get_log_subset(substring = 'Bundle compilation time', begin = begin, end = 'Bundle compilation time')
print log
return float(log[0][1].split()[-2]) / 1000
def get_model_compile_time(self, bundle):
begin = 'Compile bundle: {}'.format(bundle)
print begin
log = self.get_log_subset(substring = '...model compiled', begin = 'Compile bundle: {}'.format(bundle), end = 'Bundle compilation time')
print log
return float(log[0][1].rsplit()[-2].strip('(')) / 1000
def get_reco_compile_time(self, bundle, language):
bundle_log = self.get_log_subset(substring = '', begin = 'Compile bundle: {}'.format(bundle), end = 'Bundle compilation time')
log = self.get_log_subset(substring = 'Speech Recognition: Compilation time', begin = language, end = 'Speech Recognition: Compilation time', logs = bundle_log)
return float(log[0][1].rsplit()[-2]) / 1000
def get_timestamp_range(self, sequence):
begin = ''
end = ''
substring = ''
if sequence == 'loading':
substring = 'Load topic'
if sequence == 'compile_bundle':
begin = 'Compile bundle: welcome'
end = 'Bundle compilation time'
if sequence == 'compile_model':
begin = 'compile_bundle'
end = '...model compiled'
if sequence == 'compile_reco_Japanese':
pass
if sequence == 'compile_reco_English':
pass
log = self.get_log_subset(substring = substring, begin = begin, end = end)
return [x[0] for x in log]
def check_for_error(self, begin = '', end = ''):
self.get_log_subset(begin = '', end = '')
class NaoqiLaser(object):
# PEPPER laser specs
# see https://community.aldebaran.com/doc/2-1/family/juliette_technical/laser_juliette.html#juliette-laser
PEPPER_LASER_FREQ = 6 # [hZ] --> check on that
PEPPER_LASER_MIN_ANGLE = -0.523598776 # [rad]
PEPPER_LASER_MAX_ANGLE = 0.523598776 # [radi]
PEPPER_LASER_FOV = math.fabs(PEPPER_LASER_MIN_ANGLE) + math.fabs(
PEPPER_LASER_MAX_ANGLE)
PEPPER_LASER_MIN_RANGE = 0.1 # [m] --> no spec given here
# [m] --> same here, 5m as quality guess
PEPPER_LASER_MAX_RANGE = 5.0
# FRONT GROUND LASERS
PEPPER_LASER_GROUND_SHOVEL_POINTS = 3
PEPPER_LASER_GROUND_LEFT_POINTS = 1
PEPPER_LASER_GROUND_RIGHT_POINTS = 1
# SURROUNDING LASER
PEPPER_LASER_SRD_POINTS = 15
# memory key to fetch laser readings from
# see memory key listing https://community.aldebaran.com/doc/2-1/family/juliette_technical/juliette_dcm/actuator_sensor_names.html#lasers
# iterate over all segments e.g./SubDeviceList/Platform/LaserSensor/Front/Shovel/Seg01/X/Value
PEPPER_MEM_KEY_GROUND_SHOVEL = 'Device/SubDeviceList/Platform/LaserSensor/Front/Shovel/'
PEPPER_MEM_KEY_GROUND_LEFT = 'Device/SubDeviceList/Platform/LaserSensor/Front/Vertical/Left/'
PEPPER_MEM_KEY_GROUND_RIGHT = 'Device/SubDeviceList/Platform/LaserSensor/Front/Vertical/Right/'
PEPPER_MEM_KEY_SRD_FRONT = 'Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/'
PEPPER_MEM_KEY_SRD_LEFT = 'Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/'
PEPPER_MEM_KEY_SRD_RIGHT = 'Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/'
# ROS params to check
# acc. to spec: 40 kHz
PARAM_LASER_RATE = '~laser_rate'
PARAM_LASER_RATE_DEFAULT = PEPPER_LASER_FREQ
# frame id to publish
PARAM_LASER_SHOVEL_FRAME = '~laser_shovel_frame_id'
PARAM_LASER_SHOVEL_FRAME_DEFAULT = 'ShovelLaser_frame'
PARAM_LASER_GROUND_LEFT_FRAME = '~laser_ground_left_frame_id'
PARAM_LASER_GROUND_LEFT_FRAME_DEFAULT = 'VerticalLeftLaser_frame'
PARAM_LASER_GROUND_RIGHT_FRAME = '~laser_ground_right_frame_id'
PARAM_LASER_GROUND_RIGHT_FRAME_DEFAULT = 'VerticalRightLaser_frame'
PARAM_LASER_SRD_FRONT_FRAME = '~laser_srd_front_frame_id'
PARAM_LASER_SRD_FRONT_FRAME_DEFAULT = 'SurroundingFrontLaser_frame'
PARAM_LASER_SRD_LEFT_FRAME = '~laser_srd_left_frame_id'
PARAM_LASER_SRD_LEFT_FRAME_DEFAULT = 'SurroundingLeftLaser_frame'
PARAM_LASER_SRD_RIGHT_FRAME = '~laser_srd_right_frame_id'
PARAM_LASER_SRD_RIGHT_FRAME_DEFAULT = 'SurroundingRightLaser_frame'
TOPIC_LASER_SHOVEL = '~/laser/shovel/'
TOPIC_LASER_GROUND_LEFT = '~/laser/ground_left/'
TOPIC_LASER_GROUND_RIGHT = '~/laser/ground_right/'
TOPIC_LASER_SRD_FRONT = '/pepper/scan_front'
TOPIC_LASER_SRD_LEFT = '/pepper/scan_left'
TOPIC_LASER_SRD_RIGHT = '/pepper/scan_right'
PEPPER_LASER_SUB_NAME = 'pepper_ros_laser'
def __init__(self, pointcloud=True, laserscan=False):
self.pointcloud = pointcloud
self.laserscan = laserscan
# NaoqiNode.__init__(self, 'pepper_node')
self.session = Session()
self.session.connect('tcp://localhost:9559')
# ROS initialization;
self.connectNaoQi()
# default sensor rate: 25 Hz (50 is max, stresses Nao's CPU)
self.laserRate = rospy.Rate(
rospy.get_param(self.PARAM_LASER_RATE,
self.PARAM_LASER_RATE_DEFAULT))
self.laserShovelFrame = rospy.get_param(
self.PARAM_LASER_SHOVEL_FRAME,
self.PARAM_LASER_SHOVEL_FRAME_DEFAULT)
self.laserGroundLeftFrame = rospy.get_param(
self.PARAM_LASER_GROUND_LEFT_FRAME,
self.PARAM_LASER_GROUND_LEFT_FRAME_DEFAULT)
self.laserGroundRightFrame = rospy.get_param(
self.PARAM_LASER_GROUND_RIGHT_FRAME,
self.PARAM_LASER_GROUND_RIGHT_FRAME_DEFAULT)
self.laserSRDFrontFrame = rospy.get_param(
self.PARAM_LASER_SRD_FRONT_FRAME,
self.PARAM_LASER_SRD_FRONT_FRAME_DEFAULT)
self.laserSRDLeftFrame = rospy.get_param(
self.PARAM_LASER_SRD_LEFT_FRAME,
self.PARAM_LASER_SRD_LEFT_FRAME_DEFAULT)
self.laserSRDRightFrame = rospy.get_param(
self.PARAM_LASER_SRD_RIGHT_FRAME,
self.PARAM_LASER_SRD_RIGHT_FRAME_DEFAULT)
if self.pointcloud:
self.pcShovelPublisher = rospy.Publisher(self.TOPIC_LASER_SHOVEL +
'pointcloud',
PointCloud2,
queue_size=1)
self.pcGroundLeftPublisher = rospy.Publisher(
self.TOPIC_LASER_GROUND_LEFT + 'pointcloud',
PointCloud2,
queue_size=1)
self.pcGroundRightPublisher = rospy.Publisher(
self.TOPIC_LASER_GROUND_RIGHT + 'pointcloud',
PointCloud2,
queue_size=1)
self.pcSRDFrontPublisher = rospy.Publisher(
self.TOPIC_LASER_SRD_FRONT + 'pointcloud',
PointCloud2,
queue_size=1)
self.pcSRDLeftPublisher = rospy.Publisher(
self.TOPIC_LASER_SRD_LEFT + 'pointcloud',
PointCloud2,
queue_size=1)
self.pcSRDRightPublisher = rospy.Publisher(
self.TOPIC_LASER_SRD_RIGHT + 'pointcloud',
PointCloud2,
queue_size=1)
if self.laserscan:
self.laserShovelPublisher = rospy.Publisher(
self.TOPIC_LASER_SHOVEL + 'scan', LaserScan, queue_size=1)
self.laserGroundLeftPublisher = rospy.Publisher(
self.TOPIC_LASER_GROUND_LEFT + 'scan', LaserScan, queue_size=1)
self.laserGroundRightPublisher = rospy.Publisher(
self.TOPIC_LASER_GROUND_RIGHT + 'scan',
LaserScan,
queue_size=1)
self.laserSRDFrontPublisher = rospy.Publisher(
self.TOPIC_LASER_SRD_FRONT, LaserScan, queue_size=1)
self.laserSRDLeftPublisher = rospy.Publisher(
self.TOPIC_LASER_SRD_LEFT, LaserScan, queue_size=1)
self.laserSRDRightPublisher = rospy.Publisher(
self.TOPIC_LASER_SRD_RIGHT, LaserScan, queue_size=1)
self.laserSRDFrontPublisher_test = rospy.Publisher(
"~/pepper_navigation/front", LaserScan, queue_size=1)
# (re-) connect to NaoQI:
def connectNaoQi(self):
# rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
# self.laserProxy = self.session.service("ALLaser")
self.memProxy = self.session.service("ALMemory")
if self.memProxy is None:
print "could not start either laser or memory proxy"
exit(1)
# fetch laser values
''' the idea here is to get the point xy
calculate the length from the origin (range)
assumption: field of view spanning from very min to very max
'''
def fetchLaserValues(self, keyPrefix, scanNum):
ranges = []
# traverse backwards
tmp_array = []
for i in xrange(scanNum, 0, -1):
keyX = keyPrefix + 'Seg' + '%02d' % (i, ) + '/X/Sensor/Value'
keyY = keyPrefix + 'Seg' + '%02d' % (i, ) + '/Y/Sensor/Value'
tmp_array.append(keyX)
tmp_array.append(keyY)
memData = self.memProxy.getListData(tmp_array)
for i in xrange(0, len(tmp_array), 2):
x = memData[i]
y = memData[i + 1]
ranges.append(math.sqrt(math.pow(x, 2.0) + math.pow(y, 2.0)))
return ranges
def fetchPCValues(self, keyPrefix, scanNum):
scans = []
for i in xrange(scanNum, 0, -1):
keyX = keyPrefix + 'Seg' + '%02d' % (i, ) + '/X/Sensor/Value'
keyY = keyPrefix + 'Seg' + '%02d' % (i, ) + '/Y/Sensor/Value'
x = self.memProxy.getData(keyX)
y = self.memProxy.getData(keyY)
scans.append(x)
scans.append(y)
scans.append(0.0)
ba = struct.pack('%sf' % len(scans), *scans)
return ba
def createPointCloudMessage(self, frameID, keyPrefix, scanNum):
pointCloudMsg = PointCloud2()
pointCloudMsg.header.frame_id = frameID
pointCloudMsg.header.stamp = rospy.Time.now()
pointCloudMsg.height = 1
pointCloudMsg.width = scanNum
pointCloudMsg.is_dense = False
pointCloudMsg.is_bigendian = False
pointCloudMsg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
pointCloudMsg.point_step = 4 * 3
pointCloudMsg.row_step = pointCloudMsg.point_step * pointCloudMsg.width
pointCloudMsg.data = self.fetchLaserValues(keyPrefix, scanNum)
return pointCloudMsg
def createLaserMessage(self, frameID, keyPrefix, scanNum):
laserScanMsg = LaserScan()
laserScanMsg.header.frame_id = frameID
laserScanMsg.angle_min = self.PEPPER_LASER_MIN_ANGLE
laserScanMsg.angle_max = self.PEPPER_LASER_MAX_ANGLE
laserScanMsg.angle_increment = self.PEPPER_LASER_FOV / scanNum
laserScanMsg.range_min = self.PEPPER_LASER_MIN_RANGE
laserScanMsg.range_max = self.PEPPER_LASER_MAX_RANGE
return laserScanMsg
# do it!
def run(self):
# start subscriber to laser sensor
# self.laserProxy.subscribe(self.PEPPER_LASER_SUB_NAME)
# we publish 6 laser messages in total
# 1. shovel, 2. ground_left, 3. ground_right
# 4. srd_front 5. srd_left 6. srd_right
if self.pointcloud:
shovelPC = self.createPointCloudMessage(
self.laserShovelFrame, self.PEPPER_MEM_KEY_GROUND_SHOVEL,
self.PEPPER_LASER_GROUND_SHOVEL_POINTS)
groundLeftPC = self.createPointCloudMessage(
self.laserGroundLeftFrame, self.PEPPER_MEM_KEY_GROUND_LEFT,
self.PEPPER_LASER_GROUND_LEFT_POINTS)
groundRightPC = self.createPointCloudMessage(
self.laserGroundRightFrame, self.PEPPER_MEM_KEY_GROUND_RIGHT,
self.PEPPER_LASER_GROUND_RIGHT_POINTS)
srdFrontPC = self.createPointCloudMessage(
self.laserSRDFrontFrame, self.PEPPER_MEM_KEY_SRD_FRONT,
self.PEPPER_LASER_SRD_POINTS)
srdLeftPC = self.createPointCloudMessage(
self.laserSRDLeftFrame, self.PEPPER_MEM_KEY_SRD_LEFT,
self.PEPPER_LASER_SRD_POINTS)
srdRightPC = self.createPointCloudMessage(
self.laserSRDRightFrame, self.PEPPER_MEM_KEY_SRD_RIGHT,
self.PEPPER_LASER_SRD_POINTS)
if self.laserscan:
shovelScan = self.createLaserMessage(
self.laserShovelFrame, self.PEPPER_MEM_KEY_GROUND_SHOVEL,
self.PEPPER_LASER_GROUND_SHOVEL_POINTS)
groundLeftScan = self.createLaserMessage(
self.laserGroundLeftFrame, self.PEPPER_MEM_KEY_GROUND_LEFT,
self.PEPPER_LASER_GROUND_LEFT_POINTS)
groundRightScan = self.createLaserMessage(
self.laserGroundRightFrame, self.PEPPER_MEM_KEY_GROUND_RIGHT,
self.PEPPER_LASER_GROUND_RIGHT_POINTS)
srdFrontScan = self.createLaserMessage(
self.laserSRDFrontFrame, self.PEPPER_MEM_KEY_SRD_FRONT,
self.PEPPER_LASER_SRD_POINTS)
srdLeftScan = self.createLaserMessage(self.laserSRDLeftFrame,
self.PEPPER_MEM_KEY_SRD_LEFT,
self.PEPPER_LASER_SRD_POINTS)
srdRightScan = self.createLaserMessage(
self.laserSRDRightFrame, self.PEPPER_MEM_KEY_SRD_RIGHT,
self.PEPPER_LASER_SRD_POINTS)
while (not rospy.is_shutdown()):
if self.laserscan:
# fetch values
now = rospy.Time.now()
shovelScan.header.stamp = now
shovelScan.ranges = self.fetchLaserValues(
self.PEPPER_MEM_KEY_GROUND_SHOVEL,
self.PEPPER_LASER_GROUND_SHOVEL_POINTS)
groundLeftScan.header.stamp = now
groundLeftScan.ranges = self.fetchLaserValues(
self.PEPPER_MEM_KEY_GROUND_LEFT,
self.PEPPER_LASER_GROUND_LEFT_POINTS)
groundRightScan.header.stamp = now
groundRightScan.ranges = self.fetchLaserValues(
self.PEPPER_MEM_KEY_GROUND_RIGHT,
self.PEPPER_LASER_GROUND_RIGHT_POINTS)
srdFrontScan.header.stamp = now
srdFrontScan.ranges = self.fetchLaserValues(
self.PEPPER_MEM_KEY_SRD_FRONT,
self.PEPPER_LASER_SRD_POINTS)
srdLeftScan.header.stamp = now
srdLeftScan.ranges = self.fetchLaserValues(
self.PEPPER_MEM_KEY_SRD_LEFT, self.PEPPER_LASER_SRD_POINTS)
srdRightScan.header.stamp = now
srdRightScan.ranges = self.fetchLaserValues(
self.PEPPER_MEM_KEY_SRD_RIGHT,
self.PEPPER_LASER_SRD_POINTS)
# publish messages
self.laserShovelPublisher.publish(shovelScan)
self.laserGroundLeftPublisher.publish(groundLeftScan)
self.laserGroundRightPublisher.publish(groundRightScan)
self.laserSRDFrontPublisher.publish(srdFrontScan)
self.laserSRDLeftPublisher.publish(srdLeftScan)
self.laserSRDRightPublisher.publish(srdRightScan)
if self.pointcloud:
# fetch values
shovelPC.header.stamp = rospy.Time.now()
shovelPC.data = self.fetchPCValues(
self.PEPPER_MEM_KEY_GROUND_SHOVEL,
self.PEPPER_LASER_GROUND_SHOVEL_POINTS)
groundLeftPC.header.stamp = rospy.Time.now()
groundLeftPC.data = self.fetchPCValues(
self.PEPPER_MEM_KEY_GROUND_LEFT,
self.PEPPER_LASER_GROUND_LEFT_POINTS)
groundRightPC.header.stamp = rospy.Time.now()
groundRightPC.data = self.fetchPCValues(
self.PEPPER_MEM_KEY_GROUND_RIGHT,
self.PEPPER_LASER_GROUND_RIGHT_POINTS)
srdFrontPC.header.stamp = rospy.Time.now()
srdFrontPC.data = self.fetchPCValues(
self.PEPPER_MEM_KEY_SRD_FRONT,
self.PEPPER_LASER_SRD_POINTS)
srdLeftPC.header.stamp = rospy.Time.now()
srdLeftPC.data = self.fetchPCValues(
self.PEPPER_MEM_KEY_SRD_LEFT, self.PEPPER_LASER_SRD_POINTS)
srdRightPC.header.stamp = rospy.Time.now()
srdRightPC.data = self.fetchPCValues(
self.PEPPER_MEM_KEY_SRD_RIGHT,
self.PEPPER_LASER_SRD_POINTS)
# publish messages
self.pcShovelPublisher.publish(shovelPC)
self.pcGroundLeftPublisher.publish(groundLeftPC)
self.pcGroundRightPublisher.publish(groundRightPC)
self.pcSRDFrontPublisher.publish(srdFrontPC)
self.pcSRDLeftPublisher.publish(srdLeftPC)
self.pcSRDRightPublisher.publish(srdRightPC)
# sleep
self.laserRate.sleep()
class RobotManager:
def __init__(self):
self.connected = False
self.session = Session()
self.movePool1 = [
kisses_pose, introduction_pose, stretch1_pose, stretch2_pose,
stretch3_pose, showMuscles1_pose, showMuscles2_pose
] # before starting
self.movePool2 = [
sprinkler_pose, shaking_booty, hands_up_down_pose,
one_hand_air_rock_pose, hand_near_eyes_retro,
leg_hand_slip_dance_move
]
self.movePool3 = [
wave_move, egyptian_twist_move, squats_dance_move, duck_dance_move,
disco_right_hand, disco_left_hand, rolling_hands_right,
rolling_hands_left
]
self.movePool4 = [
hand_near_eyes_retro_left, hand_near_eyes_retro_right,
hand_near_eyes_retro, leg_twist_move, rolling_hands_left,
rolling_hands_right
]
self.movePool5 = [
rolling_hands_right, snap_right_move, snap_left_move,
clapping_in_the_air, dance_move1, leg_hand_slip_dance_move,
shaking_head_pose
]
self.movePool6 = [
hand_swinging, stamp_foot_move, two_hand_air_rock_pose,
robot_dance_move, two_hand_air_pose, one_hand_air_pose
]
self.movePool7 = [
kisses_pose, introduction_pose, winner_pose, winner2_pose,
excited_pose, excited2_pose, lookHand_pose, proud_pose, happy_pose
] # for exiting
def robotConnect(self):
print("robot trying to connect")
try:
self.textSpeakProxy = ALProxy("ALTextToSpeech", NAO_IP, NAO_PORT)
self.postureProxy = ALProxy("ALRobotPosture", NAO_IP, NAO_PORT)
self.motionProxy = ALProxy("ALMotion", NAO_IP, NAO_PORT)
self.session.connect("tcp://" + NAO_IP + ":" + str(NAO_PORT))
self.connected = True
print("robot connected")
except RuntimeError:
print("Can't connect to Naoqi at ip \"" + NAO_IP + "\" on port " +
str(NAO_PORT) + ".\n")
def initRobot(self):
if self.connected:
self.postureProxy.goToPosture("StandInit", 1.0)
self.textSpeakProxy.post.say("Hello")
self.motionProxy.setFallManagerEnabled(True)
## Dance timeline:
## StandInit -> A1(introductory moves) -> hello_wave -> A2(dance)-> StandZero ->
## -> A3(dance with squat)-> Sit -> A4(sitted poses)-> SitRelax -> A5(dances) ->
## -> Stand -> A6(last dance moves)-> wipe_forehead -> A7(finishing moves) -> Crouch
##
def planning(self):
print(self.postureProxy.getPostureList())
# listOfMoves = ["Stand", "StandZero", "Sit", "SitRelax"]
# for move in listOfMoves:
# time.sleep(1)
# self.postureProxy.goToPosture(move, 0.7)
# self.textSpeakProxy.post.say("Tired")
self.runMotion(introduction_pose)
self.runMotion(bow_closing_move)
def onFinish(self):
self.textSpeakProxy.post.say("Bye!")
self.postureProxy.goToPosture("Crouch", 0.5)
def runMotion(self, pose, post=True):
# launch animation
# if post == True:
# self.motionProxy.post.angleInterpolationBezier(pose.names, pose.times, pose.keys)
# else:
self.motionProxy.angleInterpolationBezier(pose.names, pose.times,
pose.keys)
def life_off():
s = Session()
s.connect(ssh_tools.get_ip())
alife = s.service('ALAutonomousLife')
alife.setState ('disabled')
class log_viewer(object):
bundle_buff = False
log_message = {'Bundle compilation time':[],
'...model compiled':[],
'BNFwelcomeJapanese generated':[],
'BNFwelcomeEnglish generated' : [],
'Speech Recognition: Compilation time': [],
}
#[W] 1449044596.920087 3740 Dialog.StrategyBnf: Speech Recognition: Compilation time: 63623 ms
#[W] 1449044602.347930 3740 Dialog.StrategyBnf: Speech Recognition: Compilation time: 5262 ms
def __init__(self, ip):
self.s = Session()
self.s.connect(ip)
self.log_manager = self.s.service("LogManager")
self.listener = self.log_manager.getListener()
def watch_precompile(self):
self.listener.clearFilters()
self.listener.onLogMessage.connect(_log_message_precompile_use)
def watch_compile(self):
self.listener.clearFilters()
self.listener.onLogMessage.connect(_log_message_compile_time)
def get_precompile_result(self):
if not log_viewer.log_message.has_key(" welcome 1"): # Complete
return None
else:
return log_viewer.log_message[" welcome 1"] # Complete
def get_compile_result(self):
log_viewer.log_message
if not self.check_bundle_compile_finished():
return None
else:
#print int(log_viewer.log_message['Bundle compilation time'][0].rsplit()[-2])
print log_viewer.log_message
return_val = self.treat_compile_message()
log_viewer.log_message = {'Bundle compilation time':[],
'...model compiled':[],
'BNFwelcomeJapanese generated':[],
'BNFwelcomeEnglish generated' : [],
'Speech Recognition: Compilation time': [],
}
return return_val
def check_bundle_compile_finished(self):
### Custom
#print len(log_viewer.log_message['Bundle compilation time'])
if len(log_viewer.log_message['Bundle compilation time']) != 0:
return True
else:
return False
def treat_compile_message(self):
### Custom
return_val = {}
return_val['total'] = int(log_viewer.log_message['Bundle compilation time'][0].rsplit()[-2])
return_val['model'] = int(log_viewer.log_message['...model compiled'][0].rsplit()[-2].strip('('))
return_val['jpj bnf'] = int(log_viewer.log_message['BNFwelcomeJapanese generated'][0].rsplit()[-2].strip('('))
return_val['enu bnf'] = int(log_viewer.log_message['BNFwelcomeEnglish generated'][0].rsplit()[-2].strip('('))
return_val['jpj reco'] = int(log_viewer.log_message['Speech Recognition: Compilation time'][0].rsplit()[-2])
if len(log_viewer.log_message['Speech Recognition: Compilation time']) > 1:
return_val['enu reco'] = int(log_viewer.log_message['Speech Recognition: Compilation time'][1].rsplit()[-2])
return return_val
def ending():
s = Session()
s.connect(ssh_tools.get_ip())
preference_manager = s.service("ALPreferenceManager")
preference_manager.setValue("com.aldebaran.debug", "DisableLifeAndDialog", 0)
def session(robot_ip, port):
""" Connect a session to a NAOqi """
ses = Session()
ses.connect("tcp://" + robot_ip + ":" + str(port))
return ses
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import cProfile
if __name__ == '__main__':
import cProfile
from qi import Session
s = Session()
s.connect('localhost')
dialog = s.service('ALDialog')
cProfile.run('dialog._preloadMain()')
class LogWatch(object):
is_logging = False
log_records = []
def __init__(self, ip, root_dir = '/tmp/log_watcher', message_start = '', message_stop = 'No way this exists in the logs', message_category = ''):
self.s = Session()
self.s.connect(ip)
self.log_manager = self.s.service("LogManager")
self.listener = self.log_manager.getListener()
self.root_dir = root_dir
self.signal_id = None
self.file_path = None
LogWatch.message_start = message_start
LogWatch.message_stop = message_stop
LogWatch.category_filter = message_category
self.cb = log_callback
def start_watch(self, file_name = 'log_watch', level = logging.DEBUG):
record_dir = os.path.join(self.root_dir, RECORD_DIRECTORY)
if not os.path.isdir(record_dir):
os.makedirs(record_dir)
file_path = os.path.join(record_dir, file_name)
LogWatch.file = open(file_path, 'w')
LogWatch.file.write(CSV_SEP.join([x for x in HEADERS]))
self.listener.clearFilters()
self.listener.setLevel(level)
self.signal_id = self.listener.onLogMessage.connect(self.cb)
self.file_path = file_path
def stop_watch(self):
if self.signal_id:
self.listener.onLogMessage.disconnect(self.signal_id)
self.signal_id = None
LogWatch.file.close()
return True
else:
return False
###
### Helpers
###
def get_log_subset(self, substring = '', begin = '', end = '', logs = False):
if not logs:
logs = open(self.file_path)
#logs = LogWatch.log_records
subset = []
appending = False
for log in logs:
if begin in log[1]:
appending = True
if substring in log[1] and appending:
subset.append(log)
if end in log[1]:
appending = False
return subset
#def dump_logs(self, file_name):
# record_dir = os.path.join(self.root_dir, RECORD_DIRECTORY)
# if not os.path.isdir(record_dir):
# os.makedirs(record_dir)
# file_path = os.path.join(record_dir, file_name)
# with open(file_path, 'w') as f:
# f.write(CSV_SEP.join([x for x in HEADERS]))
# for line in LogWatch.log_records:
# f.write(CSV_SEP.join([str(x) for x in line]))
def get_timestamp_range(self, substring, begin, end):
log = self.get_log_subset(substring = substring, begin = begin, end = end)
return [x[0] for x in log]
class LogWatch(object):
is_logging = False
log_records = []
def __init__(self,
ip,
root_dir='/tmp/log_watcher',
message_start='',
message_stop='No way this exists in the logs',
message_category=''):
self.s = Session()
self.s.connect(ip)
self.log_manager = self.s.service("LogManager")
self.listener = self.log_manager.getListener()
self.root_dir = root_dir
self.signal_id = None
self.file_path = None
LogWatch.message_start = message_start
LogWatch.message_stop = message_stop
LogWatch.category_filter = message_category
self.cb = log_callback
def start_watch(self, file_name='log_watch', level=logging.DEBUG):
record_dir = os.path.join(self.root_dir, RECORD_DIRECTORY)
if not os.path.isdir(record_dir):
os.makedirs(record_dir)
file_path = os.path.join(record_dir, file_name)
LogWatch.file = open(file_path, 'w')
LogWatch.file.write(CSV_SEP.join([x for x in HEADERS]))
self.listener.clearFilters()
self.listener.setLevel(level)
self.signal_id = self.listener.onLogMessage.connect(self.cb)
self.file_path = file_path
def stop_watch(self):
if self.signal_id:
self.listener.onLogMessage.disconnect(self.signal_id)
self.signal_id = None
LogWatch.file.close()
return True
else:
return False
###
### Helpers
###
def get_log_subset(self, substring='', begin='', end='', logs=False):
if not logs:
logs = open(self.file_path)
#logs = LogWatch.log_records
subset = []
appending = False
for log in logs:
if begin in log[1]:
appending = True
if substring in log[1] and appending:
subset.append(log)
if end in log[1]:
appending = False
return subset
#def dump_logs(self, file_name):
# record_dir = os.path.join(self.root_dir, RECORD_DIRECTORY)
# if not os.path.isdir(record_dir):
# os.makedirs(record_dir)
# file_path = os.path.join(record_dir, file_name)
# with open(file_path, 'w') as f:
# f.write(CSV_SEP.join([x for x in HEADERS]))
# for line in LogWatch.log_records:
# f.write(CSV_SEP.join([str(x) for x in line]))
def get_timestamp_range(self, substring, begin, end):
log = self.get_log_subset(substring=substring, begin=begin, end=end)
return [x[0] for x in log]
###
def dialog_preload_timestamp_message(mess):
if 'Dialog' in mess['category']:
if log_viewer.message_start in mess['message']:
log_viewer.is_logging = True
if log_viewer.is_logging:
log_viewer.log_records.append((mess['timestamp']['tv_sec'], mess['message'], mess['level']))
if log_viewer.message_stop in mess['message']:
log_viewer.is_logging = False
if __name__ == '__main__':
ip = '10.0.132.103'
x = log_viewer(ip = ip, message_start = 'Enable wav logging', message_stop = 'Preload finished')
s = Session()
s.connect(ip)
dialog = s.service('ALDialog')
dialog.deleteSerializationFiles()
dialog._resetPreload()
print 'Preloading ...'
dialog._preloadMain()
print 'Loading Time'
print x.get_load_time()
tme_range = x.get_timestamp_range('loading')
print tme_range
tme_range = x.get_timestamp_range('compile_bundle')
print 'Bundle compilation time'
print tme_range[-1] - tme_range[0]
print x.get_bundle_compile_time('welcome')