def ContourInfo_callback(self, contourinfo): #rospy.logwarn ('contourinfo callback, stamp=%s, initialized=%s' % (contourinfo.header.stamp, self.initialized)) if self.initialized: try: #rospy.logwarn ('CI contourinfo0 %s' % contourinfo) contourinfo = self.TransformContourinfoPlateFromCamera(contourinfo) #rospy.logwarn ('CI contourinfo1 %s' % contourinfo) contourinfo = self.FilterContourinfoWithinRadius(contourinfo, self.radiusMask) #rospy.logwarn ('CI contourinfo2 %s' % contourinfo) # Repackage the contourinfo into a list of contours self.contours = [] for i in range(len(contourinfo.x)): contour = Contour() contour.header = contourinfo.header contour.x = contourinfo.x[i] contour.y = contourinfo.y[i] contour.angle = contourinfo.angle[i] contour.area = contourinfo.area[i] contour.ecc = contourinfo.ecc[i] self.contours.append(contour) #rospy.logwarn('contour.angle=%0.2f' % (contour.angle)) # Send the contour transforms. try: self.tfbx.sendTransform((contour.x, contour.y, 0.0), tf.transformations.quaternion_about_axis(contour.angle, (0,0,1)), contour.header.stamp, "contour"+str(i), "ImageRect") except tf.Exception, e: rospy.logwarn ('Exception in sendTransform(%s->%s): %s' % ("contour"+str(i),"ImageRect",e)) # Figure out who is who in the camera image. try: self.mapContourFromObject = self.MapContoursFromObjects() except IndexError: self.mapContourFromObject = None #rospy.logwarn ('CI map=%s' % self.mapContourFromObject) #for i in range(len(self.contours)): # rospy.logwarn ('CI contour[%d].x,y=%s' % (i,[self.contours[i].x,self.contours[i].y])) # Update the robot state w/ the contour and end-effector positions. if self.mapContourFromObject is not None: if (self.nRobots==1) and (self.stateEndEffector is not None): if self.mapContourFromObject[0] is not None: # For the robot, use the end-effector angle instead of the contour 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.contours[self.mapContourFromObject[0]].angle = rpy[2] self.objects[0].Update(self.contours[self.mapContourFromObject[0]], self.stateEndEffector.pose.position) else: self.objects[0].Update(None, self.stateEndEffector.pose.position) # 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[0].state.pose.position.x, # self.objects[0].state.pose.position.y, # self.contours[self.mapContourFromObject[0]].x, # self.contours[self.mapContourFromObject[0]].y) #self.fidRobot.write(data) #rospy.loginfo ('CI update robot contour=%s' % contour) # Update the flies' states. for iFly in self.iFly_list: if self.mapContourFromObject[iFly] is not None: #rospy.logwarn ('self.contours[self.mapContourFromObject[%d]]=%s' % (iFly, self.contours[self.mapContourFromObject[iFly]])) try: self.objects[iFly].Update(self.contours[self.mapContourFromObject[iFly]], None) except IndexError, e: #rospy.logwarn('iFly=%s' % iFly) #rospy.logwarn('self.mapContourFromObject=%s' % self.mapContourFromObject) #rospy.logwarn('self.contours=%s' % self.contours) #rospy.logwarn('len(self.objects)=%s' % len(self.objects)) rospy.logwarn('Exception on objects[%d].Update(): %s' % (iFly, e)) else: self.objects[iFly].Update(None, None) #self.stateEndEffector.header.stamp,#rospy.Time.now() #rospy.loginfo ('CI update state %s contour=%s' % (iFly,contour)) # Write a file. #if self.mapContourFromObject[1] is not None: # data = '%s, %s, %s, %s\n' % (self.contours[self.mapContourFromObject[1]].x, # self.contours[self.mapContourFromObject[1]].y, # self.objects[1].state.pose.position.x, # self.objects[1].state.pose.position.y) # self.fidFly.write(data) # Construct the ArenaState message. arenastate = ArenaState() #if self.objects[0].state.pose.position.x is not None: if (self.nRobots==1) and (len(self.objects)>0): arenastate.robot.header.stamp = self.objects[0].state.header.stamp arenastate.robot.header.frame_id = self.objects[0].state.header.frame_id arenastate.robot.pose = self.objects[0].state.pose arenastate.robot.velocity = self.objects[0].state.velocity #rospy.logwarn ('CI robot.position=%s, ptOffset=%s' % ([self.objects[0].state.pose.position.x, # self.objects[0].state.pose.position.y], # [self.objects[0].ptOffset.x, # self.objects[0].ptOffset.y])) for iFly in self.iFly_list: #rospy.logwarn ('iFly=%d, self.mapContourFromObject=%s, len(self.objects)=%d' % (iFly, self.mapContourFromObject, len(self.objects))) if (self.mapContourFromObject[iFly] is not None) and (self.objects[iFly].state.pose.position.x is not None): arenastate.flies.append(MsgFrameState(header = self.objects[iFly].state.header, pose = self.objects[iFly].state.pose, velocity = self.objects[iFly].state.velocity)) #rospy.logwarn('arenastate.flies.append(%s)' % self.objects[iFly].name) # Publish the ArenaState. self.pubArenaState.publish(arenastate) # Publish the EndEffectorOffset. if len(self.objects)>0: self.pubEndEffectorOffset.publish(self.objects[0].ptOffset) # Publish a disc to indicate the arena extent. self.markerArena.header.stamp = contourinfo.header.stamp self.pubMarker.publish(self.markerArena)
def MapContoursFromObjects(self): # Make a list of object positions (i.e. robots & flies). xyObjects = [] # Robots. if (self.nRobots==1): if (self.stateEndEffector is not None): t = self.stateEndEffector.header.stamp xyRobotComputed = [self.stateEndEffector.pose.position.x + self.objects[0].ptOffset.x, self.stateEndEffector.pose.position.y + self.objects[0].ptOffset.y] #rospy.logwarn ('CI Robot image at %s' % ([self.objects[0].state.pose.position.x, # self.objects[0].state.pose.position.y])) elif (len(self.objects)>0) and (self.objects[0].isVisible): t = self.objects[0].state.header.stamp xyRobotComputed = [self.objects[0].state.pose.position.x, self.objects[0].state.pose.position.y] else: t = rospy.Time.now() xyRobotComputed = [0.0, 0.0] xyObjects.append(xyRobotComputed) self.tfbx.sendTransform((xyRobotComputed[0], xyRobotComputed[1], 0.0), tf.transformations.quaternion_about_axis(0, (0,0,1)), t, "RobotComputed", "Plate") else: t = rospy.Time.now() # Flies. for iFly in self.iFly_list: #if self.objects[iFly].isVisible: xyObjects.append([self.objects[iFly].state.pose.position.x, self.objects[iFly].state.pose.position.y]) #rospy.loginfo ('CI Object %s at x,y=%s' % (iFly,[self.objects[iFly].state.pose.position.x, # self.objects[iFly].state.pose.position.y])) # Make a list of contour positions. xyContours = [] for iContour in range(len(self.contours)): if self.contours[iContour] is not None: xyContours.append([self.contours[iContour].x, self.contours[iContour].y]) #rospy.loginfo ('CI contour %s at x,y=%s' % (iContour,[self.contours[iContour].x, # self.contours[iContour].y])) #rospy.loginfo ('CI flies[0,1].isVisible=%s' % [self.objects[0].isVisible, self.objects[1].isVisible]) # Construct two lists of contour stats, to describe the range of good robot/fly properties. contoursMin = [] contoursMax = [] contoursMean = [] # Append the robot contour stats. contour = Contour() if (self.nRobots==1): contour.area = self.objects[0].areaMin #self.areaMinRobot contour.ecc = self.objects[0].eccMin #self.eccMinRobot contoursMin.append(contour) contour.area = self.objects[0].areaMax #self.areaMaxRobot contour.ecc = self.objects[0].eccMax #self.eccMaxRobot contoursMax.append(contour) contour.area = self.objects[0].areaSum / self.objects[0].areaCount contour.ecc = self.objects[0].eccSum / self.objects[0].eccCount contoursMean.append(contour) # Append the fly contour stats. for iFly in self.iFly_list: contour.area = self.objects[iFly].areaMin #self.areaMinFly contour.ecc = self.objects[iFly].eccMin #self.eccMinFly contoursMin.append(contour) contour.area = self.objects[iFly].areaMax #self.areaMaxFly contour.ecc = self.objects[iFly].eccMax #self.eccMaxFly contoursMax.append(contour) contour.area = self.objects[iFly].areaSum / self.objects[iFly].areaCount contour.ecc = self.objects[iFly].eccSum / self.objects[iFly].eccCount contoursMean.append(contour) # Print ecc/area stats. #rospy.logwarn ('robot,fly ecc=[%0.2f, %0.2f], area=[%0.2f, %0.2f]' % (self.objects[0].eccSum/self.objects[0].eccCount, # self.objects[1].eccSum/self.objects[1].eccCount, # self.objects[0].areaSum/self.objects[0].areaCount, # self.objects[1].areaSum/self.objects[1].areaCount)) # Create a list of computed object positions, if any. ptComputed = [None for k in self.iAll_list] #[None for k in range(self.nRobots+len(self.objects))] if (self.nRobots==1): ptComputed[0] = Point(x=xyRobotComputed[0], y=xyRobotComputed[1]) # Augment the contours list, if necessary, so there are as many contours as objects. contoursAug = copy.copy(self.contours) while len(contoursAug)<len(xyObjects): contoursAug.append(ContourInfo(x=55555, y=55555, ecc=1.0, area=1.0)) # norm(x,y) must be less than 999999! #rospy.logwarn ('len(contoursAug),len(xyObjects)=%s' % [len(contoursAug),len(xyObjects)]) # Match objects with contours. #rospy.logwarn ('GetDistanceMatrixFromContours()') for m in range(len(xyObjects)): self.tfbx.sendTransform((xyObjects[m][0],xyObjects[m][1],0.0), (0,0,0,1), t, "xyObjects"+str(m), "Plate") for n in range(len(contoursAug)): self.tfbx.sendTransform((contoursAug[n].x,contoursAug[n].y,0.0), (0,0,0,1), t, "contours"+str(n), "Plate") d = self.GetDistanceMatrixFromContours(xyObjects, contoursAug, contoursMin, contoursMax, contoursMean, ptComputed) if d is not []: # Choose the algorithm. #alg = 'galeshapely' alg = 'munkres' if alg=='galeshapely': (mapObjectsGaleShapely, mapContours) = self.GetMatchGaleShapely(d) mapContoursFromObjects = mapObjectsGaleShapely if alg=='munkres': mapObjectsMunkres = self.GetMatchMunkres(d) mapContoursFromObjects = mapObjectsMunkres #rospy.logwarn('mapContoursFromObjects=%s'%mapContoursFromObjects) #rospy.logwarn('len(mapContoursFromObjects)=%d'%len(mapContoursFromObjects)) #rospy.logwarn('len(xyContours)=%d'%len(xyContours)) # Set the augmented entries to None. for m in range(len(mapContoursFromObjects)): #rospy.logwarn('mapContoursFromObjects[m], len(xyContours)=%s'%[mapContoursFromObjects[m],len(xyContours)]) if (mapContoursFromObjects[m])>=len(xyContours): mapContoursFromObjects[m] = None #rospy.logwarn ('CI mapObjectsGaleShapely =%s' % (mapObjectsGaleShapely)) #rospy.logwarn ('CI mapObjectsMunkres =%s' % mapObjectsMunkres) #rospy.logwarn ('CI mapContoursFromObjects=%s' % mapContoursFromObjects) # Prepend a None for a missing robot. #if self.stateEndEffector is None: # mapContoursFromObjects.insert(0,None) # Append a None for missing flies. while len(mapContoursFromObjects) < self.nRobots+self.nFlies: mapContoursFromObjects.append(None) else: mapContoursFromObjects = [None for k in self.iAll_list] # rospy.logwarn ('----------------------------------') # if (self.nRobots==1): # rospy.logwarn ('CI xyRobotComputed=%s' % xyRobotComputed) # rospy.logwarn ('CI xyObjects=%s' % xyObjects) # rospy.logwarn ('CI xyContours=%s' % xyContours) # if self.stateEndEffector is not None: # rospy.logwarn ('CI ptComputed=(%0.2f+%0.2f, %0.2f+%0.2f)' % (self.stateEndEffector.pose.position.x, # self.objects[0].ptOffset.x, # self.stateEndEffector.pose.position.y, # self.objects[0].ptOffset.y)) # rospy.logwarn ('CI mapObjects=%s' % (mapContoursFromObjects)) # rospy.logwarn('d=\n%s'%d) return mapContoursFromObjects