def translate(node, otype, value):
if otype == 'video_cap':
return cv2.VideoCapture(node.path() + value)
if otype == 'json_data':
return config.read(node.path() + value).get('data')
if otype == 'datetime':
return ext.unpack_datetime(value)
return None
def _default_translate(path, otype, value):
if otype == 'video_cap':
return cv2.VideoCapture(path + value)
if otype == 'video_numpy':
return readVideo(path + value)
if otype == 'json_data':
return config.read(path + value).get('data')
if otype == 'datetime':
return ext.unpack_datetime(value)
return None
def _prepare_config():
config_json = au_config.read(config_file)
if config_json == None:
print term.RED + "File '{}' not found".format(config_file) + term.END
recording_config = config_json['recording_config']
recording_path = config_json['recording_path']
config_path = config_json['config_path']
config_set = config_json['config_set']
for config in config_set:
config_set[config][1] = config_path + config_set[config][1]
return recording_config, recording_path, config_path, config_set
def __init__(self, name, translate, load, path):
self._name = name
if self._name[-1] != '/':
self._name += '/'
self._translate = translate
self._path = path
self._node_path = self._path + self._name
self._config_path = self._node_path + 'meta.json'
self._config = { }
if load:
self._config = config.read(self._config_path)
if self._config == None:
print "Error reading: '{}'".format(self._config_path)
else:
config.write(self._config_path, self._config)
def edit_config():
result = au_config.read(_recording_config)
if result == None:
print "File '{}' not found".format(_recording_config)
result = _create_config()
else:
print "File '{}' found".format(_recording_config)
edit_result = _edit_config(result)
if edit_result is not None:
result = edit_result
au_config.write(_recording_config,
result) #write again if missing parameters
print "Updated config at '{}'".format(_recording_config)
else:
print "Config not changed: '{}'".format(_recording_config)
return result
def read_or_create_config(output=True):
result = au_config.read(_recording_config)
if result == None:
print "File '{}' not found".format(_recording_config)
result = _create_config()
else:
print "File '{}' found".format(_recording_config)
check_result = _check_config(result, output)
if check_result is not None:
result = check_result
au_config.write(_recording_config,
result) #write again if missing parameters
print "Updated config at '{}'".format(_recording_config)
else:
print "Config not changed: '{}'".format(_recording_config)
return result
def _read_config_enter_attrib(name, set_name, json_file, default=None):
_config = au_config.read(json_file)
if _config == None:
print term.RED + "Error reading '{}'".format(json_file) + term.END
else:
print "Read '{}'".format(json_file)
config_set = [i for i in _config]
_output_value("Available {}: ".format(set_name),
ext.list_format(config_set))
if default == None:
default = config_set[0]
attrib = _default("Enter {}: ".format(name), default)
element = _config.get(attrib)
while element == None:
print term.RED + "{} not found in {}, try again.".format(
attrib, json_file) + term.END
attrib = raw_input(term.GREEN + "Enter {}: ".format(name) + term.END)
element = _config.get(attrib)
return attrib
def prop(key, dc_config, config_set):
return dc_config[key], config.read(config_set[key][1])[dc_config[key]]
def main():
pub = rospy.Publisher('al5d', Float32MultiArray, queue_size=10)
rospy.init_node('al5d_pub', anonymous=True)
rate = rospy.Rate(10)
pc = ps4.PS4Controller()
created = pc.create()
if not created:
print "Error finding PS4 controller"
read = False
write = False
fname = "temp_ald5.json"
base = "c"
data = {}
count = 0
num = 0
if read:
data = config.read(fname)
num = data['num']
mindex = 0
while not rospy.is_shutdown():
pc.poll()
if pc.home():
break
if read:
if count == num - 1:
break
C = data[base + str(count)]
else:
C = [
-pc.RX(), # X
pc.RY(), # Y
0, # Z
pc.LY(), # wrist_degree
pc.LT(), # wrise_rotate_degree
pc.RT(), # open
0 # reward
]
data[base + str(count)] = C
# Displacements
C = [
pc.RX(), # BASE
-pc.RY(), # SHOULDER
0, # ELBOW
0, # WRIST
0, # WRIST_ROTATE
0, # GRIPPER
]
# Trigger mod
lt = pc.LT() + 1
rt = pc.RT() + 1
# Test individuals
C = [
pc.RX(), # good
-pc.RY(), # ?
-pc.LY(), # ?
-lt + rt, # ?
pc.LX(), # ?
(-pc.L1() + pc.R1()), # ?
]
# Multipliers
C[0] = 100 * C[0] * C[0] * C[0]
C[1] = 100 * C[1] * C[1] * C[1]
C[2] = 100 * C[2] * C[2] * C[2]
C[3] = 100 * C[3] * C[3] * C[3]
C[4] = 100 * C[4] * C[4] * C[4]
C[5] = 100 * C[5] * C[5] * C[5]
rospy.loginfo(C)
msg = Float32MultiArray()
msg.data = C
pub.publish(msg)
count = count + 1
rate.sleep()
pc.destroy()
if write:
data['num'] = count
print "Writing to: {}".format(fname)
config.write(fname, data)
def read_config():
return au_config.read(_recording_config)