def SendInputPoints(self): pattern = MsgPattern() pattern.mode = 'bypoints' pattern.shape = 'constant' pattern.frame_id = 'Plate' pattern.hzPattern = 1.0 pattern.hzPoint = 100.0 pattern.count = 1 pattern.points = self.pointsInput pattern.radius = 20.0 pattern.preempt = False command = MsgGalvoCommand() command.frameid_target_list = ['Plate',] command.pattern_list = [pattern,] command.units = 'volts' #'millimeters' # 'volts' # self.pubGalvoCommand.publish(command)
def TrackFly1(self): pattern = MsgPattern() pattern.mode = 'byshape' pattern.shape = 'grid' pattern.frame_id = 'Fly1' pattern.hzPattern = 40.0 pattern.hzPoint = 1000.0 pattern.count = 1 pattern.points = [] pattern.radius = 5 pattern.preempt = False command = MsgGalvoCommand() command.frameid_target_list = ['Plate',] command.pattern_list = [pattern,] command.units = 'millimeters' # 'millimeters' or 'volts' self.pubGalvoCommand.publish(command)
def SendInputPoints(self): pattern = MsgPattern() pattern.frameidPosition = 'Arena' pattern.frameidAngle = 'Arena' pattern.shape = 'bypoints' pattern.hzPattern = 1.0 pattern.hzPoint = 100.0 pattern.count = 1 pattern.points = self.pointsInput pattern.size.x = 20.0 pattern.size.y = 20.0 pattern.restart = False pattern.param = 0.0 pattern.direction = 1 command = MsgGalvoCommand() command.pattern_list = [pattern,] command.units = 'volts' #'millimeters' # 'volts' # self.pubGalvodirectorCommand.publish(command)
def SendPoint(self, point): pattern = MsgPattern() pattern.frameidPosition = 'Arena' pattern.frameidAngle = 'Arena' pattern.shape = 'bypoints' pattern.hzPattern = 1.0 pattern.hzPoint = 100.0 pattern.count = 1 pattern.points = [point,] pattern.size.x = 0.0 pattern.size.y = 0.0 pattern.preempt = False pattern.param = 0.0 pattern.direction = 1 command = MsgGalvoCommand() command.enable_laser = True command.pattern_list = [pattern,] command.units = 'volts' #'millimeters' # 'volts' # self.pubGalvoCommand.publish(command)
def execute(self, userdata): self.experimentparams = copy.deepcopy(userdata.experimentparamsIn) self.timePrev = rospy.Time.now() if self.mode == 'pre': self.paramsIn = self.experimentparams.pre if self.mode == 'trial': self.paramsIn = self.experimentparams.trial for pattern in self.paramsIn.lasergalvos.pattern_list: rospy.loginfo("EL State Lasergalvos(%s)" % pattern) if self.paramsIn.lasergalvos.enabled: rv = 'aborted' self.timeStart = rospy.Time.now() # Create the tracking command for the galvo director. commandGalvo = MsgGalvoCommand() commandGalvo.enable_laser = True commandGalvo.units = 'millimeters' # 'millimeters' or 'volts' commandGalvo.pattern_list = self.paramsIn.lasergalvos.pattern_list # Determine if we're showing patterns only for certain states. nPatterns = len(self.paramsIn.lasergalvos.pattern_list) if len(self.paramsIn.lasergalvos.statefilterLo_list) == nPatterns and \ len(self.paramsIn.lasergalvos.statefilterHi_list) == nPatterns: isStatefiltered = True else: isStatefiltered = False if (len(self.paramsIn.lasergalvos.statefilterLo_list) != len(self.paramsIn.lasergalvos.statefilterHi_list)): rospy.logwarn('Check your experiment params structure: Number of statefilters does not match number of patterns. Not filtering states.') # Initialize statefilter vars. bInStatefilterRangePrev = [False for i in range(nPatterns)] bInStatefilterRange = [False for i in range(nPatterns)] # If unfiltered, publish the command. if not isStatefiltered: self.pubGalvoCommand.publish(commandGalvo) # Move galvos until preempt or timeout. while not rospy.is_shutdown(): self.step_time_elapsed() # If state is used to determine when pattern is shown. if isStatefiltered: # Check if any filterstates have changed. bFilterStateChanged = False for iPattern in range(nPatterns): # Get the pattern. pattern = self.paramsIn.lasergalvos.pattern_list[iPattern] # Convert strings to dicts. statefilterLo_dict = eval(self.paramsIn.lasergalvos.statefilterLo_list[iPattern]) statefilterHi_dict = eval(self.paramsIn.lasergalvos.statefilterHi_list[iPattern]) statefilterCriteria = self.paramsIn.lasergalvos.statefilterCriteria_list[iPattern] # If possible, take the pose &/or velocity &/or speed from arenastate, else use transform via ROS. pose = None velocity = None speed = None if self.arenastate is not None: if ('Robot' in pattern.frameidPosition): #pose = self.arenastate.robot.pose # For consistency w/ galvodirector, we'll get pose via transform. velocity = self.arenastate.robot.velocity speed = self.arenastate.robot.speed elif ('Fly' in pattern.frameidPosition): for iFly in range(len(self.arenastate.flies)): if (self.arenastate.flies[iFly].name in pattern.frameidPosition): #pose = self.arenastate.flies[iFly].pose # For consistency w/ galvodirector, we'll get pose via transform. velocity = self.arenastate.flies[iFly].velocity speed = self.arenastate.flies[iFly].speed break # Get the timestamp for transforms. stamp=None if (pose is None) or (velocity is None): try: stamp = self.tfrx.getLatestCommonTime('Arena', pattern.frameidPosition) except tf.Exception: pass # If we still need the pose (i.e. the frame wasn't in arenastate), then get it from ROS. if (pose is None) and (stamp is not None) and self.tfrx.canTransform('Arena', pattern.frameidPosition, stamp): try: poseStamped = self.tfrx.transformPose('Arena', PoseStamped(header=Header(stamp=stamp, frame_id=pattern.frameidPosition), pose=Pose(position=Point(0,0,0), orientation=Quaternion(0,0,0,1) ) ) ) pose = poseStamped.pose except tf.Exception: pose = None # If we still need the velocity, then get it from ROS. if (velocity is None) and (stamp is not None) and self.tfrx.canTransform('Arena', pattern.frameidPosition, stamp): try: velocity_tuple = self.tfrx.lookupTwist('Arena', pattern.frameidPosition, stamp, self.dtVelocity) except tf.Exception: velocity = None else: velocity = Twist(linear=Point(x=velocity_tuple[0][0], y=velocity_tuple[0][1], z=velocity_tuple[0][2]), angular=Point(x=velocity_tuple[1][0], y=velocity_tuple[1][1], z=velocity_tuple[1][2])) # If we still need the speed, then get it from velocity. if (speed is None) and (velocity is not None): speed = np.linalg.norm([velocity.linear.x, velocity.linear.y, velocity.linear.z]) # See if any of the states are in range of the filter. if (pose is not None) and (velocity is not None) and (speed is not None): state = MsgFrameState(pose = pose, velocity = velocity, speed = speed) bInStatefilterRangePrev[iPattern] = bInStatefilterRange[iPattern] bInStatefilterRange[iPattern] = self.Statefilter.InRange(state, statefilterLo_dict, statefilterHi_dict, statefilterCriteria) self.Statefilter.PublishMarkers (state, statefilterLo_dict, statefilterHi_dict, statefilterCriteria) # If any of the filter states have changed, then we need to update them all. if bInStatefilterRangePrev[iPattern] != bInStatefilterRange[iPattern]: bFilterStateChanged = True # end for iPattern in range(nPatterns) # If filter state has changed, then publish the new command if bFilterStateChanged: commandGalvo.pattern_list = [] for iPattern in range(nPatterns): if bInStatefilterRange[iPattern]: pattern = self.paramsIn.lasergalvos.pattern_list[iPattern] commandGalvo.pattern_list.append(pattern) if len(commandGalvo.pattern_list)>0: commandGalvo.enable_laser = True else: commandGalvo.enable_laser = False self.pubGalvoCommand.publish(commandGalvo) # else commandGalvo.pattern_list contains all patterns, and has already been published. if self.preempt_requested(): rospy.loginfo('preempt requested: Lasergalvos()') self.service_preempt() rv = 'preempt' break self.rosrate.sleep() # Handle commands. if (self.commandExperiment=='continue'): pass if (self.commandExperiment=='pause_now'): while (self.commandExperiment=='pause_now'): rospy.sleep(0.5) self.timePrev = rospy.Time.now() if (self.commandExperiment=='pause_after_trial'): pass if (self.commandExperiment=='exit_after_trial'): pass if (self.commandExperiment=='exit_now'): rv = 'aborted' break # end while() else: rv = 'disabled' # Turn off the laser. commandGalvo = MsgGalvoCommand() commandGalvo.enable_laser = False self.pubGalvoCommand.publish(commandGalvo) userdata.experimentparamsOut = self.experimentparams return rv
def execute(self, userdata): self.experimentparams = copy.deepcopy(userdata.experimentparamsIn) self.timePrev = rospy.Time.now() if self.mode == 'pre': self.paramsIn = self.experimentparams.pre if self.mode == 'trial': self.paramsIn = self.experimentparams.trial for pattern in self.paramsIn.lasergalvos.pattern_list: rospy.loginfo("EL State Lasergalvos(%s)" % pattern) if self.paramsIn.lasergalvos.enabled: rv = 'aborted' self.timeStart = rospy.Time.now() # Create the tracking command for the galvo director. commandGalvo = MsgGalvoCommand() commandGalvo.enable_laser = True commandGalvo.units = 'millimeters' # 'millimeters' or 'volts' commandGalvo.pattern_list = self.paramsIn.lasergalvos.pattern_list # Determine if we're showing patterns only for certain states. nPatterns = len(self.paramsIn.lasergalvos.pattern_list) if len(self.paramsIn.lasergalvos.statefilterLo_list) == nPatterns and \ len(self.paramsIn.lasergalvos.statefilterHi_list) == nPatterns: isStatefiltered = True else: isStatefiltered = False if (len(self.paramsIn.lasergalvos.statefilterLo_list) != len( self.paramsIn.lasergalvos.statefilterHi_list)): rospy.logwarn( 'Check your experiment params structure: Number of statefilters does not match number of patterns. Not filtering states.' ) # Initialize statefilter vars. bInStatefilterRangePrev = [False for i in range(nPatterns)] bInStatefilterRange = [False for i in range(nPatterns)] # If unfiltered, publish the command. if not isStatefiltered: self.pubGalvoCommand.publish(commandGalvo) # Move galvos until preempt or timeout. while not rospy.is_shutdown(): self.step_time_elapsed() # If state is used to determine when pattern is shown. if isStatefiltered: # Check if any filterstates have changed. bFilterStateChanged = False for iPattern in range(nPatterns): # Get the pattern. pattern = self.paramsIn.lasergalvos.pattern_list[ iPattern] # Convert strings to dicts. statefilterLo_dict = eval(self.paramsIn.lasergalvos. statefilterLo_list[iPattern]) statefilterHi_dict = eval(self.paramsIn.lasergalvos. statefilterHi_list[iPattern]) statefilterCriteria = self.paramsIn.lasergalvos.statefilterCriteria_list[ iPattern] # If possible, take the pose &/or velocity &/or speed from arenastate, else use transform via ROS. pose = None velocity = None speed = None if self.arenastate is not None: if ('Robot' in pattern.frameidPosition): #pose = self.arenastate.robot.pose # For consistency w/ galvodirector, we'll get pose via transform. velocity = self.arenastate.robot.velocity speed = self.arenastate.robot.speed elif ('Fly' in pattern.frameidPosition): for iFly in range(len(self.arenastate.flies)): if (self.arenastate.flies[iFly].name in pattern.frameidPosition): #pose = self.arenastate.flies[iFly].pose # For consistency w/ galvodirector, we'll get pose via transform. velocity = self.arenastate.flies[ iFly].velocity speed = self.arenastate.flies[ iFly].speed break # Get the timestamp for transforms. stamp = None if (pose is None) or (velocity is None): try: stamp = self.tfrx.getLatestCommonTime( 'Arena', pattern.frameidPosition) except tf.Exception: pass # If we still need the pose (i.e. the frame wasn't in arenastate), then get it from ROS. if (pose is None) and ( stamp is not None) and self.tfrx.canTransform( 'Arena', pattern.frameidPosition, stamp): try: poseStamped = self.tfrx.transformPose( 'Arena', PoseStamped(header=Header( stamp=stamp, frame_id=pattern.frameidPosition), pose=Pose( position=Point(0, 0, 0), orientation=Quaternion( 0, 0, 0, 1)))) pose = poseStamped.pose except tf.Exception: pose = None # If we still need the velocity, then get it from ROS. if (velocity is None) and ( stamp is not None) and self.tfrx.canTransform( 'Arena', pattern.frameidPosition, stamp): try: velocity_tuple = self.tfrx.lookupTwist( 'Arena', pattern.frameidPosition, stamp, self.dtVelocity) except tf.Exception: velocity = None else: velocity = Twist( linear=Point(x=velocity_tuple[0][0], y=velocity_tuple[0][1], z=velocity_tuple[0][2]), angular=Point(x=velocity_tuple[1][0], y=velocity_tuple[1][1], z=velocity_tuple[1][2])) # If we still need the speed, then get it from velocity. if (speed is None) and (velocity is not None): speed = np.linalg.norm([ velocity.linear.x, velocity.linear.y, velocity.linear.z ]) # See if any of the states are in range of the filter. if (pose is not None) and (velocity is not None) and ( speed is not None): state = MsgFrameState(pose=pose, velocity=velocity, speed=speed) bInStatefilterRangePrev[ iPattern] = bInStatefilterRange[iPattern] bInStatefilterRange[ iPattern] = self.Statefilter.InRange( state, statefilterLo_dict, statefilterHi_dict, statefilterCriteria) self.Statefilter.PublishMarkers( state, statefilterLo_dict, statefilterHi_dict, statefilterCriteria) # If any of the filter states have changed, then we need to update them all. if bInStatefilterRangePrev[ iPattern] != bInStatefilterRange[iPattern]: bFilterStateChanged = True # end for iPattern in range(nPatterns) # If filter state has changed, then publish the new command if bFilterStateChanged: commandGalvo.pattern_list = [] for iPattern in range(nPatterns): if bInStatefilterRange[iPattern]: pattern = self.paramsIn.lasergalvos.pattern_list[ iPattern] commandGalvo.pattern_list.append(pattern) if len(commandGalvo.pattern_list) > 0: commandGalvo.enable_laser = True else: commandGalvo.enable_laser = False self.pubGalvoCommand.publish(commandGalvo) # else commandGalvo.pattern_list contains all patterns, and has already been published. if self.preempt_requested(): rospy.loginfo('preempt requested: Lasergalvos()') self.service_preempt() rv = 'preempt' break self.rosrate.sleep() # Handle commands. if (self.commandExperiment == 'continue'): pass if (self.commandExperiment == 'pause_now'): while (self.commandExperiment == 'pause_now'): rospy.sleep(0.5) self.timePrev = rospy.Time.now() if (self.commandExperiment == 'pause_after_trial'): pass if (self.commandExperiment == 'exit_after_trial'): pass if (self.commandExperiment == 'exit_now'): rv = 'aborted' break # end while() else: rv = 'disabled' # Turn off the laser. commandGalvo = MsgGalvoCommand() commandGalvo.enable_laser = False self.pubGalvoCommand.publish(commandGalvo) userdata.experimentparamsOut = self.experimentparams return rv