本文整理汇总了Python中utils.utils.Utils.toHeadingSpace方法的典型用法代码示例。如果您正苦于以下问题:Python Utils.toHeadingSpace方法的具体用法?Python Utils.toHeadingSpace怎么用?Python Utils.toHeadingSpace使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类utils.utils.Utils
的用法示例。
在下文中一共展示了Utils.toHeadingSpace方法的4个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: execute
# 需要导入模块: 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'
示例2: execute
# 需要导入模块: 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'
示例3: gotFrame
# 需要导入模块: 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))
#.........这里部分代码省略.........
示例4: findLane
# 需要导入模块: 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