Example #1
0
    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
Example #2
0
    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
Example #3
0
    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)
Example #4
0
    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)