def OnShutdown_callback(self):
if self.initialized:
msgPattern = MsgPattern()
msgPattern.mode = 'byshape'
msgPattern.shape = 'constant'
msgPattern.points = []
msgPattern.frame = 'Stage'
msgPattern.hzPattern = 1.0
msgPattern.hzPoint = 100
msgPattern.count = 1
msgPattern.radius = 0.0
msgPattern.preempt = True
self.pubPatternGen.publish (msgPattern)
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.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 __init__(self):
rospy.init_node('PatternGenXY')
rospy.loginfo ("PatternGenXY name=%s", __name__)
# The pattern for continuous output.
self.output = Struct()
self.output.pattern = MsgPattern()
self.output.pattern.shape = 'none'
self.output.pattern.hzPattern = 1.0
self.output.pattern.hzPoint = 50
self.output.pattern.count = 0
self.output.pattern.points = []
self.output.pattern.frameidPosition = 'Arena'
self.output.pattern.frameidAngle = 'Arena'
self.output.pattern.size = Point(25.4, 25.4, 0.0)
self.output.pattern.restart = True
self.output.pattern.param = 0.0
self.output.pattern.isDirty = True
self.output.pathlen = 0.0
self.output.ratePoint = rospy.Rate(self.output.pattern.hzPoint)
self.output.iPoint = None
self.output.lock = threading.Lock()
self.command = 'continue'
self.command_list = ['continue','pause_now','pause_after_trial', 'exit_after_trial', 'exit_now']
self.subCommand = rospy.Subscriber('experiment/command', String, self.Command_callback)
self.subSetPattern = rospy.Subscriber('SetPattern', MsgPattern, self.SetPattern_callback)
self.tfrx = tf.TransformListener()
self.services = {}
self.services['GetPatternPoints'] = rospy.Service('GetPatternPoints', SrvGetPatternPoints, self.GetPatternPoints_callback)
# Load stage services.
self.stSignalInput = 'signal_input'
rospy.loginfo('Waiting for: '+self.stSignalInput)
rospy.wait_for_service(self.stSignalInput)
try:
self.SignalOutput = rospy.ServiceProxy(self.stSignalInput, SrvSignal)
except rospy.ServiceException, e:
rospy.loginfo ("FAILED %s: %s"%(self.stSignalInput,e))
def SetPattern_callback (self, msgPattern):
iPointPre = self.output.iPoint
# To provide a glitch-free transition, do the time-consuming setup of outputTmp first, then copy it into place at the end.
outputTmp = Struct()
outputTmp.pattern = MsgPattern()
outputTmp.pattern.isDirty = True
outputTmp.pattern.frameidPosition = msgPattern.frameidPosition
outputTmp.pattern.frameidAngle = msgPattern.frameidAngle
outputTmp.pattern.shape = msgPattern.shape
outputTmp.pattern.hzPattern = msgPattern.hzPattern
outputTmp.pattern.hzPoint = msgPattern.hzPoint
outputTmp.pattern.count = msgPattern.count
outputTmp.pattern.size = msgPattern.size
outputTmp.pattern.restart = msgPattern.restart
outputTmp.pattern.param = msgPattern.param
outputTmp.pattern.direction = msgPattern.direction # Forward (+1) or reverse (-1) through the pattern points. 0 means choose at random, +1 or -1.
# If direction==0, then choose random +1 or -1.
if (outputTmp.pattern.direction==0):
outputTmp.pattern.direction = 2*np.random.randint(2)-1
outputTmp.ratePoint = rospy.Rate(outputTmp.pattern.hzPoint)
if (outputTmp.pattern.count==-1):
outputTmp.pattern.count = 2147483640 # MAX_INT
if (outputTmp.pattern.shape=='bypoints'):
outputTmp.pattern.points = msgPattern.points
self.UpdatePatternPoints(outputTmp.pattern)
outputTmp.pathlen = self.GetPathLength(outputTmp.pattern.points)
if (0 < len(outputTmp.pattern.points)):
if (not outputTmp.pattern.restart) and (iPointPre is not None):
# Find the point in the new pattern nearest the current point in the last pattern.
# Convert Point()'s to lists.
xyNew = []
for pt in outputTmp.pattern.points:
xyNew.append([pt.x,pt.y])
#rospy.logwarn('%s, %s' % (self.output.iPoint, len(self.output.pattern.points)))
if (self.output.iPoint is not None):
pointPrev = self.output.pattern.points[self.output.iPoint] # Save the prev cursor.
xyPrev = [pointPrev.x, pointPrev.y]
dxy = np.subtract(xyPrev, xyNew)
dx = dxy[:,0]
dy = dxy[:,1]
d = np.hypot(dx, dy)
iNearest = np.argmin(d) # Index into self.output.pattern.points of the new cursor.
iPointTmp = iNearest
else:
if (0 < outputTmp.pattern.direction):
iPointTmp = 0
elif (outputTmp.pattern.direction < 0):
iPointTmp = len(outputTmp.pattern.points)-1
else:
iPointTmp = 0
rospy.logwarn ('pattern.direction==0. Pattern will not advance.')
if (iPointPre is not None):
iPointPost = self.output.iPoint
outputTmp.iPoint = (iPointTmp + (iPointPost-iPointPre)) % len(outputTmp.pattern.points) # Install the new iNearest, plus a correction for processing time.
else:
outputTmp.iPoint = iPointTmp
with self.output.lock:
self.output.pattern = outputTmp.pattern
self.output.ratePoint = outputTmp.ratePoint
self.output.pathlen = outputTmp.pathlen
self.output.iPoint = outputTmp.iPoint
def __init__(self):
rospy.init_node('Experiment')
# Fill out the data structure that defines the experiment.
self.experimentparams = ExperimentParamsChoicesRequest()
#flies = 'HCS'
flies = 'TrpA1_sugar'
#flies = 'TrpA1_paralysis'
#flies = 'TrpA1_wingextender'
t0 = 120 # off
t1 = 3 # on
t2 = 117 # off
laserpower = '260mW'
self.experimentparams.experiment.description = 'Laser is off for %d secs, on for %d secs, and off for %d secs' % (t0, t1, t2)
self.experimentparams.experiment.maxTrials = 100
self.experimentparams.experiment.timeout = -1
self.experimentparams.save.filenamebase = 'zapresponse_%s_%02ds_%02ds_%02ds_%s_' % (flies, t0, t1, t2, laserpower)
self.experimentparams.save.csv = True
self.experimentparams.save.bag = False
self.experimentparams.save.mov = False
self.experimentparams.save.fmf = False
self.experimentparams.save.imagetopic_list = ['camera/image_rect']
self.experimentparams.save.onlyWhileTriggered = False
self.experimentparams.robotspec.nRobots = 0
self.experimentparams.robotspec.width = 1.5875
self.experimentparams.robotspec.height = 1.5875
self.experimentparams.robotspec.isPresent = True # Set this to False if you remove the robot, but still want the actuation.
self.experimentparams.robotspec.description = 'Black oxide magnet'
self.experimentparams.flyspec.nFlies = 1
self.experimentparams.flyspec.description = flies
self.experimentparams.tracking.exclusionzones.enabled = False
self.experimentparams.tracking.exclusionzones.point_list = [Point(x=0.0, y=0.0)]
self.experimentparams.tracking.exclusionzones.radius_list = [0.0]
self.experimentparams.home.enabled = False
self.experimentparams.pre.robot.enabled = False
self.experimentparams.pre.lasergalvos.enabled = False
self.experimentparams.pre.ledpanels.enabled = False
self.experimentparams.pre.wait1 = t0
self.experimentparams.pre.trigger.enabled = False
self.experimentparams.pre.trigger.frameidParent = 'Arena'
self.experimentparams.pre.trigger.frameidChild = 'Fly01'
self.experimentparams.pre.trigger.speedAbsParentMin = 0.0
self.experimentparams.pre.trigger.speedAbsParentMax = 999.0
self.experimentparams.pre.trigger.speedAbsChildMin = 0.0
self.experimentparams.pre.trigger.speedAbsChildMax = 999.0
self.experimentparams.pre.trigger.speedRelMin = 0.0
self.experimentparams.pre.trigger.speedRelMax = 999.0
self.experimentparams.pre.trigger.distanceMin = 0.0
self.experimentparams.pre.trigger.distanceMax = 999.0
self.experimentparams.pre.trigger.angleMin = 0.0 * np.pi / 180.0
self.experimentparams.pre.trigger.angleMax =180.0 * np.pi / 180.0
self.experimentparams.pre.trigger.angleTest = 'inclusive'
self.experimentparams.pre.trigger.angleTestBilateral = True
self.experimentparams.pre.trigger.timeHold = 0.0
self.experimentparams.pre.trigger.timeout = -1
self.experimentparams.pre.wait2 = 0.0
# .robot, .lasergalvos, .ledpanels, and .post.trigger all run concurrently.
# The first one to finish preempts the others.
self.experimentparams.trial.robot.enabled = False
flies_list = range(1,1+self.experimentparams.flyspec.nFlies)
self.experimentparams.trial.lasergalvos.enabled = True
self.experimentparams.trial.lasergalvos.pattern_list = []
self.experimentparams.trial.lasergalvos.statefilterHi_list = []
self.experimentparams.trial.lasergalvos.statefilterLo_list = []
self.experimentparams.trial.lasergalvos.statefilterCriteria_list = []
for iFly in flies_list:
self.experimentparams.trial.lasergalvos.pattern_list.append(MsgPattern(
frameidPosition = 'Fly%02dForecast' % iFly,
frameidAngle = 'Fly%02dForecast' % iFly,
shape = 'grid',
hzPattern = 40.0,
hzPoint = 1000.0,
count = 1,
size = Point(x=2,
y=2),
restart = False,
param = 3, # Peano curve level.
direction = 1),
)
self.experimentparams.trial.ledpanels.enabled = True
self.experimentparams.trial.ledpanels.command = ['fixed'] # 'fixed', 'trackposition' (panel position follows fly position), or 'trackview' (panel position follows fly's viewpoint).
self.experimentparams.trial.ledpanels.idPattern = [3]
self.experimentparams.trial.ledpanels.frame_id = ['Fly01Forecast']
self.experimentparams.trial.ledpanels.statefilterHi = ['']
self.experimentparams.trial.ledpanels.statefilterLo = ['']
self.experimentparams.trial.ledpanels.statefilterCriteria = ['']
self.experimentparams.post.trigger.enabled = True
self.experimentparams.post.trigger.frameidParent = 'Arena'
self.experimentparams.post.trigger.frameidChild = 'Fly01'
self.experimentparams.post.trigger.speedAbsParentMin = 0.0
self.experimentparams.post.trigger.speedAbsParentMax = 999.0
self.experimentparams.post.trigger.speedAbsChildMin = 0.0
self.experimentparams.post.trigger.speedAbsChildMax = 999.0
self.experimentparams.post.trigger.speedRelMin = 0.0
self.experimentparams.post.trigger.speedRelMax = 999.0
self.experimentparams.post.trigger.distanceMin = 999.0
self.experimentparams.post.trigger.distanceMax = 888.0 # i.e. never
self.experimentparams.post.trigger.angleMin = 0.0 * np.pi / 180.0
self.experimentparams.post.trigger.angleMax =180.0 * np.pi / 180.0
self.experimentparams.post.trigger.angleTest = 'inclusive'
self.experimentparams.post.trigger.angleTestBilateral = True
self.experimentparams.post.trigger.timeHold = 0.0
self.experimentparams.post.trigger.timeout = t1
self.experimentparams.post.wait = t2
self.experimentlib = ExperimentLib.ExperimentLib(self.experimentparams,
startexperiment_callback = self.StartExperiment_callback,
starttrial_callback = self.StartTrial_callback,
endtrial_callback = self.EndTrial_callback)
def MovePattern (self):
msgPattern = MsgPattern()
# Publish the pattern message.
msgPattern.shape = self.paramsIn.robot.move.pattern.shape
msgPattern.points = []
msgPattern.frameidPosition = self.paramsIn.robot.move.pattern.frameidPosition
msgPattern.frameidAngle = self.paramsIn.robot.move.pattern.frameidAngle
msgPattern.hzPattern = self.paramsIn.robot.move.pattern.hzPattern
msgPattern.hzPoint = self.paramsIn.robot.move.pattern.hzPoint
msgPattern.count = self.paramsIn.robot.move.pattern.count
msgPattern.size = self.paramsIn.robot.move.pattern.size
msgPattern.preempt = True
msgPattern.param = 0.0#self.paramsIn.robot.move.pattern.param
msgPattern.direction = self.paramsIn.robot.move.pattern.direction
self.pubPatternGen.publish (msgPattern)
rv = 'aborted'
while not rospy.is_shutdown():
if self.preempt_requested():
rospy.loginfo('preempt requested: MovePattern()')
self.service_preempt()
rv = 'preempt'
break
#if self.paramsIn.robot.move.timeout != -1:
# if (rospy.Time.now().to_sec()-self.timeStart.to_sec()) > self.paramsIn.robot.move.timeout:
# rv = 'timeout'
# 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)
if (self.commandExperiment=='pause_after_trial'):
pass
if (self.commandExperiment=='exit_after_trial'):
pass
if (self.commandExperiment=='exit_now'):
rv = 'aborted'
break
# Turn off the pattern
msgPattern.shape = self.paramsIn.robot.move.pattern.shape
msgPattern.points = []
msgPattern.frameidPosition = self.paramsIn.robot.move.pattern.frameidPosition
msgPattern.frameidAngle = self.paramsIn.robot.move.pattern.frameidAngle
msgPattern.hzPattern = self.paramsIn.robot.move.pattern.hzPattern
msgPattern.hzPoint = self.paramsIn.robot.move.pattern.hzPoint
msgPattern.count = 0
msgPattern.size = self.paramsIn.robot.move.pattern.size
msgPattern.preempt = True
msgPattern.param = 0.0#self.paramsIn.robot.move.pattern.param
msgPattern.direction = self.paramsIn.robot.move.pattern.direction
self.pubPatternGen.publish (msgPattern)
return rv
def execute(self, userdata):
rospy.loginfo('EL State ResetRobot(%s)' % [userdata.experimentparamsChoicesIn.home.enabled, userdata.experimentparamsChoicesIn.home.x, userdata.experimentparamsChoicesIn.home.y])
rv = 'disabled'
if (userdata.experimentparamsChoicesIn.trial.robot.enabled or userdata.experimentparamsChoicesIn.pre.robot.enabled) and (userdata.experimentparamsChoicesIn.home.enabled):
self.target = MsgFrameState()
self.timeStart = rospy.Time.now()
if (self.SetStageStateRef is None):
stSrv = 'set_stage_state_ref'
try:
rospy.wait_for_service(stSrv)
self.SetStageStateRef = rospy.ServiceProxy(stSrv, SrvFrameState, persistent=True)
except rospy.ServiceException, e:
rospy.logwarn ('EL FAILED to connect service %s(): %s' % (stSrv, e))
while self.arenastate is None:
#if userdata.experimentparamsChoicesIn.home.timeout != -1:
# if (rospy.Time.now().to_sec()-self.timeStart.to_sec()) > userdata.experimentparamsChoicesIn.home.timeout:
# return 'timeout'
#if self.preempt_requested():
# self.service_preempt()
# return 'preempt'
rospy.sleep(1.0)
if (self.commandExperiment=='exit_now'):
return 'aborted'
# Turn off any prior pattern generation.
msgPattern = MsgPattern()
msgPattern.shape = 'constant'
msgPattern.points = []
msgPattern.frameidPosition = 'Arena'
msgPattern.frameidAngle = 'Arena'
msgPattern.hzPattern = 1.0
msgPattern.hzPoint = 50
msgPattern.count = 0
msgPattern.size = Point(0.0, 0.0, 0.0)
msgPattern.restart = False
msgPattern.param = 0.0
msgPattern.direction = 1
self.pubSetPattern.publish (msgPattern)
rospy.sleep(1)
# Send the 'home' command.
self.target.header = self.arenastate.robot.header
#self.target.header.stamp = rospy.Time.now()
self.target.pose.position.x = userdata.experimentparamsChoicesIn.home.x
self.target.pose.position.y = userdata.experimentparamsChoicesIn.home.y
try:
self.SetStageStateRef(SrvFrameStateRequest(state=MsgFrameState(header=self.target.header,
pose=self.target.pose,
speed = userdata.experimentparamsChoicesIn.home.speed)))
except rospy.ServiceException, e:
rospy.logwarn ('EL FAILED service call to set_stage_state_ref().')
def MovePattern(self):
msgPattern = MsgPattern()
# Choose one from each set of choices at random.
msgPattern.shape = self.paramsIn.robot.move.pattern.shape
msgPattern.points = []
msgPattern.frameidPosition = self.paramsIn.robot.move.pattern.frameidPosition
msgPattern.frameidAngle = self.paramsIn.robot.move.pattern.frameidAngle
msgPattern.hzPattern = self.paramsIn.robot.move.pattern.hzPattern
msgPattern.hzPoint = self.paramsIn.robot.move.pattern.hzPoint
msgPattern.count = self.paramsIn.robot.move.pattern.count
msgPattern.size = self.paramsIn.robot.move.pattern.size
msgPattern.restart = self.paramsIn.robot.move.pattern.restart
msgPattern.param = 0.0 #self.paramsIn.robot.move.pattern.param
msgPattern.direction = self.paramsIn.robot.move.pattern.direction
self.pubSetPattern.publish(msgPattern)
rv = 'aborted'
while not rospy.is_shutdown():
self.step_time_elapsed()
if self.preempt_requested():
rospy.loginfo('preempt requested: MovePattern()')
self.service_preempt()
rv = 'preempt'
break
# 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
self.rosrate.sleep()
# Turn off the pattern
msgPattern.count = 0
self.pubSetPattern.publish(msgPattern)
return rv
def OnShutdown_callback(self):
if self.initialized:
msgPattern = MsgPattern()
msgPattern.shape = 'constant'
msgPattern.points = []
msgPattern.frameidPosition = 'Stage'
msgPattern.frameidAngle = 'Stage'
msgPattern.hzPattern = 1.0
msgPattern.hzPoint = 100
msgPattern.count = 1
msgPattern.size = Point(x=0,y=0)
msgPattern.restart = True
msgPattern.param = 0
self.pubSetPattern.publish (msgPattern)
def OnShutdown_callback(self):
if self.initialized:
msgPattern = MsgPattern()
msgPattern.shape = 'constant'
msgPattern.points = []
msgPattern.frameidPosition = 'Stage'
msgPattern.frameidAngle = 'Stage'
msgPattern.hzPattern = 1.0
msgPattern.hzPoint = 100
msgPattern.count = 1
msgPattern.size = Point(x=0, y=0)
msgPattern.restart = True
msgPattern.param = 0
self.pubSetPattern.publish(msgPattern)
def __init__(self):
rospy.init_node('Experiment')
# Fill out the data structure that defines the experiment.
self.experimentparams = ExperimentParamsChoicesRequest()
flies = 'TrpA1_sugar'
radius = 50
t0 = 10 # off
t1 = 3 # on
t2 = 117 # off
laserpower = '260mW'
self.experimentparams.experiment.description = 'Lasers the fly when within %dmm of center, for %d secs, then off for %d secs.' % (
radius, t1, t2)
self.experimentparams.experiment.maxTrials = 10
self.experimentparams.experiment.timeout = -1
#self.experimentparams.save.filenamebase = 'HCS_normal_150mW_'
#self.experimentparams.save.filenamebase = 'TrpA1neg_AristaeIntact_150mW_' #
#self.experimentparams.save.filenamebase = 'UAS_TrpA1_parentalcontrol_180mW_'
#self.experimentparams.save.filenamebase = 'posvel_TrpA1_geosmin_fed_120mW_'
#self.experimentparams.save.filenamebase = 'posvel_TrpA1_CVA_260mW_'
self.experimentparams.save.filenamebase = 'zapresponsearea_%s_%dmm_%02ds_%02ds_%s_' % (
flies, radius, t1, t2, laserpower)
self.experimentparams.save.csv = True
self.experimentparams.save.bag = False
self.experimentparams.save.mov = False
self.experimentparams.save.fmf = False
self.experimentparams.save.imagetopic_list = ['camera/image_rect']
self.experimentparams.save.onlyWhileTriggered = False # Saves always.
self.experimentparams.robotspec.nRobots = 0
self.experimentparams.robotspec.width = 1.5875
self.experimentparams.robotspec.height = 1.5875
self.experimentparams.robotspec.isPresent = True # Set this to False if you remove the robot, but still want the actuation.
self.experimentparams.robotspec.description = 'none'
self.experimentparams.flyspec.nFlies = 1
self.experimentparams.flyspec.description = flies
self.experimentparams.tracking.exclusionzones.enabled = False
self.experimentparams.tracking.exclusionzones.point_list = [
Point(x=45.0, y=48.0)
]
self.experimentparams.tracking.exclusionzones.radius_list = [8.0]
self.experimentparams.home.enabled = False
self.experimentparams.pre.robot.enabled = False
self.experimentparams.pre.lasergalvos.enabled = False
self.experimentparams.pre.ledpanels.enabled = False
self.experimentparams.pre.wait1 = t0
self.experimentparams.pre.trigger.enabled = True
self.experimentparams.pre.trigger.frameidParent = 'Arena'
self.experimentparams.pre.trigger.frameidChild = 'Fly01'
self.experimentparams.pre.trigger.speedAbsParentMin = 0.0
self.experimentparams.pre.trigger.speedAbsParentMax = 999.0
self.experimentparams.pre.trigger.speedAbsChildMin = 0.0
self.experimentparams.pre.trigger.speedAbsChildMax = 999.0
self.experimentparams.pre.trigger.speedRelMin = 0.0
self.experimentparams.pre.trigger.speedRelMax = 999.0
self.experimentparams.pre.trigger.distanceMin = 0.0
self.experimentparams.pre.trigger.distanceMax = radius
self.experimentparams.pre.trigger.angleMin = 0.0 * np.pi / 180.0
self.experimentparams.pre.trigger.angleMax = 180.0 * np.pi / 180.0
self.experimentparams.pre.trigger.angleTest = 'inclusive'
self.experimentparams.pre.trigger.angleTestBilateral = True
self.experimentparams.pre.trigger.timeHold = 0.0
self.experimentparams.pre.trigger.timeout = -1
self.experimentparams.pre.wait2 = 0.0
# .robot, .lasergalvos, .ledpanels, and .post.trigger all run concurrently.
# The first one to finish preempts the others.
self.experimentparams.trial.robot.enabled = False
flies_list = range(1, 1 + self.experimentparams.flyspec.nFlies)
self.experimentparams.trial.lasergalvos.enabled = True
self.experimentparams.trial.lasergalvos.pattern_list = []
self.experimentparams.trial.lasergalvos.statefilterHi_list = []
self.experimentparams.trial.lasergalvos.statefilterLo_list = []
self.experimentparams.trial.lasergalvos.statefilterCriteria_list = []
for iFly in flies_list:
self.experimentparams.trial.lasergalvos.pattern_list.append(
MsgPattern(
frameidPosition='Fly%02dForecast' % iFly,
frameidAngle='Fly%02dForecast' % iFly,
shape='grid',
hzPattern=40.0,
hzPoint=1000.0,
count=1,
size=Point(x=2, y=2),
restart=False,
param=3, # Peano curve level.
direction=1), )
#self.experimentparams.trial.lasergalvos.statefilterHi_list.append("{'speed':5.0}")
#self.experimentparams.trial.lasergalvos.statefilterLo_list.append("{'speed':0.0}")
#self.experimentparams.trial.lasergalvos.statefilterHi_list.append("{'velocity':{'linear':{'x':1,'y':9999}}}")
#self.experimentparams.trial.lasergalvos.statefilterLo_list.append("{'velocity':{'linear':{'x':-9999,'y':-9999}}}")
#self.experimentparams.trial.lasergalvos.statefilterHi_list.append("{'velocity':{'linear':{'x':1,'y':9999}},'pose':{'position':{'x':0, 'y':100}}}")
#self.experimentparams.trial.lasergalvos.statefilterLo_list.append("{'velocity':{'linear':{'x':-9999,'y':-9999}},'pose':{'position':{'x':-100, 'y':-100}}}")
#self.experimentparams.trial.lasergalvos.statefilterHi_list.append("{'velocity':{'angular':{'z':999}}}")
#self.experimentparams.trial.lasergalvos.statefilterLo_list.append("{'velocity':{'angular':{'z':0.5}}}")
#self.experimentparams.trial.lasergalvos.statefilterHi_list.append("{'pose':{'position':{'x':40, 'y':40}}}")
#self.experimentparams.trial.lasergalvos.statefilterLo_list.append("{'pose':{'position':{'x':-40, 'y':-40}}}")
#self.experimentparams.trial.lasergalvos.statefilterCriteria_list.append('inclusive') # 'inclusive'=laserOn if all criteria are satisfied, laserOff if any criteria is violated.
self.experimentparams.trial.ledpanels.enabled = True
self.experimentparams.trial.ledpanels.command = [
'fixed'
] # 'fixed', 'trackposition' (panel position follows fly position), or 'trackview' (panel position follows fly's viewpoint).
self.experimentparams.trial.ledpanels.idPattern = [3]
self.experimentparams.trial.ledpanels.frame_id = ['Fly01Forecast']
self.experimentparams.trial.ledpanels.statefilterHi = ['']
self.experimentparams.trial.ledpanels.statefilterLo = ['']
self.experimentparams.trial.ledpanels.statefilterCriteria = ['']
self.experimentparams.post.trigger.enabled = True
self.experimentparams.post.trigger.frameidParent = 'Arena'
self.experimentparams.post.trigger.frameidChild = 'Fly01'
self.experimentparams.post.trigger.speedAbsParentMin = 0.0
self.experimentparams.post.trigger.speedAbsParentMax = 999.0
self.experimentparams.post.trigger.speedAbsChildMin = 0.0
self.experimentparams.post.trigger.speedAbsChildMax = 999.0
self.experimentparams.post.trigger.speedRelMin = 0.0
self.experimentparams.post.trigger.speedRelMax = 999.0
self.experimentparams.post.trigger.distanceMin = 999.0
self.experimentparams.post.trigger.distanceMax = 111.0 # i.e. never
self.experimentparams.post.trigger.angleMin = 0.0000 * np.pi / 180.0
self.experimentparams.post.trigger.angleMax = 359.9999 * np.pi / 180.0
self.experimentparams.post.trigger.angleTest = 'inclusive'
self.experimentparams.post.trigger.angleTestBilateral = False
self.experimentparams.post.trigger.timeHold = 0.0
self.experimentparams.post.trigger.timeout = t1
self.experimentparams.post.wait = t2
self.experimentlib = ExperimentLib.ExperimentLib(
self.experimentparams,
startexperiment_callback=self.StartExperiment_callback,
starttrial_callback=self.StartTrial_callback,
endtrial_callback=self.EndTrial_callback)
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.restart = False
pattern.param = 0.0
pattern.direction = 1
command = MsgGalvoCommand()
command.enable_laser = True
command.pattern_list = [pattern,]
command.units = 'volts' #'millimeters' # 'volts' #
self.pubGalvodirectorCommand.publish(command)
def __init__(self):
rospy.init_node('Experiment')
# Fill out the data structure that defines the experiment.
self.experimentparams = ExperimentParamsChoicesRequest()
self.experimentparams.experiment.description = 'Fly Chases Robot Moving in Circle, with laser activation.'
self.experimentparams.experiment.maxTrials = -1
self.experimentparams.experiment.timeout = 600
self.experimentparams.save.filenamebase = 'lasertag'
self.experimentparams.save.csv = True
self.experimentparams.save.bag = True
self.experimentparams.save.mov = False
self.experimentparams.save.imagetopic_list = ['camera/image_rect']
self.experimentparams.save.onlyWhileTriggered = True
self.experimentparams.robotspec.nRobots = 1
self.experimentparams.robotspec.width = 1.5875
self.experimentparams.robotspec.height = 1.5875
self.experimentparams.robotspec.isPresent = True # Set this to False if you remove the robot, but still want the actuation.
self.experimentparams.robotspec.description = 'Black oxide magnet'
self.experimentparams.flyspec.nFlies = 1
self.experimentparams.flyspec.description = 'unspecified'
self.experimentparams.tracking.exclusionzones.enabled = False
self.experimentparams.tracking.exclusionzones.point_list = [Point(x=0.0, y=0.0)]
self.experimentparams.tracking.exclusionzones.radius_list = [0.0]
self.experimentparams.home.enabled = False
self.experimentparams.home.x = rospy.get_param('motorarm/L1', 999)
self.experimentparams.home.y = 0.0
self.experimentparams.home.speed = 20
self.experimentparams.home.tolerance = 4
self.experimentparams.pre.robot.enabled = True
self.experimentparams.pre.robot.move.mode = 'pattern' # 'pattern' or 'relative'
self.experimentparams.pre.robot.move.pattern.frameidPosition = ['Arena']
self.experimentparams.pre.robot.move.pattern.frameidAngle = ['Arena']
self.experimentparams.pre.robot.move.pattern.shape = ['circle'] # 'constant' or 'circle' or 'square' or 'flylogo' or 'spiral' or 'ramp'
self.experimentparams.pre.robot.move.pattern.hzPattern = [1/40]
self.experimentparams.pre.robot.move.pattern.hzPoint = [100]
self.experimentparams.pre.robot.move.pattern.count = [-1]
self.experimentparams.pre.robot.move.pattern.size = [Point(x=rospy.get_param('motorarm/L1', 999),
y=0)]
self.experimentparams.pre.robot.move.pattern.param = [0]
self.experimentparams.pre.robot.move.pattern.direction = [1]
self.experimentparams.pre.robot.move.pattern.restart = [False]
self.experimentparams.pre.lasergalvos.enabled = True
self.experimentparams.pre.lasergalvos.statefilterLo_list = []
self.experimentparams.pre.lasergalvos.statefilterHi_list = []
self.experimentparams.pre.lasergalvos.statefilterCriteria_list = []
iFly = 1
self.experimentparams.pre.lasergalvos.pattern_list = []
self.experimentparams.pre.lasergalvos.pattern_list.append(MsgPattern(
frameidPosition = 'Fly%02dForecast' % iFly,
frameidAngle = 'Fly%02dForecast' % iFly,
shape = 'grid',
hzPattern = 40.0,
hzPoint = 1000.0,
count = 1,
size = Point(x=2,
y=2),
restart = False,
param = 3, # Peano curve level.
direction = 1),
)
self.experimentparams.pre.ledpanels.enabled = False
self.experimentparams.pre.wait1 = 0#150.0
self.experimentparams.pre.trigger.enabled = True
self.experimentparams.pre.trigger.frameidParent = 'Fly01'
self.experimentparams.pre.trigger.frameidChild = 'Robot'
self.experimentparams.pre.trigger.speedAbsParentMin = 4.0
self.experimentparams.pre.trigger.speedAbsParentMax = 999.0
self.experimentparams.pre.trigger.speedAbsChildMin = 0.0
self.experimentparams.pre.trigger.speedAbsChildMax = 999.0
self.experimentparams.pre.trigger.speedRelMin = 0.0
self.experimentparams.pre.trigger.speedRelMax = 999.0
self.experimentparams.pre.trigger.distanceMin = 0.0
self.experimentparams.pre.trigger.distanceMax = 7.0
self.experimentparams.pre.trigger.angleMin = 0.0 * np.pi / 180.0
self.experimentparams.pre.trigger.angleMax = 60.0 * np.pi / 180.0
self.experimentparams.pre.trigger.angleTest = 'inclusive'
self.experimentparams.pre.trigger.angleTestBilateral = True
self.experimentparams.pre.trigger.timeHold = 0.0
self.experimentparams.pre.trigger.timeout = -1
self.experimentparams.pre.wait2 = 0.0
# .robot, .lasergalvos, .ledpanels, and .post.trigger all run concurrently.
# The first one to finish preempts the others.
self.experimentparams.trial.robot.enabled = True
self.experimentparams.trial.robot.move.mode = 'pattern' # 'pattern' or 'relative'
self.experimentparams.trial.robot.move.pattern.frameidPosition = ['Arena'] #
self.experimentparams.trial.robot.move.pattern.frameidAngle = ['Arena'] #
self.experimentparams.trial.robot.move.pattern.shape = ['circle'] # 'constant' or 'circle' or 'square' or 'flylogo' or 'spiral' or 'ramp'
self.experimentparams.trial.robot.move.pattern.hzPattern = [1/40]
self.experimentparams.trial.robot.move.pattern.hzPoint = [100]
self.experimentparams.trial.robot.move.pattern.count = [-1]
self.experimentparams.trial.robot.move.pattern.size = [Point(x=rospy.get_param('motorarm/L1', 999),
y=0)]
self.experimentparams.trial.robot.move.pattern.param = [0]
self.experimentparams.trial.robot.move.pattern.direction = [1]
self.experimentparams.trial.lasergalvos.enabled = False
self.experimentparams.trial.lasergalvos.statefilterLo_list = []
self.experimentparams.trial.lasergalvos.statefilterHi_list = []
self.experimentparams.trial.lasergalvos.statefilterCriteria_list = []
iFly = 1
self.experimentparams.trial.lasergalvos.pattern_list = []
self.experimentparams.trial.lasergalvos.pattern_list.append(MsgPattern(
frameidPosition = 'Fly%02dForecast' % iFly,
frameidAngle = 'Fly%02dForecast' % iFly,
shape = 'grid',
hzPattern = 40.0,
hzPoint = 1000.0,
count = 1,
size = Point(x=2,
y=2),
restart = False,
param = 3, # Peano curve level.
direction = 1),
)
self.experimentparams.trial.ledpanels.enabled = True
self.experimentparams.trial.ledpanels.command = ['fixed'] # 'fixed', 'trackposition' (panel position follows fly position), or 'trackview' (panel position follows fly's viewpoint).
self.experimentparams.trial.ledpanels.idPattern = [1]
self.experimentparams.trial.ledpanels.origin = [Point(x=0, y=0)]
self.experimentparams.trial.ledpanels.frame_id = ['Fly01Forecast']
self.experimentparams.trial.ledpanels.statefilterHi = ['']
self.experimentparams.trial.ledpanels.statefilterLo = ['']
self.experimentparams.trial.ledpanels.statefilterCriteria = ['']
self.experimentparams.post.trigger.enabled = True
self.experimentparams.post.trigger.frameidParent = 'Fly01'
self.experimentparams.post.trigger.frameidChild = 'Robot'
self.experimentparams.post.trigger.speedAbsParentMin = 0.0
self.experimentparams.post.trigger.speedAbsParentMax = 999.0
self.experimentparams.post.trigger.speedAbsChildMin = 0.0
self.experimentparams.post.trigger.speedAbsChildMax = 999.0
self.experimentparams.post.trigger.speedRelMin = 0.0
self.experimentparams.post.trigger.speedRelMax = 999.0
self.experimentparams.post.trigger.distanceMin = 15.0
self.experimentparams.post.trigger.distanceMax = 999.0
self.experimentparams.post.trigger.angleMin = 0.0 * np.pi / 180.0
self.experimentparams.post.trigger.angleMax =180.0 * np.pi / 180.0
self.experimentparams.post.trigger.angleTest = 'inclusive'
self.experimentparams.post.trigger.angleTestBilateral = True
self.experimentparams.post.trigger.timeHold = 0.0
self.experimentparams.post.trigger.timeout = 600
self.experimentparams.post.wait = 0.0
self.experimentlib = ExperimentLib.ExperimentLib(self.experimentparams,
startexperiment_callback = self.StartExperiment_callback,
starttrial_callback = self.StartTrial_callback,
endtrial_callback = self.EndTrial_callback)
def Main(self):
msgPattern = MsgPattern()
# Publish a goto(0,0) pattern message, and wait for the end effector to initialize.
msgPattern.mode = 'byshape'
msgPattern.shape = 'constant'
msgPattern.points = []
msgPattern.frame = 'Stage'
msgPattern.hzPattern = 1.0
msgPattern.hzPoint = rospy.get_param('actuator/hzPoint', 100.0)
msgPattern.count = 1
msgPattern.radius = 0.0
msgPattern.preempt = True
self.pubPatternGen.publish (msgPattern)
while not self.initialized_endeffector:
rospy.sleep(0.5)
rospy.sleep(2)
self.initialized = True
# Publish the spiral pattern message.
msgPattern.mode = 'byshape'
msgPattern.shape = rospy.get_param('calibration/shape', 'spiral')
msgPattern.points = []
msgPattern.frame = 'Stage'
msgPattern.count = -1
msgPattern.radius = 0.8 * rospy.get_param('arena/radius_inner', 25.4)
msgPattern.preempt = True
if msgPattern.shape=='spiral':
msgPattern.hzPattern = 0.01
else:
msgPattern.hzPattern = 0.1
self.pubPatternGen.publish (msgPattern)
try:
rospy.spin()
except KeyboardInterrupt:
print "Shutting down"
# Publish a goto(0,0) pattern message.
msgPattern.mode = 'byshape'
msgPattern.shape = 'constant'
msgPattern.points = []
msgPattern.frame = 'Stage'
msgPattern.hz = 1.0
msgPattern.count = 1
msgPattern.radius = 0.0
msgPattern.preempt = True
self.pubPatternGen.publish (msgPattern)
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.preempt = False
pattern.param = 0.0
pattern.direction = 1
command = MsgGalvoCommand()
command.pattern_list = [pattern,]
command.units = 'volts' #'millimeters' # 'volts' #
self.pubGalvoCommand.publish(command)
def __init__(self):
rospy.init_node('Experiment')
# Fill out the data structure that defines the experiment.
self.experimentparams = ExperimentParamsChoicesRequest()
self.experimentparams.experiment.description = 'Measure the temperature via the laser.'
self.experimentparams.experiment.maxTrials = -1
self.experimentparams.experiment.timeout = 600
self.experimentparams.save.filenamebase = 'lasertemp'
self.experimentparams.save.csv = False
self.experimentparams.save.bag = False
self.experimentparams.save.mov = False
self.experimentparams.save.imagetopic_list = ['camera/image_rect']
self.experimentparams.save.onlyWhileTriggered = True
self.experimentparams.robotspec.nRobots = 0
self.experimentparams.robotspec.width = 1.5875
self.experimentparams.robotspec.height = 1.5875
self.experimentparams.robotspec.isPresent = True # Set this to False if you remove the robot, but still want the actuation.
self.experimentparams.robotspec.description = 'Black oxide magnet'
self.experimentparams.flyspec.nFlies = 1
self.experimentparams.flyspec.description = 'unspecified'
self.experimentparams.tracking.exclusionzones.enabled = True
self.experimentparams.tracking.exclusionzones.point_list = [
Point(x=28.0, y=0.0)
]
self.experimentparams.tracking.exclusionzones.radius_list = [20.0]
self.experimentparams.home.enabled = False
self.experimentparams.home.x = rospy.get_param('motorarm/L1', 999)
self.experimentparams.home.y = 0.0
self.experimentparams.home.speed = 20
self.experimentparams.home.tolerance = 4
self.experimentparams.pre.robot.enabled = False
self.experimentparams.pre.lasergalvos.enabled = False
self.experimentparams.pre.ledpanels.enabled = False
self.experimentparams.pre.wait1 = 0
self.experimentparams.pre.trigger.enabled = False
self.experimentparams.pre.wait2 = 0.0
# .robot, .lasergalvos, .ledpanels, and .post.trigger all run concurrently.
# The first one to finish preempts the others.
self.experimentparams.trial.robot.enabled = False
self.experimentparams.trial.lasergalvos.enabled = True
self.experimentparams.trial.lasergalvos.statefilterLo_list = []
self.experimentparams.trial.lasergalvos.statefilterHi_list = []
self.experimentparams.trial.lasergalvos.statefilterCriteria_list = []
iFly = 1
self.experimentparams.trial.lasergalvos.pattern_list = []
self.experimentparams.trial.lasergalvos.pattern_list.append(
MsgPattern(
frameidPosition='Fly%02d' % iFly,
frameidAngle='Fly%02d' % iFly,
shape='grid',
hzPattern=40.0,
hzPoint=1000.0,
count=1,
points=[],
size=Point(x=3, y=3),
restart=False,
param=3, # Peano curve level.
direction=1), )
self.experimentparams.trial.ledpanels.enabled = False
self.experimentparams.post.trigger.enabled = True
self.experimentparams.post.trigger.frameidParent = 'Fly01'
self.experimentparams.post.trigger.frameidChild = 'Robot'
self.experimentparams.post.trigger.speedAbsParentMin = 0.0
self.experimentparams.post.trigger.speedAbsParentMax = 999.0
self.experimentparams.post.trigger.speedAbsChildMin = 0.0
self.experimentparams.post.trigger.speedAbsChildMax = 999.0
self.experimentparams.post.trigger.speedRelMin = 0.0
self.experimentparams.post.trigger.speedRelMax = 999.0
self.experimentparams.post.trigger.distanceMin = 999.0
self.experimentparams.post.trigger.distanceMax = 111.0 # Never
self.experimentparams.post.trigger.angleMin = 0.0 * np.pi / 180.0
self.experimentparams.post.trigger.angleMax = 180.0 * np.pi / 180.0
self.experimentparams.post.trigger.angleTest = 'inclusive'
self.experimentparams.post.trigger.angleTestBilateral = True
self.experimentparams.post.trigger.timeHold = 0.0
self.experimentparams.post.trigger.timeout = 600
self.experimentparams.post.wait = 0.0
self.experimentlib = ExperimentLib.ExperimentLib(
self.experimentparams,
startexperiment_callback=self.StartExperiment_callback,
starttrial_callback=self.StartTrial_callback,
endtrial_callback=self.EndTrial_callback)
def Main(self):
rospy.sleep(2)
msgPattern = MsgPattern()
# Publish a goto(0,0) pattern message, and wait for the end effector to initialize.
msgPattern.frameidPosition = 'Stage'
msgPattern.frameidAngle = 'Stage'
msgPattern.shape = 'constant'
msgPattern.hzPattern = 1.0
msgPattern.hzPoint = rospy.get_param('actuator/hzPoint', 100.0)
msgPattern.count = 1
msgPattern.size = Point(x=0, y=0)
msgPattern.points = []
msgPattern.restart = True
msgPattern.param = 0
self.pubSetPattern.publish(msgPattern)
rospy.sleep(2)
self.initialized = True
# Setup the calibration pattern.
msgPattern.frameidPosition = 'Stage'
msgPattern.frameidAngle = 'Stage'
msgPattern.shape = rospy.get_param('calibration/shape', 'spiral')
if msgPattern.shape == 'spiral':
msgPattern.hzPattern = 0.008
else:
msgPattern.hzPattern = 0.1
msgPattern.count = -1
msgPattern.points = []
msgPattern.size = Point(x=0.9 *
rospy.get_param('arena/radius_inner', 25.4),
y=0)
msgPattern.restart = True
msgPattern.param = 0.0
msgPattern.direction = 1
try:
while (not rospy.is_shutdown()):
t0 = rospy.Time.now().to_sec()
msgPattern.direction *= -1
self.pubSetPattern.publish(msgPattern)
while (rospy.Time.now().to_sec() - t0 < 60):
self.ProcessImage()
except Exception:
pass
# Publish a goto(0,0) pattern message.
msgPattern.frameidPosition = 'Stage'
msgPattern.frameidAngle = 'Stage'
msgPattern.shape = 'constant'
msgPattern.hzPattern = 1.0
msgPattern.count = 1
msgPattern.size = Point(x=0, y=0)
msgPattern.points = []
msgPattern.restart = True
msgPattern.param = 0
self.pubSetPattern.publish(msgPattern)
def __init__(self):
rospy.init_node('Experiment')
# Fill out the data structure that defines the experiment.
self.experimentparams = ExperimentParamsChoicesRequest()
self.experimentparams.experiment.description = 'Aim laser at fly'
self.experimentparams.experiment.maxTrials = -1
self.experimentparams.experiment.timeout = -1
self.experimentparams.save.filenamebase = 'zap'
self.experimentparams.save.csv = True
self.experimentparams.save.bag = False
self.experimentparams.save.mov = False
self.experimentparams.save.fmf = False
self.experimentparams.save.imagetopic_list = ['camera/image_rect']
self.experimentparams.save.onlyWhileTriggered = True
self.experimentparams.robotspec.nRobots = 0
self.experimentparams.robotspec.width = 1.5875
self.experimentparams.robotspec.height = 1.5875
self.experimentparams.robotspec.isPresent = True # Set this to False if you remove the robot, but still want the actuation.
self.experimentparams.robotspec.description = 'Black oxide magnet'
self.experimentparams.flyspec.nFlies = 1
self.experimentparams.flyspec.description = 'unspecified'
self.experimentparams.tracking.exclusionzones.enabled = False
self.experimentparams.tracking.exclusionzones.point_list = [
Point(x=0.0, y=0.0)
]
self.experimentparams.tracking.exclusionzones.radius_list = [0.0]
self.experimentparams.home.enabled = False
self.experimentparams.pre.robot.enabled = False
self.experimentparams.pre.lasergalvos.enabled = False
self.experimentparams.pre.ledpanels.enabled = False
self.experimentparams.pre.wait1 = 0.0
self.experimentparams.pre.trigger.enabled = False
self.experimentparams.pre.trigger.frameidParent = 'Arena'
self.experimentparams.pre.trigger.frameidChild = 'Fly01'
self.experimentparams.pre.trigger.speedAbsParentMin = 0.0
self.experimentparams.pre.trigger.speedAbsParentMax = 999.0
self.experimentparams.pre.trigger.speedAbsChildMin = 0.0
self.experimentparams.pre.trigger.speedAbsChildMax = 999.0
self.experimentparams.pre.trigger.speedRelMin = 0.0
self.experimentparams.pre.trigger.speedRelMax = 999.0
self.experimentparams.pre.trigger.distanceMin = 0.0
self.experimentparams.pre.trigger.distanceMax = 999.0
self.experimentparams.pre.trigger.angleMin = 0.0 * np.pi / 180.0
self.experimentparams.pre.trigger.angleMax = 180.0 * np.pi / 180.0
self.experimentparams.pre.trigger.angleTest = 'inclusive'
self.experimentparams.pre.trigger.angleTestBilateral = True
self.experimentparams.pre.trigger.timeHold = 0.0
self.experimentparams.pre.trigger.timeout = -1
self.experimentparams.pre.wait2 = 0.0
# .robot, .lasergalvos, .ledpanels, and .post.trigger all run concurrently.
# The first one to finish preempts the others.
self.experimentparams.trial.robot.enabled = False
mode = 'platepoint' # 'platepoint' or 'plategrid' or 'trackgrid1' or 'trackgrid' or 'tracknumber' or 'trackflylogo' or 'fixedmaze' or 'fixedpoint'
self.experimentparams.trial.lasergalvos.enabled = True
self.experimentparams.trial.lasergalvos.pattern_list = []
if mode == 'platepoint':
# Draw a point.
self.experimentparams.trial.lasergalvos.pattern_list.append(
MsgPattern(frameidPosition='Arena',
frameidAngle='Arena',
shape='constant',
hzPattern=40.0,
hzPoint=1000.0,
count=1,
size=Point(x=0, y=0),
restart=False,
param=2,
direction=1), # Peano curve level.
)
if mode == 'plategrid':
self.experimentparams.trial.lasergalvos.pattern_list.append(
MsgPattern(frameidPosition='Arena',
frameidAngle='Arena',
shape='grid',
hzPattern=40.0,
hzPoint=1000.0,
count=1,
size=Point(x=1, y=1),
restart=False,
param=3,
direction=1), # Peano curve level.
)
if mode == 'trackgrid1':
self.experimentparams.trial.lasergalvos.pattern_list.append(
MsgPattern(frameidPosition='Fly01',
frameidAngle='Fly01',
shape='grid',
hzPattern=40.0,
hzPoint=1000.0,
count=1,
size=Point(x=3, y=3),
restart=False,
param=2,
direction=1), # Peano curve level.
)
if mode == 'trackgrid':
for iFly in range(self.experimentparams.flyspec.nFlies):
self.experimentparams.trial.lasergalvos.pattern_list.append(
MsgPattern(frameidPosition='Fly%02dForecast' % (iFly + 1),
frameidAngle='Fly%02dForecast' % (iFly + 1),
shape='grid',
hzPattern=40.0,
hzPoint=1000.0,
count=1,
size=Point(x=3, y=3),
restart=False,
param=2,
direction=1), # Peano curve level.
)
if mode == 'tracknumber':
for iFly in range(self.experimentparams.flyspec.nFlies):
self.experimentparams.trial.lasergalvos.pattern_list.append(
MsgPattern(frameidPosition='Fly%02dForecast' % (iFly + 1),
frameidAngle='Fly%02dForecast' % (iFly + 1),
shape='%s' % (iFly + 1),
hzPattern=40.0,
hzPoint=1000.0,
count=1,
size=Point(x=6, y=6),
restart=False,
param=2,
direction=1), # Peano curve level.
)
if mode == 'trackflylogo':
for iFly in range(self.experimentparams.flyspec.nFlies):
self.experimentparams.trial.lasergalvos.pattern_list.append(
MsgPattern(frameidPosition='Fly%02dForecast' % (iFly + 1),
frameidAngle='Fly%02dForecast' % (iFly + 1),
shape='flylogo',
hzPattern=40.0,
hzPoint=1000.0,
count=1,
size=Point(x=6, y=6),
restart=False,
param=2,
direction=1), # Peano curve level.
)
if mode == 'fixedmaze':
# Draw a maze.
self.experimentparams.trial.lasergalvos.pattern_list.append(
MsgPattern(frameidPosition='Arena',
frameidAngle='Arena',
shape='grid',
hzPattern=40.0,
hzPoint=1000.0,
count=1,
size=Point(x=140, y=140),
restart=False,
param=2,
direction=1), # Peano curve level.
)
if mode == 'fixedpoint':
# Draw a point.
self.experimentparams.trial.lasergalvos.pattern_list.append(
MsgPattern(frameidPosition='Arena',
frameidAngle='Arena',
shape='constant',
hzPattern=40.0,
hzPoint=1000.0,
count=1,
size=Point(x=0, y=0),
restart=False,
param=2,
direction=1), # Peano curve level.
)
self.experimentparams.trial.ledpanels.enabled = False
self.experimentparams.trial.ledpanels.command = [
'fixed'
] # 'fixed', 'trackposition' (panel position follows fly position), or 'trackview' (panel position follows fly's viewpoint).
self.experimentparams.trial.ledpanels.idPattern = [1]
self.experimentparams.trial.ledpanels.frame_id = ['Fly01Forecast']
self.experimentparams.trial.ledpanels.statefilterHi = ['']
self.experimentparams.trial.ledpanels.statefilterLo = ['']
self.experimentparams.trial.ledpanels.statefilterCriteria = ['']
self.experimentparams.post.trigger.enabled = True
self.experimentparams.post.trigger.distanceMin = 999.0
self.experimentparams.post.trigger.distanceMax = 888.0 # i.e. never
self.experimentparams.post.trigger.speedAbsParentMin = 0.0
self.experimentparams.post.trigger.speedAbsParentMax = 999.0
self.experimentparams.post.trigger.speedAbsChildMin = 0.0
self.experimentparams.post.trigger.speedAbsChildMax = 999.0
self.experimentparams.post.trigger.speedRelMin = 0.0
self.experimentparams.post.trigger.speedRelMax = 999.0
self.experimentparams.post.trigger.angleMin = 0.0 * np.pi / 180.0
self.experimentparams.post.trigger.angleMax = 180.0 * np.pi / 180.0
self.experimentparams.post.trigger.angleTest = 'inclusive'
self.experimentparams.post.trigger.angleTestBilateral = True
self.experimentparams.post.trigger.timeHold = 0.0
self.experimentparams.post.trigger.timeout = 660
self.experimentparams.post.wait = 0.0
self.experimentlib = ExperimentLib.ExperimentLib(
self.experimentparams,
startexperiment_callback=self.StartExperiment_callback,
starttrial_callback=self.StartTrial_callback,
endtrial_callback=self.EndTrial_callback)
def __init__(self):
rospy.init_node('Experiment')
# Fill out the data structure that defines the experiment.
self.experimentparams = ExperimentParamsChoicesRequest()
self.experimentparams.experiment.description = 'See how long the fly lives with Laser (200mW)'
self.experimentparams.experiment.maxTrials = -1
self.experimentparams.experiment.timeout = -1
self.experimentparams.save.filenamebase = 'HCS_trytokill_'
self.experimentparams.save.csv = True
self.experimentparams.save.bag = False
self.experimentparams.save.mov = False
self.experimentparams.save.fmf = False
self.experimentparams.save.imagetopic_list = ['camera/image_rect']
self.experimentparams.save.onlyWhileTriggered = False
self.experimentparams.robotspec.nRobots = 0
self.experimentparams.robotspec.width = 1.5875
self.experimentparams.robotspec.height = 1.5875
self.experimentparams.robotspec.isPresent = True # Set this to False if you remove the robot, but still want the actuation.
self.experimentparams.robotspec.description = 'Black oxide magnet'
self.experimentparams.flyspec.nFlies = 1
self.experimentparams.flyspec.description = 'HCS normal'
self.experimentparams.tracking.exclusionzones.enabled = False
self.experimentparams.tracking.exclusionzones.point_list = [Point(x=0.00304053, y=0.00015492)]
self.experimentparams.tracking.exclusionzones.radius_list = [0]
self.experimentparams.home.enabled = False
self.experimentparams.pre.robot.enabled = False
self.experimentparams.pre.lasergalvos.enabled = False
self.experimentparams.pre.ledpanels.enabled = False
self.experimentparams.pre.wait1 = 0.0
self.experimentparams.pre.trigger.enabled = False
self.experimentparams.pre.trigger.frameidParent = 'Arena'
self.experimentparams.pre.trigger.frameidChild = 'Fly01'
self.experimentparams.pre.trigger.speedAbsParentMin = 0.0
self.experimentparams.pre.trigger.speedAbsParentMax = 999.0
self.experimentparams.pre.trigger.speedAbsChildMin = 0.0
self.experimentparams.pre.trigger.speedAbsChildMax = 999.0
self.experimentparams.pre.trigger.speedRelMin = 0.0
self.experimentparams.pre.trigger.speedRelMax = 999.0
self.experimentparams.pre.trigger.distanceMin = 0.0
self.experimentparams.pre.trigger.distanceMax = 999.0
self.experimentparams.pre.trigger.angleMin = 0.0 * np.pi / 180.0
self.experimentparams.pre.trigger.angleMax =180.0 * np.pi / 180.0
self.experimentparams.pre.trigger.angleTest = 'inclusive'
self.experimentparams.pre.trigger.angleTestBilateral = True
self.experimentparams.pre.trigger.timeHold = 0.0
self.experimentparams.pre.trigger.timeout = -1
self.experimentparams.pre.wait2 = 0.0
# .robot, .lasergalvos, .ledpanels, and .post.trigger all run concurrently.
# The first one to finish preempts the others.
self.experimentparams.trial.robot.enabled = False
#mode='fixedpointlist' # Laser to specific locations.
#mode='fixedcircle'
#mode='fixedgrid'
mode='trackgrid' # Small grid tracks flies.
#mode='tracknumber' # Draw a numeral on flies.
#mode='trackflylogo'
flies_list = range(1,1+self.experimentparams.flyspec.nFlies)
self.experimentparams.trial.lasergalvos.enabled = True
self.experimentparams.trial.lasergalvos.pattern_list = []
self.experimentparams.trial.lasergalvos.statefilterLo_list = []
self.experimentparams.trial.lasergalvos.statefilterHi_list = []
self.experimentparams.trial.lasergalvos.statefilterCriteria_list = []
if mode=='fixedpointlist':
# Draw a point.
self.experimentparams.trial.lasergalvos.pattern_list.append(MsgPattern(
frameidPosition = 'Arena',
frameidAngle = 'Arena',
shape = 'bypoints',
hzPattern = 40.0,
hzPoint = 1000.0,
count = 1,
points = [#Point(x=0 ,y=0)],
Point(x=34 ,y=34),
Point(x=34 ,y=34+6),
Point(x=34 ,y=34-6),
Point(x=34+6,y=34),
Point(x=34-6,y=34)],
size = Point(x=0,
y=0),
restart = False,
param = 3, # Peano curve level.
direction = 1),
)
if mode=='fixedcircle':
# Draw a point.
self.experimentparams.trial.lasergalvos.pattern_list.append(MsgPattern(
frameidPosition = 'Arena',
frameidAngle = 'Arena',
shape = 'circle',
hzPattern = 40.0,
hzPoint = 1000.0,
count = 1,
size = Point(x=80,
y= 0),
restart = False,
param = 3,
direction = 1), # Peano curve level.
)
if mode=='fixedgrid':
# Draw a maze.
self.experimentparams.trial.lasergalvos.pattern_list.append(MsgPattern(
frameidPosition = 'Arena',
frameidAngle = 'Arena',
shape = 'grid',
hzPattern = 40.0,
hzPoint = 1000.0,
count = 1,
size = Point(x=0,
y=0),
restart = False,
param = 3, # Peano curve level.
direction = 1),
)
if mode=='trackgrid':
for iFly in flies_list:
self.experimentparams.trial.lasergalvos.pattern_list.append(MsgPattern(
frameidPosition = 'Fly%02dForecast' % iFly,
frameidAngle = 'Fly%02dForecast' % iFly,
shape = 'grid',
hzPattern = 40.0,
hzPoint = 1000.0,
count = 1,
size = Point(x=2,
y=2),
restart = False,
param = 3, # Peano curve level.
direction = 1),
)
if mode=='tracknumber':
for iFly in flies_list:
self.experimentparams.trial.lasergalvos.pattern_list.append(MsgPattern(
frameidPosition = 'Fly%02dForecast' % iFly,
frameidAngle = 'Fly%02dForecast' % iFly,
shape = '%s' % iFly,
hzPattern = 10.0,
hzPoint = 1000.0,
count = 1,
size = Point(x=8,
y=8),
restart = False,
param = 0,
direction = 1), # Peano curve level.
)
if mode=='trackflylogo':
for iFly in flies_list:
self.experimentparams.trial.lasergalvos.pattern_list.append(MsgPattern(
frameidPosition = 'Fly%02dForecast' % iFly,
frameidAngle = 'Fly%02dForecast' % iFly,
shape = 'flylogo',
hzPattern = 40.0,
hzPoint = 1000.0,
count = 1,
size = Point(x=6,
y=6),
restart = False,
param = 0,
direction = 1), # Peano curve level.
)
self.experimentparams.trial.ledpanels.enabled = False
self.experimentparams.trial.ledpanels.command = ['fixed'] # 'fixed', 'trackposition' (panel position follows fly position), or 'trackview' (panel position follows fly's viewpoint).
self.experimentparams.trial.ledpanels.idPattern = [1]
self.experimentparams.trial.ledpanels.frame_id = ['Fly01Forecast']
self.experimentparams.trial.ledpanels.statefilterHi = ['']
self.experimentparams.trial.ledpanels.statefilterLo = ['']
self.experimentparams.trial.ledpanels.statefilterCriteria = ['']
self.experimentparams.post.trigger.enabled = True
self.experimentparams.post.trigger.frameidParent = 'Arena'
self.experimentparams.post.trigger.frameidChild = 'Fly01'
self.experimentparams.post.trigger.speedAbsParentMin = 0.0
self.experimentparams.post.trigger.speedAbsParentMax = 999.0
self.experimentparams.post.trigger.speedAbsChildMin = 0.0
self.experimentparams.post.trigger.speedAbsChildMax = 999.0
self.experimentparams.post.trigger.speedRelMin = 0.0
self.experimentparams.post.trigger.speedRelMax = 999.0
self.experimentparams.post.trigger.distanceMin = 999.0
self.experimentparams.post.trigger.distanceMax = 888.0 # i.e. never
self.experimentparams.post.trigger.angleMin = 0.0 * np.pi / 180.0
self.experimentparams.post.trigger.angleMax =180.0 * np.pi / 180.0
self.experimentparams.post.trigger.angleTest = 'inclusive'
self.experimentparams.post.trigger.angleTestBilateral = True
self.experimentparams.post.trigger.timeHold = 0.0
self.experimentparams.post.trigger.timeout = 1800
self.experimentparams.post.wait = 0.0
self.experimentlib = ExperimentLib.ExperimentLib(self.experimentparams,
startexperiment_callback = self.StartExperiment_callback,
starttrial_callback = self.StartTrial_callback,
endtrial_callback = self.EndTrial_callback)
def __init__(self):
rospy.init_node('Experiment')
# Fill out the data structure that defines the experiment.
self.experimentparams = ExperimentParamsChoicesRequest()
self.experimentparams.experiment.description = 'Food is in the hot zone.'
self.experimentparams.experiment.maxTrials = -1
self.experimentparams.experiment.timeout = -1
self.experimentparams.save.filenamebase = 'zaphotfood'
self.experimentparams.save.csv = True
self.experimentparams.save.bag = False
self.experimentparams.save.mov = False
self.experimentparams.save.fmf = False
self.experimentparams.save.imagetopic_list = ['camera/image_rect']
self.experimentparams.save.onlyWhileTriggered = False # Saves always.
self.experimentparams.robotspec.nRobots = 0
self.experimentparams.robotspec.width = 1.5875
self.experimentparams.robotspec.height = 1.5875
self.experimentparams.robotspec.isPresent = True # Set this to False if you remove the robot, but still want the actuation.
self.experimentparams.robotspec.description = 'Black oxide magnet'
self.experimentparams.flyspec.nFlies = 1
self.experimentparams.flyspec.description = 'unspecified'
self.experimentparams.tracking.exclusionzones.enabled = True
self.experimentparams.tracking.exclusionzones.point_list = [
Point(x=52.3, y=-51.0)
]
self.experimentparams.tracking.exclusionzones.radius_list = [7.0]
self.experimentparams.home.enabled = False
self.experimentparams.pre.robot.enabled = False
self.experimentparams.pre.lasergalvos.enabled = False
self.experimentparams.pre.ledpanels.enabled = False
self.experimentparams.pre.wait1 = 0.0
self.experimentparams.pre.trigger.enabled = False
self.experimentparams.pre.trigger.frameidParent = 'Arena'
self.experimentparams.pre.trigger.frameidChild = 'Fly01'
self.experimentparams.pre.trigger.speedAbsParentMin = 0.0
self.experimentparams.pre.trigger.speedAbsParentMax = 999.0
self.experimentparams.pre.trigger.speedAbsChildMin = 0.0
self.experimentparams.pre.trigger.speedAbsChildMax = 999.0
self.experimentparams.pre.trigger.speedRelMin = 0.0
self.experimentparams.pre.trigger.speedRelMax = 999.0
self.experimentparams.pre.trigger.distanceMin = 0.0
self.experimentparams.pre.trigger.distanceMax = 999.0
self.experimentparams.pre.trigger.angleMin = 0.0 * np.pi / 180.0
self.experimentparams.pre.trigger.angleMax = 180.0 * np.pi / 180.0
self.experimentparams.pre.trigger.angleTest = 'inclusive'
self.experimentparams.pre.trigger.angleTestBilateral = False
self.experimentparams.pre.trigger.timeHold = 0.0
self.experimentparams.pre.trigger.timeout = -1
self.experimentparams.pre.wait2 = 0.0
# .robot, .lasergalvos, .ledpanels, and .post.trigger all run concurrently.
# The first one to finish preempts the others.
self.experimentparams.trial.robot.enabled = False
self.experimentparams.trial.lasergalvos.enabled = True
self.experimentparams.trial.lasergalvos.pattern_list = []
self.experimentparams.trial.lasergalvos.statefilterHi_list = []
self.experimentparams.trial.lasergalvos.statefilterLo_list = []
self.experimentparams.trial.lasergalvos.statefilterCriteria_list = []
for iFly in range(self.experimentparams.flyspec.nFlies): #range(3):#
self.experimentparams.trial.lasergalvos.pattern_list.append(
MsgPattern(frameidPosition='Fly%02dForecast' % (iFly + 1),
frameidAngle='Fly%02dForecast' % (iFly + 1),
shape='grid',
hzPattern=40.0,
hzPoint=1000.0,
count=1,
size=Point(x=2, y=2),
restart=False,
param=3,
direction=1), # Peano curve level.
)
#self.experimentparams.trial.lasergalvos.statefilterHi_list.append("{'speed':5.0}")
#self.experimentparams.trial.lasergalvos.statefilterLo_list.append("{'speed':0.0}")
#self.experimentparams.trial.lasergalvos.statefilterHi_list.append("{'velocity':{'linear':{'x':+6,'y':+6}}}")
#self.experimentparams.trial.lasergalvos.statefilterLo_list.append("{'velocity':{'linear':{'x':-6,'y':-6}}}")
#self.experimentparams.trial.lasergalvos.statefilterHi_list.append("{'velocity':{'angular':{'z':999}}}")
#self.experimentparams.trial.lasergalvos.statefilterLo_list.append("{'velocity':{'angular':{'z':0.5}}}")
self.experimentparams.trial.lasergalvos.statefilterHi_list.append(
"{'pose':{'position':{'x':-30, 'y':50}}}"
) # This is the cool zone.
self.experimentparams.trial.lasergalvos.statefilterLo_list.append(
"{'pose':{'position':{'x':-50, 'y':30}}}")
self.experimentparams.trial.lasergalvos.statefilterCriteria_list.append(
"exclusive")
self.experimentparams.trial.ledpanels.enabled = False
self.experimentparams.trial.ledpanels.command = [
'fixed'
] # 'fixed', 'trackposition' (panel position follows fly position), or 'trackview' (panel position follows fly's viewpoint).
self.experimentparams.trial.ledpanels.idPattern = [1]
self.experimentparams.trial.ledpanels.frame_id = ['Fly01Forecast']
self.experimentparams.trial.ledpanels.statefilterHi = ['']
self.experimentparams.trial.ledpanels.statefilterLo = ['']
self.experimentparams.trial.ledpanels.statefilterCriteria = ['']
self.experimentparams.post.trigger.enabled = True
self.experimentparams.post.trigger.frameidParent = 'Arena'
self.experimentparams.post.trigger.frameidChild = 'Fly01'
self.experimentparams.post.trigger.speedAbsParentMin = 0.0
self.experimentparams.post.trigger.speedAbsParentMax = 999.0
self.experimentparams.post.trigger.speedAbsChildMin = 0.0
self.experimentparams.post.trigger.speedAbsChildMax = 999.0
self.experimentparams.post.trigger.speedRelMin = 0.0
self.experimentparams.post.trigger.speedRelMax = 999.0
self.experimentparams.post.trigger.distanceMin = 999.0
self.experimentparams.post.trigger.distanceMax = 111.0 # i.e. never
self.experimentparams.post.trigger.angleMin = 0.0000 * np.pi / 180.0
self.experimentparams.post.trigger.angleMax = 359.9999 * np.pi / 180.0
self.experimentparams.post.trigger.angleTest = 'inclusive'
self.experimentparams.post.trigger.angleTestBilateral = False
self.experimentparams.post.trigger.timeHold = 0.0
self.experimentparams.post.trigger.timeout = 3600
self.experimentparams.post.wait = 0.0
self.experimentlib = ExperimentLib.ExperimentLib(
self.experimentparams,
startexperiment_callback=self.StartExperiment_callback,
starttrial_callback=self.StartTrial_callback,
endtrial_callback=self.EndTrial_callback)
def execute(self, userdata):
rospy.loginfo('EL State ResetRobot(%s)' % [
userdata.experimentparamsChoicesIn.home.enabled,
userdata.experimentparamsChoicesIn.home.x,
userdata.experimentparamsChoicesIn.home.y
])
rv = 'disabled'
if (userdata.experimentparamsChoicesIn.trial.robot.enabled
or userdata.experimentparamsChoicesIn.pre.robot.enabled) and (
userdata.experimentparamsChoicesIn.home.enabled):
self.target = MsgFrameState()
self.timeStart = rospy.Time.now()
if (self.SetStageStateRef is None):
stSrv = 'set_stage_state_ref'
try:
rospy.wait_for_service(stSrv)
self.SetStageStateRef = rospy.ServiceProxy(stSrv,
SrvFrameState,
persistent=True)
except rospy.ServiceException, e:
rospy.logwarn('EL FAILED to connect service %s(): %s' %
(stSrv, e))
while self.arenastate is None:
#if userdata.experimentparamsChoicesIn.home.timeout != -1:
# if (rospy.Time.now().to_sec()-self.timeStart.to_sec()) > userdata.experimentparamsChoicesIn.home.timeout:
# return 'timeout'
#if self.preempt_requested():
# self.service_preempt()
# return 'preempt'
rospy.sleep(1.0)
if (self.commandExperiment == 'exit_now'):
return 'aborted'
# Turn off any prior pattern generation.
msgPattern = MsgPattern()
msgPattern.shape = 'constant'
msgPattern.points = []
msgPattern.frameidPosition = 'Arena'
msgPattern.frameidAngle = 'Arena'
msgPattern.hzPattern = 1.0
msgPattern.hzPoint = 50
msgPattern.count = 0
msgPattern.size = Point(0.0, 0.0, 0.0)
msgPattern.restart = False
msgPattern.param = 0.0
msgPattern.direction = 1
self.pubSetPattern.publish(msgPattern)
rospy.sleep(1)
# Send the 'home' command.
self.target.header = self.arenastate.robot.header
#self.target.header.stamp = rospy.Time.now()
self.target.pose.position.x = userdata.experimentparamsChoicesIn.home.x
self.target.pose.position.y = userdata.experimentparamsChoicesIn.home.y
try:
self.SetStageStateRef(
SrvFrameStateRequest(state=MsgFrameState(
header=self.target.header,
pose=self.target.pose,
speed=userdata.experimentparamsChoicesIn.home.speed)))
except rospy.ServiceException, e:
rospy.logwarn(
'EL FAILED service call to set_stage_state_ref().')
def MovePattern (self):
msgPattern = MsgPattern()
# Choose one from each set of choices at random.
msgPattern.shape = self.paramsIn.robot.move.pattern.shape
msgPattern.points = []
msgPattern.frameidPosition = self.paramsIn.robot.move.pattern.frameidPosition
msgPattern.frameidAngle = self.paramsIn.robot.move.pattern.frameidAngle
msgPattern.hzPattern = self.paramsIn.robot.move.pattern.hzPattern
msgPattern.hzPoint = self.paramsIn.robot.move.pattern.hzPoint
msgPattern.count = self.paramsIn.robot.move.pattern.count
msgPattern.size = self.paramsIn.robot.move.pattern.size
msgPattern.restart = self.paramsIn.robot.move.pattern.restart
msgPattern.param = 0.0#self.paramsIn.robot.move.pattern.param
msgPattern.direction = self.paramsIn.robot.move.pattern.direction
self.pubSetPattern.publish (msgPattern)
rv = 'aborted'
while not rospy.is_shutdown():
self.step_time_elapsed()
if self.preempt_requested():
rospy.loginfo('preempt requested: MovePattern()')
self.service_preempt()
rv = 'preempt'
break
# 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
self.rosrate.sleep()
# Turn off the pattern
msgPattern.count = 0
self.pubSetPattern.publish (msgPattern)
return rv
def Main(self):
rospy.sleep(2)
msgPattern = MsgPattern()
# Publish a goto(0,0) pattern message, and wait for the end effector to initialize.
msgPattern.frameidPosition = 'Stage'
msgPattern.frameidAngle = 'Stage'
msgPattern.shape = 'constant'
msgPattern.hzPattern = 1.0
msgPattern.hzPoint = rospy.get_param('actuator/hzPoint', 100.0)
msgPattern.count = 1
msgPattern.size = Point(x=0,y=0)
msgPattern.points = []
msgPattern.restart = True
msgPattern.param = 0
self.pubSetPattern.publish (msgPattern)
rospy.sleep(2)
self.initialized = True
# Setup the calibration pattern.
msgPattern.frameidPosition = 'Stage'
msgPattern.frameidAngle = 'Stage'
msgPattern.shape = rospy.get_param('calibration/shape', 'spiral')
if msgPattern.shape=='spiral':
msgPattern.hzPattern = 0.008
else:
msgPattern.hzPattern = 0.1
msgPattern.count = -1
msgPattern.points = []
msgPattern.size = Point(x=0.9*rospy.get_param('arena/radius_inner', 25.4), y=0)
msgPattern.restart = True
msgPattern.param = 0.0
msgPattern.direction = 1
try:
while (not rospy.is_shutdown()):
t0 = rospy.Time.now().to_sec()
msgPattern.direction *= -1
self.pubSetPattern.publish (msgPattern)
while (rospy.Time.now().to_sec()-t0 < 60):
self.ProcessImage()
except Exception:
pass
# Publish a goto(0,0) pattern message.
msgPattern.frameidPosition = 'Stage'
msgPattern.frameidAngle = 'Stage'
msgPattern.shape = 'constant'
msgPattern.hzPattern = 1.0
msgPattern.count = 1
msgPattern.size = Point(x=0,y=0)
msgPattern.points = []
msgPattern.restart = True
msgPattern.param = 0
self.pubSetPattern.publish (msgPattern)
def Main(self):
rospy.sleep(2)
msgPattern = MsgPattern()
# Publish a goto(0,0) pattern message, and wait for the end effector to initialize.
msgPattern.frameidPosition = 'Stage'
msgPattern.frameidAngle = 'Stage'
msgPattern.shape = 'constant'
msgPattern.hzPattern = 1.0
msgPattern.hzPoint = rospy.get_param('actuator/hzPoint', 100.0)
msgPattern.count = 1
msgPattern.size = Point(x=0,y=0)
msgPattern.points = []
msgPattern.preempt = True
msgPattern.param = 0
self.pubSetPattern.publish (msgPattern)
rospy.sleep(2)
self.initialized = True
# Publish the calibration pattern.
msgPattern.frameidPosition = 'Stage'
msgPattern.frameidAngle = 'Stage'
msgPattern.shape = rospy.get_param('calibration/shape', 'spiral')
if msgPattern.shape=='spiral':
msgPattern.hzPattern = 0.008
else:
msgPattern.hzPattern = 0.1
msgPattern.count = -1
msgPattern.points = []
msgPattern.size = Point(x=0.9*rospy.get_param('arena/radius_inner', 25.4), y=0)
msgPattern.preempt = True
msgPattern.param = 0.0
msgPattern.direction = 1
self.pubSetPattern.publish (msgPattern)
rospy.spin()
# Publish a goto(0,0) pattern message.
msgPattern.frameidPosition = 'Stage'
msgPattern.frameidAngle = 'Stage'
msgPattern.shape = 'constant'
msgPattern.hzPattern = 1.0
msgPattern.count = 1
msgPattern.size = Point(x=0,y=0)
msgPattern.points = []
msgPattern.preempt = True
msgPattern.param = 0
self.pubSetPattern.publish (msgPattern)