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.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)
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 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 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):
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()
# 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
