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 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 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 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.preempt = True
msgPattern.param = 0
self.pubSetPattern.publish (msgPattern)
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.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 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 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
