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)