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