def execute(self, userdata):
rospy.loginfo("EL State ResetLEDPanels()")
self.timePrev = rospy.Time.now()
self.experimentparamsChoices = copy.deepcopy(userdata.experimentparamsChoicesIn)
#self.paramsIn =
rv = 'disabled'
if (userdata.experimentparamsChoicesIn.pre.ledpanels.enabled):
rv = 'success'
msgPanelsCommand = MsgPanelsCommand(command='stop')
self.pubLEDPanelsCommand.publish (msgPanelsCommand)
# Choose one of the choices.
experimentparams = ExperimentLib.Chooser().GetChoice(userdata.experimentparamsChoicesIn)
paramsIn = experimentparams.pre
# Publish the LED commands.
msgPanelsCommand = MsgPanelsCommand(command='set_pattern_id',
arg1=paramsIn.ledpanels.idPattern)
self.pubLEDPanelsCommand.publish (msgPanelsCommand)
msgPanelsCommand = MsgPanelsCommand(command='set_position',
arg1=paramsIn.ledpanels.origin.x,
arg2=paramsIn.ledpanels.origin.y) # Set (x,y) position for the experiment.
self.pubLEDPanelsCommand.publish (msgPanelsCommand)
else:
msgPanelsCommand = MsgPanelsCommand(command='all_off')
self.pubLEDPanelsCommand.publish (msgPanelsCommand)
return rv
def StartExperiment_callback(self, userdata):
# Set the LED pattern to the origin.
msgPanelsCommand = MsgPanelsCommand(command='stop')
self.pubLEDPanels.publish (msgPanelsCommand)
msgPanelsCommand = MsgPanelsCommand(command='set_pattern_id', arg1=1) # Assumes preprogrammed to the three-section pattern from Reiser paper.
self.pubLEDPanels.publish (msgPanelsCommand)
msgPanelsCommand = MsgPanelsCommand(command='set_position', arg1=0, arg2=0)
self.pubLEDPanels.publish (msgPanelsCommand)
return 'success'
def run(self):
rospy.spin()
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='all_off',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0))
def EndTrial_callback(self, userdata):
# Create a rotation matrix R for an angle of +90 or -90, chosen at random.
plusorminusone = (np.random.random()>=0.5) * 2 - 1 # -1 means +90 (CCW), +1 means -90 (CW).
R = np.array([[0,plusorminusone],[-plusorminusone,0]])
# Rotate the pattern on the LED panels.
self.xPanelPattern += plusorminusone * 48 # 48 = 1/4 of the pixel width of the 24 panels. 192 pixels all the way around. 24 panels * 8 pixels per panel = 192.
self.xPanelPattern %= 192
self.yPanelPattern = 0
msgPanelsCommand = MsgPanelsCommand(command='set_position', arg1=self.xPanelPattern, arg2=self.yPanelPattern)
self.pubLEDPanels.publish (msgPanelsCommand)
# Rotate the points in the statefilter strings by R. (Convert string to dict, rotate, then convert dict back to string).
statefilterHi_list = []
statefilterLo_list = []
for iFilter in range(len(userdata.experimentparamsIn.lasergalvos.statefilterLo_list)):
# Convert strings to dicts.
statefilterHi_dict = eval(userdata.experimentparamsIn.lasergalvos.statefilterHi_list[iFilter])
statefilterLo_dict = eval(userdata.experimentparamsIn.lasergalvos.statefilterLo_list[iFilter])
# Rotate
if 'pose' in statefilterLo_dict:
if 'position' in statefilterLo_dict['pose']:
x = statefilterHi_dict['pose']['position']['x']
y = statefilterHi_dict['pose']['position']['y']
pt = np.array([x,y])
[xRotA,yRotA] = np.dot(R,pt)
x = statefilterLo_dict['pose']['position']['x']
y = statefilterLo_dict['pose']['position']['y']
pt = np.array([x,y])
[xRotB,yRotB] = np.dot(R,pt)
# Rotated hi/lo are now not necessarily in the same order.
statefilterHi_dict['pose']['position']['x'] = max(xRotA,xRotB)
statefilterHi_dict['pose']['position']['y'] = max(yRotA,yRotB)
statefilterLo_dict['pose']['position']['x'] = min(xRotA,xRotB)
statefilterLo_dict['pose']['position']['y'] = min(yRotA,yRotB)
statefilterLo_list.append(str(statefilterLo_dict))
statefilterHi_list.append(str(statefilterHi_dict))
# Save the results into the output.
experimentparamsOut = userdata.experimentparamsIn
experimentparamsOut.lasergalvos.statefilterLo_list = statefilterLo_list
experimentparamsOut.lasergalvos.statefilterHi_list = statefilterHi_list
userdata.experimentparamsOut = experimentparamsOut
return 'success'
def create_msgpanels_vel(self, flystate):
leftMajor = flystate.left.angles[0] if (
0 < len(flystate.left.angles)) else 0.0
leftMinor = flystate.left.angles[1] if (
1 < len(flystate.left.angles)) else 0.0
rightMajor = flystate.right.angles[0] if (
0 < len(flystate.right.angles)) else 0.0
rightMinor = flystate.right.angles[1] if (
1 < len(flystate.right.angles)) else 0.0
angleHead = flystate.head.angles[0] if (
0 < len(flystate.head.angles)) else 0.0
radiusHead = flystate.head.radii[0] if (
0 < len(flystate.head.radii)) else 0.0
angleAbdomen = flystate.abdomen.angles[0] if (
0 < len(flystate.abdomen.angles)) else 0.0
radiusAbdomen = flystate.abdomen.radii[0] if (
0 < len(flystate.abdomen.radii)) else 0.0
state = np.array([
1.0, leftMajor, leftMinor, rightMajor, rightMinor, angleHead,
radiusHead, angleAbdomen, radiusAbdomen, flystate.aux.intensity
],
dtype=np.float32)
vel = np.dot(self.a, state)
# gain, bias are in frames per sec, times ten, i.e. 10=1.0, 127=12.7
gain_x = (int(vel[0]) + 128) % 256 - 128
bias_x = 0
gain_y = (int(vel[1]) +
128) % 256 - 128 # As if y were on a sphere, not a cylinder.
bias_y = 0
msgVel = MsgPanelsCommand(command='send_gain_bias',
arg1=gain_x,
arg2=bias_x,
arg3=gain_y,
arg4=bias_y,
arg5=0,
arg6=0)
return msgVel
def create_msgpanels_pos(self, flystate):
leftMajor = flystate.left.angles[0] if (
0 < len(flystate.left.angles)) else 0.0
leftMinor = flystate.left.angles[1] if (
1 < len(flystate.left.angles)) else 0.0
rightMajor = flystate.right.angles[0] if (
0 < len(flystate.right.angles)) else 0.0
rightMinor = flystate.right.angles[1] if (
1 < len(flystate.right.angles)) else 0.0
angleHead = flystate.head.angles[0] if (
0 < len(flystate.head.angles)) else 0.0
radiusHead = flystate.head.radii[0] if (
0 < len(flystate.head.radii)) else 0.0
angleAbdomen = flystate.abdomen.angles[0] if (
0 < len(flystate.abdomen.angles)) else 0.0
radiusAbdomen = flystate.abdomen.radii[0] if (
0 < len(flystate.abdomen.radii)) else 0.0
state = np.array([
1.0, leftMajor, leftMinor, rightMajor, rightMinor, angleHead,
radiusHead, angleAbdomen, radiusAbdomen, flystate.aux.intensity
],
dtype=np.float32)
pos = np.dot(self.a, state)
# index is in frames.
index_x = int(pos[0])
index_y = int(pos[1])
msgPos = MsgPanelsCommand(command='set_position',
arg1=index_x,
arg2=index_y,
arg3=0,
arg4=0,
arg5=0,
arg6=0)
return msgPos
def command_callback(self, command):
self.command = command.data
if (self.command == 'exit'):
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='stop',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0))
self.bRunning = False
rospy.signal_shutdown('User requested exit.')
if (self.command == 'stop'):
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))
self.bRunning = False
if (self.command == 'start'):
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='start',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0))
self.bRunning = True
if (self.command == 'help'):
rospy.logwarn(
'The %s/command topic accepts the following string commands:' %
self.nodename.rstrip('/'))
rospy.logwarn(' help This message.')
rospy.logwarn(' stop Stop the ledpanels.')
rospy.logwarn(' start Start the ledpanels.')
rospy.logwarn(' exit Exit the program.')
rospy.logwarn('')
rospy.logwarn(
'You can send the above commands at the shell prompt via:')
rospy.logwarn(
'rostopic pub -1 %s/command std_msgs/String commandtext' %
self.nodename.rstrip('/'))
rospy.logwarn('')
rospy.logwarn('Parameters are settable as launch-time parameters.')
rospy.logwarn('')
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 Main(self):
panelcommand = MsgPanelsCommand(command='all_off')
self.PanelsCommand_callback(panelcommand)
rospy.spin()
def execute(self, userdata):
self.experimentparams = copy.deepcopy(userdata.experimentparamsIn)
self.timePrev = rospy.Time.now()
if self.mode == 'pre':
self.paramsIn = self.experimentparams.pre
if self.mode == 'trial':
self.paramsIn = self.experimentparams.trial
# Create the panels command.
command = MsgPanelsCommand(command='all_off', arg1=0, arg2=0, arg3=0, arg4=0)
if self.paramsIn.ledpanels.enabled:
self.timeStart = rospy.Time.now()
rv = 'aborted'
xmax = self.xpanels * 8 # pixels per panel.
# Determine if we're operating only for certain states.
if len(self.paramsIn.ledpanels.statefilterHi) > 0:
isStatefiltered = True
else:
isStatefiltered = False
# Initialize statefilter vars.
bInStatefilterRangePrev = True
bInStatefilterRange = True
# Set the panels pattern.
command.command = 'set_pattern_id'
command.arg1 = self.paramsIn.ledpanels.idPattern
self.pubLEDPanelsCommand.publish(command)
# Loop sending commands to the panels, until preempt or timeout.
while not rospy.is_shutdown():
self.step_time_elapsed()
# If possible, take the pose &/or velocity &/or speed from arenastate, else use transform via ROS.
pose = None
velocity = None
speed = None
if self.arenastate is not None:
if ('Robot' in self.paramsIn.ledpanels.frame_id):
#pose = self.arenastate.robot.pose # For consistency w/ galvodirector, we'll get pose via transform.
velocity = self.arenastate.robot.velocity
speed = self.arenastate.robot.speed
elif ('Fly' in self.paramsIn.ledpanels.frame_id):
for iFly in range(len(self.arenastate.flies)):
if (self.arenastate.flies[iFly].name in self.paramsIn.ledpanels.frame_id):
#pose = self.arenastate.flies[iFly].pose # For consistency w/ galvodirector, we'll get pose via transform.
velocity = self.arenastate.flies[iFly].velocity
speed = self.arenastate.flies[iFly].speed
break
# Get the timestamp for transforms.
stamp=None
if (pose is None) or (velocity is None):
try:
stamp = self.tfrx.getLatestCommonTime('Arena', self.paramsIn.ledpanels.frame_id)
except tf.Exception:
pass
# If we still need the pose (i.e. the frame wasn't in arenastate), then get it from ROS.
if (pose is None) and (stamp is not None) and self.tfrx.canTransform('Arena', self.paramsIn.ledpanels.frame_id, stamp):
try:
poseStamped = self.tfrx.transformPose('Arena', PoseStamped(header=Header(stamp=stamp,
frame_id=self.paramsIn.ledpanels.frame_id),
pose=Pose(position=Point(0,0,0),
orientation=Quaternion(0,0,0,1)
)
)
)
pose = poseStamped.pose
except tf.Exception:
pose = None
# If we still need the velocity, then get it from ROS.
if (velocity is None) and (stamp is not None) and self.tfrx.canTransform('Arena', self.paramsIn.ledpanels.frame_id, stamp):
try:
velocity_tuple = self.tfrx.lookupTwist('Arena', self.paramsIn.ledpanels.frame_id, stamp, self.dtVelocity)
except tf.Exception:
velocity = None
else:
velocity = Twist(linear=Point(x=velocity_tuple[0][0],
y=velocity_tuple[0][1],
z=velocity_tuple[0][2]),
angular=Point(x=velocity_tuple[1][0],
y=velocity_tuple[1][1],
z=velocity_tuple[1][2]))
# If we still need the speed, then get it from velocity.
if (speed is None) and (velocity is not None):
speed = np.linalg.norm([velocity.linear.x, velocity.linear.y, velocity.linear.z])
# See if the state is in range of the filter.
if isStatefiltered:
# Check if any filterstates have changed.
bFilterStateChanged = False
# Convert filter string to dict.
statefilterLo_dict = eval(self.paramsIn.ledpanels.statefilterLo)
statefilterHi_dict = eval(self.paramsIn.ledpanels.statefilterHi)
statefilterCriteria = self.paramsIn.ledpanels.statefilterCriteria
if (pose is not None) and (velocity is not None) and (speed is not None):
state = MsgFrameState(pose = pose,
velocity = velocity,
speed = speed)
bInStatefilterRangePrev = bInStatefilterRange
bInStatefilterRange = self.Statefilter.InRange(state, statefilterLo_dict, statefilterHi_dict, statefilterCriteria)
self.Statefilter.PublishMarkers (state, statefilterLo_dict, statefilterHi_dict, statefilterCriteria)
# Check if the filter state has changed.
if bInStatefilterRangePrev != bInStatefilterRange:
bFilterStateChanged = True
# end if isStatefiltered
# If in range of the statefilter, then publish the new command
if bInStatefilterRange and (pose is not None) and (velocity is not None) and (speed is not None):
bValidCommand = False
if self.paramsIn.ledpanels.command == 'fixed':
command.command = 'set_position'
command.arg1 = self.paramsIn.ledpanels.origin.x
command.arg2 = self.paramsIn.ledpanels.origin.y
bValidCommand = True
elif self.paramsIn.ledpanels.command == 'trackposition':
angle = (2.0*np.pi) - np.arctan2(pose.position.y, pose.position.x) % (2.0*np.pi)
if np.isfinite(angle):
x = xmax * angle / (2.0*np.pi)
y = 0
command.command = 'set_position'
command.arg1 = int(x)
command.arg2 = int(y)
bValidCommand = True
elif self.paramsIn.ledpanels.command == 'trackview':
r = rospy.get_param('ledpanels/radius', 120) # Radius of the panels.
xp = pose.position.x
yp = pose.position.y
q = pose.orientation
rpy = tf.transformations.euler_from_quaternion((q.x, q.y, q.z, q.w))
theta = -rpy[2] % (2.0 * np.pi)
tantheta = np.tan(theta)
# Points on the circle intersecting with the fly's axis.
x1=(1+tantheta**2)**(-1)*((-1)*yp*tantheta+xp*tantheta**2 + (-1)*(r**2+(-1)*yp**2+2*xp*yp*tantheta+r**2*tantheta**2+(-1)*xp**2*tantheta**2)**(1/2))
y1=yp+(-1)*xp*tantheta+tantheta*(1+tantheta**2)**(-1)*((-1)*yp*tantheta+xp*tantheta**2+(-1)*(r**2+(-1)*yp**2+2*xp*yp*tantheta+r**2*tantheta**2+(-1)*xp**2*tantheta**2)**(1/2));
x2=(1+tantheta**2)**(-1)*((-1)*yp*tantheta+xp*tantheta**2 + (r**2+(-1)*yp**2+2*xp*yp*tantheta+r**2*tantheta**2+(-1)*xp**2*tantheta**2)**(1/2))
y2=yp+(-1)*xp*tantheta+tantheta*(1+tantheta**2)**(-1)*((-1)*yp*tantheta+xp*tantheta**2+(r**2+(-1)*yp**2+2*xp*yp*tantheta+r**2*tantheta**2+(-1)*xp**2*tantheta**2)**(1/2))
# Choose between the two intersection points by moving forward 1mm, and seeing which point we got closer to.
r1 = np.linalg.norm([xp-x1, yp-y1]) # Fly to pt1 distance
r2 = np.linalg.norm([xp-x2, yp-y2]) # Fly to pt2 distance
xa = xp + 1 * np.cos(theta) # Go one millimeter forward.
ya = yp + 1 * np.sin(theta)
r1a= np.linalg.norm([xa-x1, ya-y1]) # Fly+1mm to pt1 distance
# If we got closer to pt1 at xa, then use pt1, else use pt2
if r1a < r1:
angle = np.arctan2(y1,x1) % (2.0*np.pi)
else:
angle = np.arctan2(y2,x2) % (2.0*np.pi)
if np.isfinite(angle):
x = xmax * angle / (2.0*np.pi)
y = 0
command.command = 'set_position'
command.arg1 = int(x)
command.arg2 = int(y)
bValidCommand = True
#else:
#rospy.logwarn('isnan: (%f,%f), (%f,%f)' % (x1,y1,x2,y2))
# end if command == 'trackposition' or 'trackview'
if (bValidCommand):
self.pubLEDPanelsCommand.publish(command)
if self.preempt_requested():
rospy.loginfo('preempt requested: LEDPanels()')
self.service_preempt()
rv = 'preempt'
break
self.rosrate.sleep()
# Handle commands.
if (self.commandExperiment=='continue'):
pass
if (self.commandExperiment=='pause_now'):
while (self.commandExperiment=='pause_now'):
rospy.sleep(0.5)
self.timePrev = rospy.Time.now()
if (self.commandExperiment=='pause_after_trial'):
pass
if (self.commandExperiment=='exit_after_trial'):
pass
if (self.commandExperiment=='exit_now'):
rv = 'aborted'
break
# End while not rospy.is_shutdown()
else:
rv = 'disabled'
command.command = 'all_off'
self.pubLEDPanelsCommand.publish(command)
# end if self.paramsIn.ledpanels.enabled
userdata.experimentparamsOut = self.experimentparams
return rv
# End class Action()
def __init__(self):
self.pub = rospy.Publisher('/ledpanels/command',
MsgPanelsCommand,
queue_size=10)
self.msg = MsgPanelsCommand()