Python Utils.toHeadingSpace方法代码示例

# 需要导入模块: from utils.utils import Utils [as 别名]
# 或者: from utils.utils.Utils import toHeadingSpace [as 别名]
    def execute(self, userdata):
        if self.comms.isAborted or self.comms.isKilled:
            self.comms.abortMission()
            return 'aborted'

        if not self.comms.retVal or \
           len(self.comms.retVal['matches']) == 0:
            return 'lost'

        self.comms.sendMovement(d=self.comms.aligningDepth,
                                blocking=True)
        try:
            # Align with the bins
            dAngle = Utils.toHeadingSpace(self.comms.nearest)
            adjustAngle = Utils.normAngle(dAngle + self.comms.curHeading)
            self.comms.adjustHeading = adjustAngle
            self.comms.visionFilter.visionMode = BinsVision.BINSMODE
            self.comms.sendMovement(h=adjustAngle,
                                    d=self.comms.aligningDepth,
                                    blocking=True)
            #self.comms.sendMovement(h=adjustAngle,
            #                        d=self.comms.sinkingDepth,
            #                        blocking=True)
            return 'aligned'
        except Exception as e:
            rospy.logerr(str(e))
            adjustAngle = self.comms.curHeading
            self.comms.adjustHeading = adjustAngle
            self.comms.sendMovement(h=adjustAngle, blocking=True)
            #self.comms.sendMovement(d=self.comms.sinkingDepth,
            #                        blocking=True)
            return 'aligned'

# 需要导入模块: from utils.utils import Utils [as 别名]
# 或者: from utils.utils.Utils import toHeadingSpace [as 别名]
    def execute(self, userdata):
        if self.comms.isKilled or self.comms.isAborted:
            self.comms.abortMission()
            return 'aborted'

        curCorner = self.comms.visionFilter.curCorner

        start = time.time()
        while not self.comms.retVal or \
              self.comms.retVal.get('foundLines', None) is None or \
              len(self.comms.retVal['foundLines']) == 0:
            if self.comms.isKilled or self.comms.isAborted:
                self.comms.abortMission()
                return 'aborted'
            if time.time() - start > self.timeout:
                if curCorner == 4: 
                    self.comms.failTask()
                    return 'lost'
                else:
                    self.comms.visionFilter.curCorner += 1
                    self.comms.detectingBox = True
                    return 'next_corner'
            rospy.sleep(rospy.Duration(0.1))

        # Calculate angle between box and lane
        if self.comms.visionFilter.curCorner == 0:
            boxCentroid = (self.centerX, self.centerY)
        else:
            boxCentroid = self.comms.visionFilter.corners[curCorner]
        laneCentroid = self.comms.retVal['foundLines'][0]['pos']
        boxLaneAngle = math.atan2(laneCentroid[1] - boxCentroid[1],
                                  laneCentroid[0] - boxCentroid[0])
        self.angleSampler.newSample(math.degrees(boxLaneAngle))

        variance = self.angleSampler.getVariance()
        rospy.loginfo("Variance: {}".format(variance))
        if (variance < 5.0):
            dAngle = Utils.toHeadingSpace(self.angleSampler.getMedian())
            adjustHeading = Utils.normAngle(self.comms.curHeading + dAngle)
            self.comms.inputHeading = adjustHeading
            rospy.loginfo("box-lane angle: {}".format(self.comms.inputHeading))
            self.comms.sendMovement(h=adjustHeading,
                                    d=self.comms.laneSearchDepth,
                                    blocking=True)
            self.comms.sendMovement(f=self.forward_dist, h=adjustHeading,
                                    d=self.comms.laneSearchDepth,
                                    blocking=True)
            self.comms.visionFilter.curCorner = 0
            return 'aligned'
        else:
            rospy.sleep(rospy.Duration(0.05))
            return 'aligning'

