def Update(self, contourinfo): if (self.initialized): self.count += 1 #rospy.logwarn(contourinfo) if (contourinfo is not None) and (not np.isnan(contourinfo.x)) and (not np.isnan(contourinfo.y)) and (not np.isnan(contourinfo.angle)): bValidContourinfo = True else: bValidContourinfo = False # Use null contourinfo. contourinfoNone = Contourinfo() contourinfoNone.header = Header() contourinfoNone.x = None contourinfoNone.y = None contourinfoNone.angle = None contourinfoNone.area = None contourinfoNone.ecc = None contourinfoNone.imgRoi = None contourinfo = contourinfoNone self.contourinfo = contourinfo self.dt = self.contourinfo.header.stamp - self.stampPrev self.stampPrev = self.contourinfo.header.stamp #with self.lock: # SetDict.SetWithOverwrite(self.params, rospy.get_param('/',{})) # Update the position & orientation filters self.isVisible = False if (bValidContourinfo): # Update min/max ecc & area. if contourinfo.ecc < self.eccMin: self.eccMin = contourinfo.ecc if self.eccMax < contourinfo.ecc: self.eccMax = contourinfo.ecc if contourinfo.area < self.areaMin: self.areaMin = contourinfo.area if self.areaMax < contourinfo.area: self.areaMax = contourinfo.area a = self.dt.to_sec() / 60 if (self.updated): self.areaMean = (1-a)*self.areaMean + a*contourinfo.area self.eccMean = (1-a)*self.eccMean + a*contourinfo.ecc else: #if (not np.isnan(contourinfo.area)): self.areaMean = contourinfo.area #if (not np.isnan(contourinfo.ecc)): self.eccMean = contourinfo.ecc self.isVisible = True (xKalman,yKalman,vxKalman,vyKalman) = self.kfState.Update((self.contourinfo.x, self.contourinfo.y), contourinfo.header.stamp.to_sec()) (zKalman, vzKalman) = (0.0, 0.0) #(xKalman,yKalman) = (self.contourinfo.x,self.contourinfo.y) # Unfiltered. self.lpAngleContour.Update(contourinfo.angle, contourinfo.header.stamp.to_sec()) self.apAngleContour.Update(contourinfo.angle, contourinfo.header.stamp.to_sec()) (angleLeft, angleRight) = self.GetWingAngles(contourinfo) if (np.abs(self.contourinfo.x)>9999) or (np.abs(xKalman)>9999): rospy.logwarn ('FLY LARGE CONTOUR, x,x=%s, %s. Check your background image, lighting, and the parameter tracking/diff_threshold.' % (self.contourinfo.x, xKalman)) else: # We don't have a contourinfo. #rospy.logwarn('FLY No contour seen for %s; check your nFlies parameter.' % self.name) (xKalman, yKalman, vxKalman, vyKalman) = self.kfState.Update(None, contourinfo.header.stamp.to_sec()) (zKalman, vzKalman) = (0.0, 0.0) (angleLeft, angleRight) = (np.pi, np.pi) if (contourinfo.angle is None): contourinfo.angle = 0.0 # If good data, then use it. if (xKalman is not None) and (yKalman is not None): x = xKalman y = yKalman z = zKalman vx = vxKalman vy = vyKalman vz = vzKalman else: # Use the unfiltered data for the case where the filters return None results. rospy.logwarn('FLY Object %s not yet initialized, %s' % (self.name, [self.contourinfo.x, self.contourinfo.y])) x = 0.0#self.contourinfo.x y = 0.0#self.contourinfo.y z = 0.0 vx = 0.0 vy = 0.0 vz = 0.0 # Store the latest state. self.state.header.stamp = self.contourinfo.header.stamp self.state.pose.position.x = x self.state.pose.position.y = y self.state.pose.position.z = z #self.state.pose.orientation comes from self.GetResolvedAngleFiltered() later. self.state.velocity.linear.x = vx self.state.velocity.linear.y = vy self.state.velocity.linear.z = vz self.state.wings.left.angle = angleLeft self.state.wings.right.angle = angleRight # HACK #self.state.pose.position.x = -4.1 #self.state.pose.position.y = -4.0 # HACK if 'Robot' not in self.name: angle = self.GetResolvedAngleFiltered() else: angle = contourinfo.angle (self.state.pose.orientation.x, self.state.pose.orientation.y, self.state.pose.orientation.z, self.state.pose.orientation.w) = tf.transformations.quaternion_about_axis(angle, (0,0,1)) # Get the angular velocities. if self.isVisible: try: ((vx2,vy2,vz2),(wx,wy,wz)) = self.tfrx.lookupTwist(self.name, self.state.header.frame_id, self.state.header.stamp-self.dtVelocity, self.dtVelocity) except (tf.Exception, AttributeError), e: ((vx2,vy2,vz2),(wx,wy,wz)) = ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0)) else: ((vx2,vy2,vz2),(wx,wy,wz)) = ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0)) # Fix glitches due to flip orientation. if wz>6.28: wz = self.lpWz.GetValue() self.state.velocity.angular.x = self.lpWx.Update(wx, self.state.header.stamp.to_sec()) self.state.velocity.angular.y = self.lpWy.Update(wy, self.state.header.stamp.to_sec()) self.state.velocity.angular.z = self.lpWz.Update(wz, self.state.header.stamp.to_sec()) speedPre = np.linalg.norm([self.state.velocity.linear.x, self.state.velocity.linear.y, self.state.velocity.linear.z]) self.speed = self.lpSpeed.Update(speedPre, self.state.header.stamp.to_sec()) # Update the most recent angle of travel, and flip state. self.SetAngleOfTravel() self.UpdateFlipState() if (bValidContourinfo): # Send the Raw transform. if self.isVisible: self.tfbx.sendTransform((self.contourinfo.x, self.contourinfo.y, 0.0), tf.transformations.quaternion_about_axis(self.contourinfo.angle, (0,0,1)), self.contourinfo.header.stamp, self.name+'Contour', 'Arena') # Send the Filtered transform. if self.state.pose.position.x is not None: q = self.state.pose.orientation self.tfbx.sendTransform((self.state.pose.position.x, self.state.pose.position.y, self.state.pose.position.z), (q.x, q.y, q.z, q.w), self.state.header.stamp, self.name, self.state.header.frame_id) # Send the Forecast transform. if self.state.pose.position.x is not None: poseForecast = Pose()#copy.copy(self.state.pose) poseForecast.position.x = self.state.pose.position.x + self.state.velocity.linear.x * self.params['tracking']['dtForecast'] poseForecast.position.y = self.state.pose.position.y + self.state.velocity.linear.y * self.params['tracking']['dtForecast'] poseForecast.position.z = self.state.pose.position.z + self.state.velocity.linear.z * self.params['tracking']['dtForecast'] q = self.state.pose.orientation self.tfbx.sendTransform((poseForecast.position.x, poseForecast.position.y, poseForecast.position.z), (q.x, q.y, q.z, q.w), self.state.header.stamp, self.name+'Forecast', self.state.header.frame_id) # Publish a 3D model marker for the robot. if 'Robot' in self.name: markerRobot = Marker(header=Header(stamp = self.state.header.stamp, frame_id='Arena'), ns=self.name, id=1, type=Marker.CYLINDER, action=0, pose=Pose(position=Point(x=self.state.pose.position.x, y=self.state.pose.position.y, z=self.state.pose.position.z), orientation=self.state.pose.orientation), scale=Vector3(x=self.params['tracking']['robot']['width'], y=self.params['tracking']['robot']['length'], z=self.params['tracking']['robot']['height']), color=ColorRGBA(a=0.7, r=0.5, g=0.5, b=0.5), lifetime=rospy.Duration(0.5)) self.pubMarker.publish(markerRobot) self.updated = True
def ProcessContourinfoLists(self): contourinfolistsPixels = None with self.lockBuffer: if (self.bufferContourinfolists[self.iBufferWorking] is not None): contourinfolistsPixels = self.bufferContourinfolists[self.iBufferWorking] # Mark this buffer entry as available for loading. self.bufferContourinfolists[self.iBufferWorking] = None # Go to the next slot. self.iBufferWorking = (self.iBufferWorking+1) % len(self.bufferContourinfolists) if (contourinfolistsPixels is not None): if (self.initialized): with self.lockThreads: try: # Publish state of the EndEffector for ourselves (so we get the EE via bag-recordable message rather than via tf. if (0 < self.nRobots): try: stamp = self.tfrx.getLatestCommonTime('Arena', 'EndEffector') (translationEE,rotationEE) = self.tfrx.lookupTransform('Arena', 'EndEffector', stamp) except tf.Exception, e: pass # No transform - Either the EE is still initializing, or because we're replaying a bag file. #rospy.logwarn ('CI EndEffector not yet initialized: %s' % e) else: self.pubStateEE.publish(MsgFrameState(header=Header(stamp=contourinfolistsPixels.header.stamp, frame_id='Arena'), pose=Pose(position=Point(translationEE[0],translationEE[1],translationEE[2]), orientation=Quaternion(rotationEE[0],rotationEE[1],rotationEE[2],rotationEE[3])) ) ) # Apply the arena mask, and transform to arena frame. contourinfolistsPixels = self.FilterContourinfolistsWithinMask(contourinfolistsPixels) contourinfolists = self.TransformContourinfolistsArenaFromCamera(contourinfolistsPixels) # Repackage the contourinfolists as a list of contourinfos, ignoring any that are in an exclusion zone. self.contourinfo_list = [] for i in range(len(contourinfolists.x)): inExclusionzone = False if (self.enabledExclusionzone): # See if the contourinfo is in any of the exclusionzones. for k in range(len(self.pointExclusionzone_list)): inExclusionzone = inExclusionzone or (np.linalg.norm([contourinfolists.x[i]-self.pointExclusionzone_list[k].x, contourinfolists.y[i]-self.pointExclusionzone_list[k].y]) < self.radiusExclusionzone_list[k]) if (not inExclusionzone): contourinfo = Contourinfo() contourinfo.header = contourinfolists.header contourinfo.x = contourinfolists.x[i] contourinfo.y = contourinfolists.y[i] if (not np.isnan(contourinfolists.angle[i])): contourinfo.angle = contourinfolists.angle[i] else: contourinfo.angle = self.contouranglePrev self.contouranglePrev = contourinfo.angle contourinfo.area = contourinfolists.area[i] contourinfo.ecc = contourinfolists.ecc[i] contourinfo.imgRoi = contourinfolists.imgRoi[i] self.contourinfo_list.append(contourinfo) # Figure out who is who in the camera image. try: self.mapContourinfoFromObject = self.MapContoursFromObjects() except IndexError: self.mapContourinfoFromObject = None if (self.mapContourinfoFromObject is not None): # Update the robot state w/ the contourinfo and end-effector positions. for iRobot in self.iRobot_list: # If the map points to a contourinfo, then use it. if (self.mapContourinfoFromObject[iRobot] is not None): # For the robot, use the end-effector angle instead of the contourinfo angle. if (False):#(self.stateKinematic is not None): q = self.stateKinematic.pose.orientation rpy = tf.transformations.euler_from_quaternion((q.x, q.y, q.z, q.w)) self.contourinfo_list[self.mapContourinfoFromObject[iRobot]].angle = rpy[2] # Use this contourinfo. contourinfo = self.contourinfo_list[self.mapContourinfoFromObject[iRobot]] # Else use the kinematic state, if available. elif (self.stateKinematic is not None): q = self.stateKinematic.pose.orientation rpy = tf.transformations.euler_from_quaternion((q.x, q.y, q.z, q.w)) area = np.pi * (self.params['tracking']['robot']['width']/2)**2 # Make a 'fake' contourinfo from kinematics. contourinfo = Contourinfo(header=self.stateKinematic.header, x=self.stateKinematic.pose.position.x, y=self.stateKinematic.pose.position.y, angle=rpy[2], area=area, ecc=1.0, imgRoi=None ) else: contourinfo = None # Update the object. if (iRobot < len(self.objects)): self.objects[iRobot].Update(contourinfo) # Write a file (for getting Kalman covariances, etc). #data = '%s, %s, %s, %s, %s, %s\n' % (self.stateKinematic.pose.position.x, # self.stateKinematic.pose.position.y, # self.objects[iRobot].state.pose.position.x, # self.objects[iRobot].state.pose.position.y, # self.contourinfo_list[self.mapContourinfoFromObject[iRobot]].x, # self.contourinfo_list[self.mapContourinfoFromObject[iRobot]].y) #self.fidRobot.write(data) # Update the flies' states. for iFly in self.iFly_list: if (self.mapContourinfoFromObject[iFly] is not None): contourinfo = self.contourinfo_list[self.mapContourinfoFromObject[iFly]] else: contourinfo = None if (iFly < len(self.objects)): self.objects[iFly].Update(contourinfo) # Write a file. #if self.mapContourinfoFromObject[1] is not None: # data = '%s, %s, %s, %s\n' % (self.contourinfo_list[self.mapContourinfoFromObject[1]].x, # self.contourinfo_list[self.mapContourinfoFromObject[1]].y, # self.objects[1].state.pose.position.x, # self.objects[1].state.pose.position.y) # self.fidFly.write(data) self.PublishArenaStateFromObjects() # Publish the visual_state. if (self.bUseVisualServoing) and (0 in self.iRobot_list) and (0 < len(self.objects)): self.pubVisualState.publish(self.objects[0].state) # Publish a marker to indicate the size of the arena. self.markerArenaOuter.header.stamp = contourinfolists.header.stamp self.markerArenaInner.header.stamp = contourinfolists.header.stamp self.pubMarker.publish(self.markerArenaOuter) self.pubMarker.publish(self.markerArenaInner) # Publish markers for all the exclusionzones. if self.enabledExclusionzone: for marker in self.markerExclusionzone_list: marker.header.stamp = contourinfolists.header.stamp self.pubMarker.publish(marker) except rospy.exceptions.ROSException, e: rospy.loginfo('CI ROSException: %s' % e)
def __init__(self, name=None, tfrx=None, lock=None): global globalLock2 if (lock is not None): self.lock = lock else: self.lock = globalLock2 self.initialized = False self.updated = False self.name = name # Read all the params. with self.lock: self.params = rospy.get_param('/', {}) defaults = { 'tracking': { 'dtVelocity': 0.2, 'dtForecast': 0.15, 'rcFilterFlip': 3.0, 'rcFilterSpeed': 0.2, 'rcFilterAngle': 0.1, 'rcFilterAngularVel': 0.1, 'rcForeground': 1.0, 'speedThresholdForTravel': 5.0, 'robot': { 'width': 1.0, 'length': 1.0, 'height': 1.0 }, 'roi': { 'width': 15, 'height': 15 }, 'lengthBody': 9, 'widthBody': 5, 'thresholdBody': 40.0, 'scalarMeanFlySubtraction': 1.0, 'nStdDevWings': 2.0, 'rcWingMean': 10.0, # The time constant for exponentially weighted wing mean. 'rcWingStdDev': 100.0, # The time constant for exponentially weighted wing std dev. 'nonessentialPublish': False, 'nonessentialCalcSuper': False } } SetDict.SetWithPreserve(self.params, defaults) self.cvbridge = CvBridge() self.tfrx = tfrx self.tfbx = tf.TransformBroadcaster() self.pubMarker = rospy.Publisher('visualization_marker', Marker) # Nonessential stuff to publish. if self.params['tracking']['nonessentialPublish']: self.pubImageRoiMean = rospy.Publisher(self.name + '/image_mean', Image) self.pubImageRoi = rospy.Publisher(self.name + '/image', Image) self.pubImageRoiReg = rospy.Publisher(self.name + '/image_reg', Image) self.pubImageRoiWings = rospy.Publisher(self.name + '/image_wings', Image) self.pubImageMaskWings = rospy.Publisher( self.name + '/image_mask_wings', Image) self.pubImageMaskBody = rospy.Publisher( self.name + '/image_mask_body', Image) self.pubLeft = rospy.Publisher(self.name + '/leftwing', Float32) self.pubLeftIntensity = rospy.Publisher( self.name + '/leftintensity', Float32) self.pubLeftMeanIntensity = rospy.Publisher( self.name + '/leftmean', Float32) self.pubLeftMeanPlusStdDev = rospy.Publisher( self.name + '/leftmeanplusstddev', Float32) self.pubLeftMeanMinusStdDev = rospy.Publisher( self.name + '/leftmeanminusstddev', Float32) self.pubLeftDev = rospy.Publisher(self.name + '/leftdev', Float32) self.pubRight = rospy.Publisher(self.name + '/rightwing', Float32) self.pubRightIntensity = rospy.Publisher( self.name + '/rightintensity', Float32) self.pubRightMeanIntensity = rospy.Publisher( self.name + '/rightmean', Float32) self.pubRightMeanPlusStdDev = rospy.Publisher( self.name + '/rightmeanplusstddev', Float32) self.pubRightMeanMinusStdDev = rospy.Publisher( self.name + '/rightmeanminusstddev', Float32) self.pubRightDev = rospy.Publisher(self.name + '/rightdev', Float32) self.pubImageSuper = rospy.Publisher(self.name + '/image_super', Image) self.pubImageSuperCount = rospy.Publisher( self.name + '/image_supercount', Image) self.pubImageSuperGrad = rospy.Publisher( self.name + '/image_supergrad', Image) rospy.logwarn( '(%s) Filter time constants: rcFilterAngle=%s, rcFilterAngularVel=%s' % (self.name, self.params['tracking']['rcFilterAngle'], self.params['tracking']['rcFilterAngularVel'])) self.kfState = filters.KalmanFilter() self.lpAngle = filters.LowPassCircleFilter( RC=self.params['tracking']['rcFilterAngle']) self.lpAngle.SetValue(0.0) self.lpAngleContour = filters.LowPassHalfCircleFilter( RC=self.params['tracking']['rcFilterAngle']) self.lpAngleContour.SetValue(0.0) self.apAngleContour = filters.LowPassHalfCircleFilter(RC=0.0) self.apAngleContour.SetValue(0.0) self.stampPrev = rospy.Time.now() self.dtVelocity = rospy.Duration( self.params['tracking'] ['dtVelocity']) # Interval over which to calculate velocity. self.stampPrev = rospy.Time.now() # Orientation detection stuff. self.lpFlip = filters.LowPassFilter( RC=self.params['tracking']['rcFilterFlip']) self.lpFlip.SetValue(0.0) # (0.5) TEST self.lpSpeed = filters.LowPassFilter( RC=self.params['tracking']['rcFilterSpeed']) self.lpSpeed.SetValue(0.0) self.angleOfTravelRecent = 0.0 self.contourinfo = Contourinfo() self.speed = 0.0 self.lpWx = filters.LowPassFilter( RC=self.params['tracking']['rcFilterAngularVel']) self.lpWy = filters.LowPassFilter( RC=self.params['tracking']['rcFilterAngularVel']) self.lpWz = filters.LowPassFilter( RC=self.params['tracking']['rcFilterAngularVel']) self.lpWx.SetValue(0.0) self.lpWy.SetValue(0.0) self.lpWz.SetValue(0.0) self.isVisible = False self.isDead = False # TO DO: dead fly detection. self.state = MsgFrameState() self.state.header.frame_id = 'Arena' self.state.pose.position.x = 0.0 self.state.pose.position.y = 0.0 self.state.pose.position.z = 0.0 q = tf.transformations.quaternion_about_axis(0.0, (0, 0, 1)) self.state.pose.orientation.x = q[ 0] # Note: The orientation is ambiguous as to head/tail. self.state.pose.orientation.y = q[ 1] # Check the self.lpFlip sign to resolve it: use self.ResolvedAngle(). self.state.pose.orientation.z = q[2] # self.state.pose.orientation.w = q[3] # self.state.velocity.linear.x = 0.0 self.state.velocity.linear.y = 0.0 self.state.velocity.linear.z = 0.0 self.state.velocity.angular.x = 0.0 self.state.velocity.angular.y = 0.0 self.state.velocity.angular.z = 0.0 self.state.wings.left.angle = 0.0 self.state.wings.right.angle = 0.0 self.eccMin = 999.9 self.eccMean = 0.0 self.eccMax = 0.0 self.areaMin = 999.9 self.areaMean = 1.0 self.areaMax = 0.0 # Wing angle stuff. self.npfRoiMean = None self.npfRoiSum = None self.npMaskWings = None self.npMaskCircle = np.zeros([ self.params['tracking']['roi']['height'], self.params['tracking']['roi']['width'] ], dtype=np.uint8) cv2.circle(self.npMaskCircle, (int(self.params['tracking']['roi']['height'] / 2), int(self.params['tracking']['roi']['width'] / 2)), int(self.params['tracking']['roi']['height'] / 2), 255, cv.CV_FILLED) self.meanIntensityLeft = None self.meanIntensityRight = None self.varIntensityLeft = None self.varIntensityRight = None self.sumIntensityLeft = 0.0 self.sumIntensityRight = 0.0 self.sumSqDevIntensityLeft = 0.0 self.stddevIntensityLeft = 0.0 self.sumSqDevIntensityRight = 0.0 self.stddevIntensityRight = 0.0 self.count = 0 # Super-resolution image. if self.params['tracking']['nonessentialCalcSuper']: self.heightSuper = 16 * self.params['tracking']['roi'][ 'height'] # The resolution increase for super-res. self.widthSuper = 16 * self.params['tracking']['roi']['width'] self.npfSuper = np.zeros([self.heightSuper, self.widthSuper], dtype=np.float) self.npfSuperSum = np.zeros([self.heightSuper, self.widthSuper], dtype=np.float) self.npfSuperCount = np.zeros([self.heightSuper, self.widthSuper], dtype=np.float) rospy.logwarn('FLY object added, name=%s' % name) self.timePrev = rospy.Time.now().to_sec() self.theta = 0.0 self.initialized = True
def Update(self, contourinfo): if (self.initialized): self.count += 1 #rospy.logwarn(contourinfo) if (contourinfo is not None) and (not np.isnan( contourinfo.x)) and (not np.isnan( contourinfo.y)) and (not np.isnan(contourinfo.angle)): bValidContourinfo = True else: bValidContourinfo = False # Use null contourinfo. contourinfoNone = Contourinfo() contourinfoNone.header = Header() contourinfoNone.x = None contourinfoNone.y = None contourinfoNone.angle = None contourinfoNone.area = None contourinfoNone.ecc = None contourinfoNone.imgRoi = None contourinfo = contourinfoNone self.contourinfo = contourinfo self.dt = self.contourinfo.header.stamp - self.stampPrev self.stampPrev = self.contourinfo.header.stamp #with self.lock: # SetDict.SetWithOverwrite(self.params, rospy.get_param('/',{})) # Update the position & orientation filters self.isVisible = False if (bValidContourinfo): # Update min/max ecc & area. if contourinfo.ecc < self.eccMin: self.eccMin = contourinfo.ecc if self.eccMax < contourinfo.ecc: self.eccMax = contourinfo.ecc if contourinfo.area < self.areaMin: self.areaMin = contourinfo.area if self.areaMax < contourinfo.area: self.areaMax = contourinfo.area a = self.dt.to_sec() / 60 if (self.updated): self.areaMean = (1 - a) * self.areaMean + a * contourinfo.area self.eccMean = (1 - a) * self.eccMean + a * contourinfo.ecc else: #if (not np.isnan(contourinfo.area)): self.areaMean = contourinfo.area #if (not np.isnan(contourinfo.ecc)): self.eccMean = contourinfo.ecc self.isVisible = True (xKalman, yKalman, vxKalman, vyKalman) = self.kfState.Update( (self.contourinfo.x, self.contourinfo.y), contourinfo.header.stamp.to_sec()) (zKalman, vzKalman) = (0.0, 0.0) #(xKalman,yKalman) = (self.contourinfo.x,self.contourinfo.y) # Unfiltered. self.lpAngleContour.Update(contourinfo.angle, contourinfo.header.stamp.to_sec()) self.apAngleContour.Update(contourinfo.angle, contourinfo.header.stamp.to_sec()) (angleLeft, angleRight) = self.GetWingAngles(contourinfo) if (np.abs(self.contourinfo.x) > 9999) or (np.abs(xKalman) > 9999): rospy.logwarn( 'FLY LARGE CONTOUR, x,x=%s, %s. Check your background image, lighting, and the parameter tracking/diff_threshold.' % (self.contourinfo.x, xKalman)) else: # We don't have a contourinfo. #rospy.logwarn('FLY No contour seen for %s; check your nFlies parameter.' % self.name) (xKalman, yKalman, vxKalman, vyKalman) = self.kfState.Update( None, contourinfo.header.stamp.to_sec()) (zKalman, vzKalman) = (0.0, 0.0) (angleLeft, angleRight) = (np.pi, np.pi) if (contourinfo.angle is None): contourinfo.angle = 0.0 # If good data, then use it. if (xKalman is not None) and (yKalman is not None): x = xKalman y = yKalman z = zKalman vx = vxKalman vy = vyKalman vz = vzKalman else: # Use the unfiltered data for the case where the filters return None results. rospy.logwarn( 'FLY Object %s not yet initialized, %s' % (self.name, [self.contourinfo.x, self.contourinfo.y])) x = 0.0 #self.contourinfo.x y = 0.0 #self.contourinfo.y z = 0.0 vx = 0.0 vy = 0.0 vz = 0.0 # Store the latest state. self.state.header.stamp = self.contourinfo.header.stamp self.state.pose.position.x = x self.state.pose.position.y = y self.state.pose.position.z = z #self.state.pose.orientation comes from self.GetResolvedAngleFiltered() later. self.state.velocity.linear.x = vx self.state.velocity.linear.y = vy self.state.velocity.linear.z = vz self.state.wings.left.angle = angleLeft self.state.wings.right.angle = angleRight # HACK #self.state.pose.position.x = -4.1 #self.state.pose.position.y = -4.0 # HACK if 'Robot' not in self.name: angle = self.GetResolvedAngleFiltered() else: angle = contourinfo.angle (self.state.pose.orientation.x, self.state.pose.orientation.y, self.state.pose.orientation.z, self.state.pose.orientation.w ) = tf.transformations.quaternion_about_axis(angle, (0, 0, 1)) # Get the angular velocities. if self.isVisible: try: ((vx2, vy2, vz2), (wx, wy, wz)) = self.tfrx.lookupTwist( self.name, self.state.header.frame_id, self.state.header.stamp - self.dtVelocity, self.dtVelocity) except (tf.Exception, AttributeError), e: ((vx2, vy2, vz2), (wx, wy, wz)) = ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0)) else: ((vx2, vy2, vz2), (wx, wy, wz)) = ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0)) # Fix glitches due to flip orientation. if wz > 6.28: wz = self.lpWz.GetValue() self.state.velocity.angular.x = self.lpWx.Update( wx, self.state.header.stamp.to_sec()) self.state.velocity.angular.y = self.lpWy.Update( wy, self.state.header.stamp.to_sec()) self.state.velocity.angular.z = self.lpWz.Update( wz, self.state.header.stamp.to_sec()) speedPre = np.linalg.norm([ self.state.velocity.linear.x, self.state.velocity.linear.y, self.state.velocity.linear.z ]) self.speed = self.lpSpeed.Update(speedPre, self.state.header.stamp.to_sec()) # Update the most recent angle of travel, and flip state. self.SetAngleOfTravel() self.UpdateFlipState() if (bValidContourinfo): # Send the Raw transform. if self.isVisible: self.tfbx.sendTransform( (self.contourinfo.x, self.contourinfo.y, 0.0), tf.transformations.quaternion_about_axis( self.contourinfo.angle, (0, 0, 1)), self.contourinfo.header.stamp, self.name + 'Contour', 'Arena') # Send the Filtered transform. if self.state.pose.position.x is not None: q = self.state.pose.orientation self.tfbx.sendTransform((self.state.pose.position.x, self.state.pose.position.y, self.state.pose.position.z), (q.x, q.y, q.z, q.w), self.state.header.stamp, self.name, self.state.header.frame_id) # Send the Forecast transform. if self.state.pose.position.x is not None: poseForecast = Pose() #copy.copy(self.state.pose) poseForecast.position.x = self.state.pose.position.x + self.state.velocity.linear.x * self.params[ 'tracking']['dtForecast'] poseForecast.position.y = self.state.pose.position.y + self.state.velocity.linear.y * self.params[ 'tracking']['dtForecast'] poseForecast.position.z = self.state.pose.position.z + self.state.velocity.linear.z * self.params[ 'tracking']['dtForecast'] q = self.state.pose.orientation self.tfbx.sendTransform( (poseForecast.position.x, poseForecast.position.y, poseForecast.position.z), (q.x, q.y, q.z, q.w), self.state.header.stamp, self.name + 'Forecast', self.state.header.frame_id) # Publish a 3D model marker for the robot. if 'Robot' in self.name: markerRobot = Marker( header=Header(stamp=self.state.header.stamp, frame_id='Arena'), ns=self.name, id=1, type=Marker.CYLINDER, action=0, pose=Pose(position=Point(x=self.state.pose.position.x, y=self.state.pose.position.y, z=self.state.pose.position.z), orientation=self.state.pose.orientation), scale=Vector3( x=self.params['tracking']['robot']['width'], y=self.params['tracking']['robot']['length'], z=self.params['tracking']['robot']['height']), color=ColorRGBA(a=0.7, r=0.5, g=0.5, b=0.5), lifetime=rospy.Duration(0.5)) self.pubMarker.publish(markerRobot) self.updated = True
def ContourinfoLists_callback(self, contourinfolistsPixels): if self.initialized: if self.nRobots>0: # Publish state of the EndEffector for ourselves (so we get the EE via bag-recordable message rather than via tf. try: stamp = self.tfrx.getLatestCommonTime('Arena', 'EndEffector') (translationEE,rotationEE) = self.tfrx.lookupTransform('Arena', 'EndEffector', stamp) except tf.Exception, e: pass # No transform - Either the EE is still initializing, or because we're replaying a bag file. #rospy.logwarn ('CI EndEffector not yet initialized: %s' % e) else: self.pubStateEE.publish(MsgFrameState(header=Header(stamp=contourinfolistsPixels.header.stamp, frame_id='Arena'), pose=Pose(position=Point(translationEE[0],translationEE[1],translationEE[2]), orientation=Quaternion(rotationEE[0],rotationEE[1],rotationEE[2],rotationEE[3])) ) ) contourinfolistsPixels = self.FilterContourinfoWithinMask(contourinfolistsPixels) contourinfolists = self.TransformContourinfoArenaFromCamera(contourinfolistsPixels) # Create a null contourinfo. contourinfoNone = Contourinfo() contourinfoNone.header = contourinfolists.header contourinfoNone.x = None contourinfoNone.y = None contourinfoNone.angle = None contourinfoNone.area = None contourinfoNone.ecc = None contourinfoNone.imgRoi = None # Repackage the contourinfolists into a list of contourinfos, ignoring any that are in the exclusion zone. self.contourinfo_list = [] for i in range(len(contourinfolists.x)): inExclusionzone = False if (self.enabledExclusionzone): # See if the contourinfo is in any of the exclusionzones. for k in range(len(self.pointExclusionzone_list)): inExclusionzone = inExclusionzone or (N.linalg.norm([contourinfolists.x[i]-self.pointExclusionzone_list[k].x, contourinfolists.y[i]-self.pointExclusionzone_list[k].y]) < self.radiusExclusionzone_list[k]) if (not inExclusionzone): contourinfo = Contourinfo() contourinfo.header = contourinfolists.header contourinfo.x = contourinfolists.x[i] contourinfo.y = contourinfolists.y[i] if (contourinfolists.angle[i] != 99.9) and (not N.isnan(contourinfolists.angle[i])): contourinfo.angle = contourinfolists.angle[i] else: contourinfo.angle = self.contouranglePrev self.contouranglePrev = contourinfo.angle contourinfo.area = contourinfolists.area[i] contourinfo.ecc = contourinfolists.ecc[i] contourinfo.imgRoi = contourinfolists.imgRoi[i] self.contourinfo_list.append(contourinfo) # Figure out who is who in the camera image. try: self.mapContourinfoFromObject = self.MapContoursFromObjects() except IndexError: self.mapContourinfoFromObject = None if self.mapContourinfoFromObject is not None: # Update the robot state w/ the contourinfo and end-effector positions. for iRobot in self.iRobot_list: if self.mapContourinfoFromObject[iRobot] is not None: # For the robot, use the end-effector angle instead of the contourinfo angle. if self.stateEndEffector is not None: q = self.stateEndEffector.pose.orientation rpy = tf.transformations.euler_from_quaternion((q.x, q.y, q.z, q.w)) self.contourinfo_list[self.mapContourinfoFromObject[iRobot]].angle = rpy[2] contourinfo = self.contourinfo_list[self.mapContourinfoFromObject[iRobot]] else: contourinfo = contourinfoNone # Use the computed pose if one exists. if self.stateEndEffector is not None: poses = PoseStamped(header=self.stateEndEffector.header, pose=self.stateEndEffector.pose) else: poses = None # Update the object. if (iRobot < len(self.objects)): self.objects[iRobot].Update(contourinfo, poses) # Write a file (for getting Kalman covariances, etc). #data = '%s, %s, %s, %s, %s, %s\n' % (self.stateEndEffector.pose.position.x, # self.stateEndEffector.pose.position.y, # self.objects[iRobot].state.pose.position.x, # self.objects[iRobot].state.pose.position.y, # self.contourinfo_list[self.mapContourinfoFromObject[iRobot]].x, # self.contourinfo_list[self.mapContourinfoFromObject[iRobot]].y) #self.fidRobot.write(data) # Update the flies' states. for iFly in self.iFly_list: if self.mapContourinfoFromObject[iFly] is not None: contourinfo = self.contourinfo_list[self.mapContourinfoFromObject[iFly]] else: contourinfo = contourinfoNone #rospy.logwarn ('No contourinfo for fly %d' % iFly) if (iFly < len(self.objects)): self.objects[iFly].Update(contourinfo, None) # Write a file. #if self.mapContourinfoFromObject[1] is not None: # data = '%s, %s, %s, %s\n' % (self.contourinfo_list[self.mapContourinfoFromObject[1]].x, # self.contourinfo_list[self.mapContourinfoFromObject[1]].y, # self.objects[1].state.pose.position.x, # self.objects[1].state.pose.position.y) # self.fidFly.write(data) self.PublishArenaStateFromObjects() # Publish the VisualState. if (0 in self.iRobot_list) and (0 < len(self.objects)): self.pubVisualState.publish(self.objects[0].state) # Publish a marker to indicate the size of the arena. self.markerArenaOuter.header.stamp = contourinfolists.header.stamp self.markerArenaInner.header.stamp = contourinfolists.header.stamp self.pubMarker.publish(self.markerArenaOuter) self.pubMarker.publish(self.markerArenaInner) # Publish markers for all the exclusionzones. if self.enabledExclusionzone: for marker in self.markerExclusionzone_list: marker.header.stamp = contourinfolists.header.stamp self.pubMarker.publish(marker)