def execute(self, userdata):
rospy.loginfo("EL State ResetGalvos()")
if (userdata.experimentparamsChoicesIn.trial.lasergalvos.enabled):
commandGalvoBeamsink = MsgGalvoCommand()
commandGalvoBeamsink.enable_laser = False # False will send it to the beamsink.
self.pubGalvoCommand.publish(commandGalvoBeamsink)
rv = 'success'
else:
rv = 'disabled'
#rospy.loginfo ('EL Exiting ResetHardware()')
return rv
def execute(self, userdata):
rospy.loginfo("EL State ResetGalvos()")
if (userdata.experimentparamsChoicesIn.trial.lasergalvos.enabled):
commandGalvoBeamsink = MsgGalvoCommand()
commandGalvoBeamsink.enable_laser = False # False will send it to the beamsink.
self.pubGalvoCommand.publish(commandGalvoBeamsink)
rv = 'success'
else:
rv = 'disabled'
#rospy.loginfo ('EL Exiting ResetHardware()')
return rv
def SendInputPoints(self):
pattern = MsgPattern()
pattern.mode = 'bypoints'
pattern.shape = 'constant'
pattern.frame_id = 'Plate'
pattern.hzPattern = 1.0
pattern.hzPoint = 100.0
pattern.count = 1
pattern.points = self.pointsInput
pattern.radius = 20.0
pattern.preempt = False
command = MsgGalvoCommand()
command.frameid_target_list = ['Plate',]
command.pattern_list = [pattern,]
command.units = 'volts' #'millimeters' # 'volts' #
self.pubGalvoCommand.publish(command)
def TrackFly1(self):
pattern = MsgPattern()
pattern.mode = 'byshape'
pattern.shape = 'grid'
pattern.frame_id = 'Fly1'
pattern.hzPattern = 40.0
pattern.hzPoint = 1000.0
pattern.count = 1
pattern.points = []
pattern.radius = 5
pattern.preempt = False
command = MsgGalvoCommand()
command.frameid_target_list = ['Plate',]
command.pattern_list = [pattern,]
command.units = 'millimeters' # 'millimeters' or 'volts'
self.pubGalvoCommand.publish(command)
def SendInputPoints(self):
pattern = MsgPattern()
pattern.frameidPosition = 'Arena'
pattern.frameidAngle = 'Arena'
pattern.shape = 'bypoints'
pattern.hzPattern = 1.0
pattern.hzPoint = 100.0
pattern.count = 1
pattern.points = self.pointsInput
pattern.size.x = 20.0
pattern.size.y = 20.0
pattern.restart = False
pattern.param = 0.0
pattern.direction = 1
command = MsgGalvoCommand()
command.pattern_list = [pattern,]
command.units = 'volts' #'millimeters' # 'volts' #
self.pubGalvodirectorCommand.publish(command)
def SendPoint(self, point):
pattern = MsgPattern()
pattern.frameidPosition = 'Arena'
pattern.frameidAngle = 'Arena'
pattern.shape = 'bypoints'
pattern.hzPattern = 1.0
pattern.hzPoint = 100.0
pattern.count = 1
pattern.points = [point,]
pattern.size.x = 0.0
pattern.size.y = 0.0
pattern.preempt = False
pattern.param = 0.0
pattern.direction = 1
command = MsgGalvoCommand()
command.enable_laser = True
command.pattern_list = [pattern,]
command.units = 'volts' #'millimeters' # 'volts' #
self.pubGalvoCommand.publish(command)
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
for pattern in self.paramsIn.lasergalvos.pattern_list:
rospy.loginfo("EL State Lasergalvos(%s)" % pattern)
if self.paramsIn.lasergalvos.enabled:
rv = 'aborted'
self.timeStart = rospy.Time.now()
# Create the tracking command for the galvo director.
commandGalvo = MsgGalvoCommand()
commandGalvo.enable_laser = True
commandGalvo.units = 'millimeters' # 'millimeters' or 'volts'
commandGalvo.pattern_list = self.paramsIn.lasergalvos.pattern_list
# Determine if we're showing patterns only for certain states.
nPatterns = len(self.paramsIn.lasergalvos.pattern_list)
if len(self.paramsIn.lasergalvos.statefilterLo_list) == nPatterns and \
len(self.paramsIn.lasergalvos.statefilterHi_list) == nPatterns:
isStatefiltered = True
else:
isStatefiltered = False
if (len(self.paramsIn.lasergalvos.statefilterLo_list) != len(self.paramsIn.lasergalvos.statefilterHi_list)):
rospy.logwarn('Check your experiment params structure: Number of statefilters does not match number of patterns. Not filtering states.')
# Initialize statefilter vars.
bInStatefilterRangePrev = [False for i in range(nPatterns)]
bInStatefilterRange = [False for i in range(nPatterns)]
# If unfiltered, publish the command.
if not isStatefiltered:
self.pubGalvoCommand.publish(commandGalvo)
# Move galvos until preempt or timeout.
while not rospy.is_shutdown():
self.step_time_elapsed()
# If state is used to determine when pattern is shown.
if isStatefiltered:
# Check if any filterstates have changed.
bFilterStateChanged = False
for iPattern in range(nPatterns):
# Get the pattern.
pattern = self.paramsIn.lasergalvos.pattern_list[iPattern]
# Convert strings to dicts.
statefilterLo_dict = eval(self.paramsIn.lasergalvos.statefilterLo_list[iPattern])
statefilterHi_dict = eval(self.paramsIn.lasergalvos.statefilterHi_list[iPattern])
statefilterCriteria = self.paramsIn.lasergalvos.statefilterCriteria_list[iPattern]
# 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 pattern.frameidPosition):
#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 pattern.frameidPosition):
for iFly in range(len(self.arenastate.flies)):
if (self.arenastate.flies[iFly].name in pattern.frameidPosition):
#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', pattern.frameidPosition)
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', pattern.frameidPosition, stamp):
try:
poseStamped = self.tfrx.transformPose('Arena', PoseStamped(header=Header(stamp=stamp,
frame_id=pattern.frameidPosition),
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', pattern.frameidPosition, stamp):
try:
velocity_tuple = self.tfrx.lookupTwist('Arena', pattern.frameidPosition, 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 any of the states are in range of the filter.
if (pose is not None) and (velocity is not None) and (speed is not None):
state = MsgFrameState(pose = pose,
velocity = velocity,
speed = speed)
bInStatefilterRangePrev[iPattern] = bInStatefilterRange[iPattern]
bInStatefilterRange[iPattern] = self.Statefilter.InRange(state, statefilterLo_dict, statefilterHi_dict, statefilterCriteria)
self.Statefilter.PublishMarkers (state, statefilterLo_dict, statefilterHi_dict, statefilterCriteria)
# If any of the filter states have changed, then we need to update them all.
if bInStatefilterRangePrev[iPattern] != bInStatefilterRange[iPattern]:
bFilterStateChanged = True
# end for iPattern in range(nPatterns)
# If filter state has changed, then publish the new command
if bFilterStateChanged:
commandGalvo.pattern_list = []
for iPattern in range(nPatterns):
if bInStatefilterRange[iPattern]:
pattern = self.paramsIn.lasergalvos.pattern_list[iPattern]
commandGalvo.pattern_list.append(pattern)
if len(commandGalvo.pattern_list)>0:
commandGalvo.enable_laser = True
else:
commandGalvo.enable_laser = False
self.pubGalvoCommand.publish(commandGalvo)
# else commandGalvo.pattern_list contains all patterns, and has already been published.
if self.preempt_requested():
rospy.loginfo('preempt requested: Lasergalvos()')
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()
else:
rv = 'disabled'
# Turn off the laser.
commandGalvo = MsgGalvoCommand()
commandGalvo.enable_laser = False
self.pubGalvoCommand.publish(commandGalvo)
userdata.experimentparamsOut = self.experimentparams
return rv
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
for pattern in self.paramsIn.lasergalvos.pattern_list:
rospy.loginfo("EL State Lasergalvos(%s)" % pattern)
if self.paramsIn.lasergalvos.enabled:
rv = 'aborted'
self.timeStart = rospy.Time.now()
# Create the tracking command for the galvo director.
commandGalvo = MsgGalvoCommand()
commandGalvo.enable_laser = True
commandGalvo.units = 'millimeters' # 'millimeters' or 'volts'
commandGalvo.pattern_list = self.paramsIn.lasergalvos.pattern_list
# Determine if we're showing patterns only for certain states.
nPatterns = len(self.paramsIn.lasergalvos.pattern_list)
if len(self.paramsIn.lasergalvos.statefilterLo_list) == nPatterns and \
len(self.paramsIn.lasergalvos.statefilterHi_list) == nPatterns:
isStatefiltered = True
else:
isStatefiltered = False
if (len(self.paramsIn.lasergalvos.statefilterLo_list) != len(
self.paramsIn.lasergalvos.statefilterHi_list)):
rospy.logwarn(
'Check your experiment params structure: Number of statefilters does not match number of patterns. Not filtering states.'
)
# Initialize statefilter vars.
bInStatefilterRangePrev = [False for i in range(nPatterns)]
bInStatefilterRange = [False for i in range(nPatterns)]
# If unfiltered, publish the command.
if not isStatefiltered:
self.pubGalvoCommand.publish(commandGalvo)
# Move galvos until preempt or timeout.
while not rospy.is_shutdown():
self.step_time_elapsed()
# If state is used to determine when pattern is shown.
if isStatefiltered:
# Check if any filterstates have changed.
bFilterStateChanged = False
for iPattern in range(nPatterns):
# Get the pattern.
pattern = self.paramsIn.lasergalvos.pattern_list[
iPattern]
# Convert strings to dicts.
statefilterLo_dict = eval(self.paramsIn.lasergalvos.
statefilterLo_list[iPattern])
statefilterHi_dict = eval(self.paramsIn.lasergalvos.
statefilterHi_list[iPattern])
statefilterCriteria = self.paramsIn.lasergalvos.statefilterCriteria_list[
iPattern]
# 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 pattern.frameidPosition):
#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 pattern.frameidPosition):
for iFly in range(len(self.arenastate.flies)):
if (self.arenastate.flies[iFly].name
in pattern.frameidPosition):
#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', pattern.frameidPosition)
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', pattern.frameidPosition, stamp):
try:
poseStamped = self.tfrx.transformPose(
'Arena',
PoseStamped(header=Header(
stamp=stamp,
frame_id=pattern.frameidPosition),
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', pattern.frameidPosition, stamp):
try:
velocity_tuple = self.tfrx.lookupTwist(
'Arena', pattern.frameidPosition, 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 any of the states are in range of the filter.
if (pose is not None) and (velocity is not None) and (
speed is not None):
state = MsgFrameState(pose=pose,
velocity=velocity,
speed=speed)
bInStatefilterRangePrev[
iPattern] = bInStatefilterRange[iPattern]
bInStatefilterRange[
iPattern] = self.Statefilter.InRange(
state, statefilterLo_dict,
statefilterHi_dict, statefilterCriteria)
self.Statefilter.PublishMarkers(
state, statefilterLo_dict, statefilterHi_dict,
statefilterCriteria)
# If any of the filter states have changed, then we need to update them all.
if bInStatefilterRangePrev[
iPattern] != bInStatefilterRange[iPattern]:
bFilterStateChanged = True
# end for iPattern in range(nPatterns)
# If filter state has changed, then publish the new command
if bFilterStateChanged:
commandGalvo.pattern_list = []
for iPattern in range(nPatterns):
if bInStatefilterRange[iPattern]:
pattern = self.paramsIn.lasergalvos.pattern_list[
iPattern]
commandGalvo.pattern_list.append(pattern)
if len(commandGalvo.pattern_list) > 0:
commandGalvo.enable_laser = True
else:
commandGalvo.enable_laser = False
self.pubGalvoCommand.publish(commandGalvo)
# else commandGalvo.pattern_list contains all patterns, and has already been published.
if self.preempt_requested():
rospy.loginfo('preempt requested: Lasergalvos()')
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()
else:
rv = 'disabled'
# Turn off the laser.
commandGalvo = MsgGalvoCommand()
commandGalvo.enable_laser = False
self.pubGalvoCommand.publish(commandGalvo)
userdata.experimentparamsOut = self.experimentparams
return rv