# 需要导入模块: from utils.utils import Utils [as 别名]
# 或者: from utils.utils.Utils import toHeadingSpace [as 别名]
    def gotFrame(self, img):
        """ Main processing function, should return (retData, outputImg) """
        centroids = list()
        outImg = None
        matches = list()
        retData = {'centroids': centroids, 'matches': matches, 'meanX': -1}

        img = cv2.resize(img, (self.screen['width'], self.screen['height']))
        img = Vision.enhance(img)
        img = cv2.GaussianBlur(img, (5, 5), 0)

        ### Detecting Aliens ###
        hsvImg = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
        binImg = cv2.inRange(hsvImg, self.hsvLoThresh1, self.hsvHiThresh1)
        binImg |= cv2.inRange(hsvImg, self.hsvLoThresh2, self.hsvHiThresh2)

        binImg = self.morphology(binImg)

        if self.debugMode:
            outImg1 = cv2.cvtColor(binImg.copy(), cv2.COLOR_GRAY2BGR)
            # Draw the aiming rectangle
            midX = self.screen['width']/2.0
            midY = self.screen['height']/2.0
            maxDeltaX = self.screen['width']*0.03
            maxDeltaY = self.screen['height']*0.03
            cv2.rectangle(outImg1,
                          (int(midX-maxDeltaX), int(midY-maxDeltaY)),
                          (int(midX+maxDeltaX), int(midY+maxDeltaY)),
                          (0, 255, 0), 2)

        scratchImg = binImg.copy()
        alienContours = self.findContourAndBound(scratchImg,
                                                 bounded=True,
                                                 minArea=self.minContourArea)
        #if not contours or len(contours) < 1: return retData, outImg
        alienContours = sorted(alienContours, key=cv2.contourArea, reverse=True)

        for contour in alienContours:
            moment = cv2.moments(contour, False)
            centroids.append((moment['m10']/moment['m00'],
                              moment['m01']/moment['m00']))

        if self.debugMode:
            for centroid in centroids:
                cv2.circle(outImg1, (int(centroid[0]), int(centroid[1])), 5,
                           (0, 0, 255))

        ### Detecting bins ###
        # Threshold and find contours that represent the black bins
        grayImg = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        #grayImg = cv2.equalizeHist(grayImg)
        mean = cv2.mean(grayImg)[0]
        lowest = cv2.minMaxLoc(grayImg)[0]
        thVal = min((lowest + mean)/self.adaptiveCoeff + self.adaptiveOffset,
                    self.blackThresh)
        grayImg = cv2.threshold(grayImg, thVal, 255, cv2.THRESH_BINARY_INV)[1]
        grayImg = self.morphology2(grayImg)
        if self.debugMode == True:
            outImg2 = cv2.cvtColor(grayImg.copy(), cv2.COLOR_GRAY2BGR)

        if self.visionMode == self.BINSMODE:
            matches = self.findBins(grayImg, outImg2)
        elif self.visionMode == self.ALIENSMODE:
            matches = self.findBins(grayImg, False)
            #matches = self.match(alienContours, centroids,
            #                     self.findContourAndBound(grayImg.copy(),
            #                                              minArea=self.areaThresh))
        retData['matches'] = matches

        if self.debugMode:
            for match in matches:
                center = match['centroid']
                angle = match['angle']
                endPt = (int(center[0] + 100*math.cos(math.radians(angle))),
                         int(center[1] + 100*math.sin(math.radians(angle))))
                center = (int(center[0]), int(center[1]))

                #if self.visionMode == self.ALIENSMODE:
                #    #rospy.loginfo("Alien Area: {}".format(cv2.contourArea(match['alien'])))
                #    Vision.drawRect(outImg1,
                #                    cv2.cv.BoxPoints(cv2.minAreaRect(match['alien'])))
                Vision.drawRect(outImg2,
                                cv2.cv.BoxPoints(cv2.minAreaRect(match['contour'])))
                cv2.line(outImg2, center, endPt, (0, 255, 0), 2)
                cv2.putText(outImg2,
                            "{0:.2f}".format(Utils.toHeadingSpace(angle)),
                            (int(center[0]), int(center[1])),
                            cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
            if len(matches) > 0:
                meanX = np.mean(map(lambda c: c['centroid'][0], matches))
                meanY = np.mean(map(lambda c: c['centroid'][1], matches))
                retData['meanX'] = meanX
                #if self.visionMode == self.ALIENSMODE:
                #    closest = min(matches,
                #                  key=lambda m:
                #                  Utils.distBetweenPoints(m['centroid'],
                #                                          (self.centerX, self.centerY)))
                #    Vision.drawRect(outImg1,
                #                    cv2.cv.BoxPoints(cv2.minAreaRect(closest['alien'])),
                #                    color=(0, 255, 255))
#.........这里部分代码省略.........

# 需要导入模块: from utils.utils import Utils [as 别名]
# 或者: from utils.utils.Utils import toHeadingSpace [as 别名]

#.........这里部分代码省略.........
        # Find lines in each bounded rectangle region and find angle
        for contour in contours:
            rect = cv2.minAreaRect(contour)

            # Mask for the region
            #mask = np.zeros_like(binImg, dtype=np.uint8)
            points = np.int32(cv2.cv.BoxPoints(rect))
            #cv2.fillPoly(mask, [points], 255)
            #rectImg = np.bitwise_and(binImg, mask)

            #Find the lane heading
            edge1 = points[1] - points[0]
            edge2 = points[2] - points[1]

            len1 = cv2.norm(edge1)
            len2 = cv2.norm(edge2)

            # Make sure not to detectin the yellow box i.e false positive
            #ratio = len1/len2 if len1 > len2 else len2/len1
            #if ratio < self.ratioBound:
            #    continue
            #if self.curCorner > 0:
            #    dX = (rect[0][0] - self.corners[self.curCorner][0])/self.width
            #    dY = (rect[0][1] - self.corners[self.curCorner][1])/self.height
            #    if dX < 0.05 and dY < 0.05:
            #        continue

            #Choose the vertical edge
            if len1 > len2:
                rectAngle = math.degrees(math.atan2(edge1[1], edge1[0]))
            else:
                rectAngle = math.degrees(math.atan2(edge2[1], edge2[0]))

            # Draw bounding rect
            if self.debugMode:
                Vision.drawRect(outImg, points)

            foundLines.append({'pos': rect[0], 'angle': rectAngle})

        if len(foundLines) >= 2 and self.comms.expectedLanes == 2:
            line1 = foundLines[0]
            line2 = foundLines[1]
            # If there are 2 lines, find their intersection and adjust angle
            l1 = self.vectorizeLine(line1['pos'], line1['angle'])
            l2 = self.vectorizeLine(line2['pos'], line2['angle'])
            crossPt = self.findIntersection(l1, l2) # intersection b/w l1 & l2

            if self.debugMode:
                cv2.circle(outImg, (int(crossPt[0]), int(crossPt[1])),
                           3, (0, 255, 0))
            line1['angle'] = np.rad2deg(math.atan2(l1[0][1]-crossPt[1],
                                                   l1[0][0]-crossPt[0]))
            line2['angle'] = np.rad2deg(math.atan2(l2[0][1]-crossPt[1],
                                                   l2[0][0]-crossPt[0]))
            retData['crossPoint'] = crossPt

            # Figure out which lane marker is left or right
            left = Utils.normAngle(line1['angle'])
            right = Utils.normAngle(line2['angle'])
            if (not ((right-left > 0 and abs(right-left) < 180) or
                     (right-left < 0 and abs(right-left) > 180))):
                line1, line2 = line2, line1
            centroid = ((line1['pos'][0]+line2['pos'][0])/2,
                        (line1['pos'][1]+line2['pos'][1])/2)
            retData['centroid'] = centroid

            if self.debugMode:
                startPt = (int(line1['pos'][0])-70, int(line1['pos'][1]))
                cv2.putText(outImg, "left", startPt,
                            cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1)
        elif len(foundLines) >= 1:
            centroid = foundLines[0]['pos']
            retData['centroid'] = centroid
            # Otherwise adjust to the angle closest to input heading
            lineAngle = foundLines[0]['angle']
            adjustAngle = Utils.normAngle(self.comms.curHeading +
                                          Utils.toHeadingSpace(lineAngle))
            if 90 < abs(Utils.normAngle(self.comms.inputHeading) - adjustAngle) < 270:
                foundLines[0]['angle'] = Utils.invertAngle(lineAngle)

        if self.debugMode:
            # Draw the centroid
            cv2.circle(outImg,
                       (int(centroid[0]), int(centroid[1])),
                       3, (0, 0, 255))
            # Draw vector line and angle
            for line in foundLines:
                startpt = line['pos']
                gradient = np.deg2rad(line['angle'])
                endpt = (int(startpt[0] + 100 * math.cos(gradient)),
                         int(startpt[1] + 100 * math.sin(gradient)))
                startpt = (int(startpt[0]), int(startpt[1]))
                angleStr = "{0:.2f}".format(Utils.toHeadingSpace(line['angle']))

                cv2.line(outImg, startpt, endpt, (255, 0, 0), 2)
                cv2.circle(outImg, startpt, 3, (0, 0, 255), 1)
                cv2.putText(outImg, angleStr, startpt,
                            cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)

        return retData, outImg