def replace_cvt(fn, args, ret):
from_, to = next(cvt_re.finditer(fn)).groups()
arg_map = int_maps.get(from_, SetDict())
ret_map = int_maps.get(to, SetDict())
sub_many = lambda string: lambda acc, it: re.sub(f"{it[0]}$", it[1], acc or
string)
master_map = arg_map | double_map | float_map
return ([[arg[0], reduce(sub_many(arg[1]), master_map.items(), "")]
for arg in args],
reduce(sub_many(ret), (ret_map | double_map | float_map).items(),
""))
def flystate_callback(self, flystate):
if (self.bRunning):
self.params = rospy.get_param('%s' % self.nodename.rstrip('/'), {})
SetDict().set_dict_with_preserve(self.params, self.defaults)
self.update_coefficients_from_params()
if (self.params['method'] == 'usb'):
if (self.params['mode'] == 'velocity'):
msgVel = self.create_msgpanels_vel(flystate)
self.pubPanelsCommand.publish(msgVel)
#rospy.logwarn('vel: %s' % msgVel)
else:
msgPos = self.create_msgpanels_pos(flystate)
self.pubPanelsCommand.publish(msgPos)
#rospy.logwarn('pos: %s' % msgPos)
elif (self.params['method'] == 'voltage'):
pass
else:
rospy.logwarn('%s: method must be '
'usb'
' or '
'voltage'
'' % self.name)
def __init__(self):
self.bInitialized = False
# initialize
self.name = 'acquirevoltages2msg'
rospy.init_node(self.name, anonymous=False)
self.nodename = rospy.get_name()
self.namespace = rospy.get_namespace()
# Load the parameters.
self.params = rospy.get_param('%s' % self.nodename.rstrip('/'), {})
self.defaults = {'channels_ai':[0,1,2,3,4,5,6,7],
'channels_di':[0,1,2,3,4,5,6,7]}
SetDict().set_dict_with_preserve(self.params, self.defaults)
rospy.set_param('%s' % self.nodename.rstrip('/'), self.params)
self.command = None
# Messages & Services.
self.topicAI = '%s/ai' % self.namespace.rstrip('/')
self.pubAI = rospy.Publisher(self.topicAI, MsgAnalogIn)
self.topicDI = '%s/di' % self.namespace.rstrip('/')
self.pubDI = rospy.Publisher(self.topicDI, MsgDigitalIn)
self.subCommand = rospy.Subscriber('%s/command' % self.nodename.rstrip('/'), String, self.command_callback, queue_size=1000)
self.get_ai = rospy.ServiceProxy('get_ai', SrvPhidgetsInterfaceKitGetAI)
self.get_di = rospy.ServiceProxy('get_di', SrvPhidgetsInterfaceKitGetDI)
rospy.sleep(1) # Allow time to connect.
self.bInitialized = True
def replace_abs(fn, args, ret):
ret_maps = {
"epi64": SetDict({
"__m128i": "u64x2",
"__m256i": "u64x4"
}),
"epu64": SetDict({
"__m128i": "u64x2",
"__m256i": "u64x4"
}),
"epi32": SetDict({
"__m128i": "u32x4",
"__m256i": "u32x8"
}),
"epu32": SetDict({
"__m128i": "u32x4",
"__m256i": "u32x8"
}),
"epi16": SetDict({
"__m128i": "u16x8",
"__m256i": "u16x16"
}),
"epu16": SetDict({
"__m128i": "u16x8",
"__m256i": "u16x16"
}),
"epi8": SetDict({
"__m128i": "u8x16",
"__m256i": "u8x32"
}),
"epu8": SetDict({
"__m128i": "u8x16",
"__m256i": "u8x32"
})
}
suffix = next(type_re.finditer(fn)).group()
int_map = int_maps.get(suffix, SetDict())
ret_map = ret_maps.get(suffix, SetDict())
sub_many = lambda string: lambda acc, it: re.sub(f"{it[0]}$", it[1], acc or
string)
master_map = int_map | double_map | float_map
return ([[arg[0], reduce(sub_many(arg[1]), master_map.items(), "")]
for arg in args],
reduce(sub_many(ret), (ret_map | double_map | float_map).items(),
""))
def replace_default(fn, args, ret):
int_map = int_maps.get(next(int_re.finditer(fn)).group(),
SetDict()) if int_re.search(fn) else {}
sub_many = lambda string: lambda acc, it: re.sub(f"{it[0]}$", it[1], acc or
string)
master_map = int_map | double_map | float_map
return ([[arg[0], reduce(sub_many(arg[1]), master_map.items(), "")]
for arg in args], reduce(sub_many(ret), master_map.items(), ""))
def __init__(self):
self.bInitialized = False
self.bAttached = False
self.lock = threading.Lock()
# initialize
self.name = 'Msg2PhidgetsAnalog'
rospy.init_node(self.name, anonymous=True)
self.nodename = rospy.get_name()
self.namespace = rospy.get_namespace()
# Load the parameters.
self.params = rospy.get_param('%s' % self.nodename.rstrip('/'), {})
self.defaults = {'v0enable':True, 'v1enable':True, 'v2enable':True, 'v3enable':True,
'serial':0, # The serial number of the Phidget. 0==any.
'topic':'ai',
'scale':1.0
}
SetDict().set_dict_with_preserve(self.params, self.defaults)
rospy.set_param('%s' % self.nodename.rstrip('/'), self.params)
# Enable the voltage output channels.
self.enable = [self.params['v0enable'], self.params['v1enable'], self.params['v2enable'], self.params['v3enable']]
# WBD
# ----------------------------------------------------------------------
# Connect to the Phidget.
#self.analog = Phidgets.Devices.Analog.Analog()
#if (self.params['serial']==0):
# self.analog.openPhidget()
#else:
# self.analog.openPhidget(self.params['serial'])
#
#self.analog.setOnAttachHandler(self.attach_callback)
#self.analog.setOnDetachHandler(self.detach_callback)
# -----------------------------------------------------------------------
with self.lock:
self.aout_chan_list = []
for i in range(4):
aout_chan = Phidget22.Devices.VoltageOutput.VoltageOutput()
aout_chan.setOnAttaself.analogHandler(self.attach_callback)
aout_chan.setOnDetaself.analogHandler(self.detach_callback))
aout_chan.setChannel(i)
aout_chan.openWaitForAttachment(5000)
self.aout_chan_list.append(aout_chan)
# -----------------------------------------------------------------------
# Subscriptions.
self.subAI = rospy.Subscriber(self.params['topic'], MsgAnalogIn, self.ai_callback)
self.subCommand = rospy.Subscriber('%s/command' % self.nodename.rstrip('/'), String, self.command_callback, queue_size=1000)
#rospy.sleep(1) # Allow time to connect publishers & subscribers.
self.bInitialized = True
def __init__(self):
self.bInitialized = False
self.bAttached = False
# initialize
self.name = 'Msg2PhidgetsAnalog'
rospy.init_node(self.name, anonymous=True)
self.nodename = rospy.get_name()
self.namespace = rospy.get_namespace()
# Load the parameters.
self.params = rospy.get_param('%s' % self.nodename.rstrip('/'), {})
self.defaults = {
'v0enable': True,
'v1enable': True,
'v2enable': True,
'v3enable': True,
'serial': 0, # The serial number of the Phidget. 0==any.
'topic': 'ai',
'scale': 1.0
}
SetDict().set_dict_with_preserve(self.params, self.defaults)
rospy.set_param('%s' % self.nodename.rstrip('/'), self.params)
# Enable the voltage output channels.
self.enable = [
self.params['v0enable'], self.params['v1enable'],
self.params['v2enable'], self.params['v3enable']
]
# Connect to the Phidget.
self.analog = Phidgets.Devices.Analog.Analog()
if (self.params['serial'] == 0):
self.analog.openPhidget()
else:
self.analog.openPhidget(self.params['serial'])
self.analog.setOnAttachHandler(self.attach_callback)
self.analog.setOnDetachHandler(self.detach_callback)
# Subscriptions.
self.subAI = rospy.Subscriber(self.params['topic'], MsgAnalogIn,
self.ai_callback)
self.subCommand = rospy.Subscriber('%s/command' %
self.nodename.rstrip('/'),
String,
self.command_callback,
queue_size=1000)
#rospy.sleep(1) # Allow time to connect publishers & subscribers.
self.bInitialized = True
def __init__(self):
self.bInitialized = False
self.iCount = 0
self.bAttached = False
# initialize
self.name = 'Flystate2PhidgetsAnalog'
rospy.init_node(self.name, anonymous=True)
self.nodename = rospy.get_name()
self.namespace = rospy.get_namespace()
# Load the parameters.
self.params = rospy.get_param('%s' % self.nodename.rstrip('/'), {})
self.defaults = {'v0enable':True, 'v1enable':True, 'v2enable':True, 'v3enable':True,
'v00': 0.0, 'v0l1':1.0, 'v0l2':0.0, 'v0lr':0.0, 'v0r1':0.0, 'v0r2':0.0, 'v0rr':0.0, 'v0ha':0.0, 'v0hr':0.0, 'v0aa':0.0, 'v0ar':0.0, 'v0xi':0.0, # L
'v10': 0.0, 'v1l1':0.0, 'v1l2':0.0, 'v1lr':0.0, 'v1r1':1.0, 'v1r2':0.0, 'v1rr':0.0, 'v1ha':0.0, 'v1hr':0.0, 'v1aa':0.0, 'v1ar':0.0, 'v1xi':0.0, # R
'v20': 0.0, 'v2l1':1.0, 'v2l2':0.0, 'v2lr':0.0, 'v2r1':-1.0, 'v2r2':0.0, 'v2rr':0.0, 'v2ha':0.0, 'v2hr':0.0, 'v2aa':0.0, 'v2ar':0.0, 'v2xi':0.0, # L-R
'v30': 0.0, 'v3l1':1.0, 'v3l2':0.0, 'v3lr':0.0, 'v3r1':1.0, 'v3r2':0.0, 'v3rr':0.0, 'v3ha':0.0, 'v3hr':0.0, 'v3aa':0.0, 'v3ar':0.0, 'v3xi':0.0, # L+R
'autorange':False,
'serial':0 # The serial number of the Phidget. 0==any.
}
SetDict().set_dict_with_preserve(self.params, self.defaults)
rospy.set_param('%s' % self.nodename.rstrip('/'), self.params)
self.stateMin = np.array([ np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf, np.inf])
self.stateMax = np.array([-np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf, -np.inf])
self.update_coefficients()
# Connect to the Phidget.
self.analog = Phidgets.Devices.Analog.Analog()
if (self.params['serial']==0):
self.analog.openPhidget()
else:
self.analog.openPhidget(self.params['serial'])
self.analog.setOnAttachHandler(self.attach_callback)
self.analog.setOnDetachHandler(self.detach_callback)
# Subscriptions.
self.subFlystate = rospy.Subscriber('%s/flystate' % self.namespace.rstrip('/'), MsgFlystate, self.flystate_callback, queue_size=1000)
self.subCommand = rospy.Subscriber('%s/command' % self.nodename.rstrip('/'), String, self.command_callback, queue_size=1000)
#rospy.sleep(1) # Allow time to connect publishers & subscribers.
self.bInitialized = True
def __init__(self):
self.bInitialized = False
self.iCount = 0
self.bAttached = False
self.bWarnedAboutNotAttached = False
# initialize
self.name = 'phidgets_interfacekit'
rospy.init_node(self.name, anonymous=False)
self.nodename = rospy.get_name()
self.namespace = rospy.get_namespace()
# Load the parameters.
self.params = rospy.get_param('%s' % self.nodename.rstrip('/'), {})
self.defaults = {'serial': 0}
SetDict().set_dict_with_preserve(self.params, self.defaults)
rospy.set_param('%s' % self.nodename.rstrip('/'), self.params)
# Connect to the Phidget.
self.interfacekit = Phidgets.Devices.InterfaceKit.InterfaceKit()
if (self.params['serial'] == 0):
self.interfacekit.openPhidget()
else:
self.interfacekit.openPhidget(self.params['serial'])
self.interfacekit.setOnAttachHandler(self.attach_callback)
self.interfacekit.setOnDetachHandler(self.detach_callback)
# Messages & Services.
self.subCommand = rospy.Subscriber(
'%s/command' % self.nodename.rstrip('/'), String,
self.command_callback)
self.service = rospy.Service('%s/get_ai' % self.namespace.rstrip('/'),
SrvPhidgetsInterfaceKitGetAI,
self.get_ai_callback)
self.service = rospy.Service('%s/get_di' % self.namespace.rstrip('/'),
SrvPhidgetsInterfaceKitGetDI,
self.get_di_callback)
self.service = rospy.Service('%s/set_do' % self.namespace.rstrip('/'),
SrvPhidgetsInterfaceKitSetDO,
self.set_do_callback)
rospy.sleep(1) # Allow time to connect publishers & subscribers.
self.bInitialized = True
def __init__(self):
self.bInitialized = False
self.bRunning = False
# initialize
self.name = 'Flystate2ledpanels'
rospy.init_node(self.name, anonymous=True)
self.nodename = rospy.get_name()
self.namespace = rospy.get_namespace()
# Load the parameters.
self.params = rospy.get_param('%s' % self.nodename.rstrip('/'), {})
self.defaults = {'method': 'voltage', # 'voltage' or 'usb'; How we communicate with the panel controller.
'pattern_id': 1,
'mode': 'velocity', # 'velocity' or 'position'; Fly is controlling vel or pos.
'axis': 'x', # 'x' or 'y'; The axis on which the frames move.
'coeff_voltage':{
'adc0':1, # When using voltage method, coefficients adc0-3 and funcx,y determine how the panels controller interprets its input voltage(s).
'adc1':0, # e.g. xvel = adc0*bnc0 + adc1*bnc1 + adc2*bnc2 + adc3*bnc3 + funcx*f(x) + funcy*f(y); valid on [-128,+127], and 10 corresponds to 1.0.
'adc2':0,
'adc3':0,
'funcx':0,
'funcy':0,
},
'coeff_usb':{ # When using usb method, coefficients x0,xl1,xl2, ... ,yaa,yar,yxi determine the pos or vel command sent to the controller over USB.
'x0': 0.0,
'xl1':1.0,
'xl2':0.0,
'xr1':-1.0,
'xr2':0.0,
'xha':0.0,
'xhr':0.0,
'xaa':0.0,
'xar':0.0,
'xxi':0.0,
'y0': 0.0,
'yl1':0.0,
'yl2':0.0,
'yr1':0.0,
'yr2':0.0,
'yha':0.0,
'yhr':0.0,
'yaa':0.0,
'yar':0.0,
'yxi':0.0,
}
}
SetDict().set_dict_with_preserve(self.params, self.defaults)
self.update_coefficients_from_params()
rospy.set_param('%s' % self.nodename.rstrip('/'), self.params)
self.msgpanels = MsgPanelsCommand(command='all_off',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0)
# Publishers.
self.pubPanelsCommand = rospy.Publisher('/ledpanels/command',
MsgPanelsCommand)
# Subscriptions.
self.subFlystate = rospy.Subscriber('%s/flystate' %
self.namespace.rstrip('/'),
MsgFlystate,
self.flystate_callback,
queue_size=1000)
self.subCommand = rospy.Subscriber('%s/command' %
self.nodename.rstrip('/'),
String,
self.command_callback,
queue_size=1000)
rospy.sleep(1) # Time to connect publishers & subscribers.
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='set_posfunc_id',
arg1=1,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0)) # Set default function ch1.
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='set_posfunc_id',
arg1=2,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0)) # Set default function ch2.
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='set_pattern_id',
arg1=self.params['pattern_id'],
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0))
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='set_mode',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0)) # xvel=funcx, yvel=funcy
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='set_position',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0)) # Set position to 0
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='send_gain_bias',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0)) # Set vel to 0
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='stop',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0))
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='all_off',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0))
if (self.params['method'] == 'voltage'):
# Assemble a command: set_mode_(pos|vel)_custom_(x|y)
cmd = 'set_mode'
if (self.params['mode'] == 'velocity'):
cmd += '_vel'
elif (self.params['mode'] == 'position'):
cmd += '_pos'
else:
rospy.logwarn('%s: mode must be '
'velocity'
' or '
'position'
'.' % self.name)
if (self.params['axis'] == 'x'):
cmd += '_custom_x'
elif (self.params['axis'] == 'y'):
cmd += '_custom_y'
else:
rospy.logwarn('%s: axis must be '
'x'
' or '
'y'
'.' % self.name)
# Set the panels controller to the custom mode, with the specified coefficients.
self.pubPanelsCommand.publish(
MsgPanelsCommand(command=cmd,
arg1=self.params['coeff_voltage']['adc0'],
arg2=self.params['coeff_voltage']['adc1'],
arg3=self.params['coeff_voltage']['adc2'],
arg4=self.params['coeff_voltage']['adc3'],
arg5=self.params['coeff_voltage']['funcx'],
arg6=self.params['coeff_voltage']['funcy']))
self.bInitialized = True
def reconfigure_callback(self, config, level):
rospy.logwarn("Reconfigure Request: Phidget Analog")
rospy.logwarn(
""" v0L1:{v0l1} , v0R1:{v0r1} , v0H:{v0ha} """.format(**config))
rospy.logwarn(
""" v1L1:{v1l1} , v1R1:{v1r1} , v1H:{v1ha} """.format(**config))
rospy.logwarn(
""" v2L1:{v2l1} , v2R1:{v2r1} , v2H:{v2ha} """.format(**config))
rospy.logwarn(
""" v3L1:{v3l1} , v3R1:{v3r1} , v3H:{v3ha} """.format(**config))
# Save the new params & update coefficents
SetDict().set_dict_with_overwrite(self.params, config)
self.update_coefficients()
# Publish paramters to topic
vstate = MsgAOPhidgetState()
vstate.header = Header(seq=self.vCount,
stamp=rospy.Time.now(),
frame_id='Fly')
vstate.v0 = MsgAOPhidget()
vstate.v1 = MsgAOPhidget()
vstate.v2 = MsgAOPhidget()
vstate.v3 = MsgAOPhidget()
vstate.v0.L = [
self.params['v0l1'], self.params['v0l2'], self.params['v0lr']
]
vstate.v0.R = [
self.params['v0r1'], self.params['v0r2'], self.params['v0rr']
]
vstate.v0.H = [self.params['v0ha'], self.params['v0hr']]
vstate.v0.A = [self.params['v0aa'], self.params['v0ar']]
vstate.v1.L = [
self.params['v1l1'], self.params['v1l2'], self.params['v1lr']
]
vstate.v1.R = [
self.params['v1r1'], self.params['v1r2'], self.params['v1rr']
]
vstate.v1.H = [self.params['v1ha'], self.params['v1hr']]
vstate.v1.A = [self.params['v1aa'], self.params['v1ar']]
vstate.v2.L = [
self.params['v2l1'], self.params['v2l2'], self.params['v2lr']
]
vstate.v2.R = [
self.params['v2r1'], self.params['v2r2'], self.params['v2rr']
]
vstate.v2.H = [self.params['v2ha'], self.params['v2hr']]
vstate.v2.A = [self.params['v2aa'], self.params['v2ar']]
vstate.v3.L = [
self.params['v3l1'], self.params['v3l2'], self.params['v3lr']
]
vstate.v3.R = [
self.params['v3r1'], self.params['v3r2'], self.params['v3rr']
]
vstate.v3.H = [self.params['v3ha'], self.params['v3hr']]
vstate.v3.A = [self.params['v3aa'], self.params['v3ar']]
self.pubVC.publish(vstate)
self.vCount += 1
return config
fn_re = re.compile(
"" + r"pub (?:unsafe)? fn ([a-zA-z0-9_]*)\s*" # decl + name
+ r"\s*\(([^)]*?)\)\s*" # args
+ r"\s*->\s*([a-zA-z0-9_]*)\s*{", # return type
flags=re.MULTILINE)
type_re = re.compile("(?:[sp][sd]|e?[sp][iu][0-9]{1,2})")
int_re = re.compile("e?[sp][iu][0-9]{1,2}")
cvt_re = re.compile(
"_mm(?:256)?_cvt(e?p[sdiu][0-9]{1,2})_(e?p[sdiu][0-9]{1,2})")
pack_re = re.compile("_mm(?:256)?_packu?s_(e?p[sdiu][0-9]{1,2})")
abs_re = re.compile("_mm(?:256)?_abs_(e?p[sdiu][0-9]{1,2})")
int_maps = {
"epi64": SetDict({
"__m128i": "i64x2",
"__m256i": "i64x4"
}),
"epu64": SetDict({
"__m128i": "u64x2",
"__m256i": "u64x4"
}),
"epi32": SetDict({
"__m128i": "i32x4",
"__m256i": "i32x8"
}),
"epu32": SetDict({
"__m128i": "u32x4",
"__m256i": "u32x8"
}),
"epi16": SetDict({
"__m128i": "i16x8",
