本文整理汇总了Python中utils.utils.Utils.normAngle方法的典型用法代码示例。如果您正苦于以下问题：Python Utils.normAngle方法的具体用法？Python Utils.normAngle怎么用？Python Utils.normAngle使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类utils.utils.Utils的用法示例。
在下文中一共展示了Utils.normAngle方法的5个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: findSamples
# 需要导入模块: from utils.utils import Utils [as 别名]
# 或者: from utils.utils.Utils import normAngle [as 别名]
def findSamples(self, category, binImg, samples, outImg):
contours = self.findContourAndBound(binImg.copy(), bounded=True,
upperbounded=True,
bound=self.minContourArea,
upperbound=self.maxContourArea)
contours = sorted(contours, key=cv2.contourArea, reverse=True)
for contour in contours:
# Find the center of each contour
rect = cv2.minAreaRect(contour)
centroid = rect[0]
# Find the orientation of each contour
points = np.int32(cv2.cv.BoxPoints(rect))
edge1 = points[1] - points[0]
edge2 = points[2] - points[1]
if cv2.norm(edge1) > cv2.norm(edge2):
angle = math.degrees(math.atan2(edge1[1], edge1[0]))
else:
angle = math.degrees(math.atan2(edge2[1], edge2[0]))
if 90 < abs(Utils.normAngle(self.comms.curHeading) -
Utils.normAngle(angle)) < 270:
angle = Utils.invertAngle(angle)
samples.append({'centroid': centroid, 'angle': angle,
'area': cv2.contourArea(contour),
'category': category})
if self.debugMode:
Vision.drawRect(outImg, points)示例2: turnRight
# 需要导入模块: from utils.utils import Utils [as 别名]
# 或者: from utils.utils.Utils import normAngle [as 别名]
def turnRight(self):
rospy.loginfo("Turning right...")
# Turn to the right and look for another bin
self.comms.sendMovement(h=Utils.normAngle(self.comms.adjustHeading+90),
d=self.comms.turnDepth,
blocking=True)
self.comms.sendMovement(f=0.9, d=self.comms.turnDepth, blocking=True)示例3: execute
# 需要导入模块: from utils.utils import Utils [as 别名]
# 或者: from utils.utils.Utils import normAngle [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'示例4: execute
# 需要导入模块: from utils.utils import Utils [as 别名]
# 或者: from utils.utils.Utils import normAngle [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'示例5: findLane
# 需要导入模块: from utils.utils import Utils [as 别名]
# 或者: from utils.utils.Utils import normAngle [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