def eyes_py(id):
global polePosX, polePosY, dispar, sadws
# 初期化宣言 : このソフトウェアは"eyes_py_node"という名前
rospy.init_node('eyes_py_node', anonymous=True)
# インスタンスの作成
eyes = Eyes(id)
# 処理周期の設定
r = rospy.Rate(PUB_RATE)
print("[eyes3] start")
# ctl + Cで終了しない限りwhileループで処理し続ける
while not rospy.is_shutdown():
#==================================================
# Loop
ret0, imgL = RP3_Cap_L.read()
ret1, imgR = RP3_Cap_R.read()
if dispMode == 1:
cv2.imshow( "Left Camera", imgL )
cv2.imshow( "Right Camera", imgR )
#-- Create DiparityMap --
disparity = GetDisparityMap()
#-- Cal Pole Distance --
capL_target = GetBlueDetect( imgL )
Lx, Ly, Lw, Lh = cv2.boundingRect( capL_target )
capR_target = GetBlueDetect( imgR )
Rx, Ry, Rw, Rh = cv2.boundingRect( capR_target )
# Xは左右中心の更に中心
Lpos = Lx + (Lw / 2)
Rpos = Rx + (Rw / 2)
polePosX = (Lpos + Rpos) / 2
# Yは片側基準でOK
polePosY = Ly + (Lh / 2)
# from disparity to dist
# Z = f x T / um / disparity
poleDist = 6.6 * 62 / 0.003 / disparity[polePosY][polePosX]
print polePosX, polePosY, poleDist
if dispMode == 1:
cv2.rectangle( imgL, (Lx, Ly), (Lx + Lw, Ly + Lh), (0, 0, 255), 1 )
cv2.imshow( "Left Pole", imgL )
cv2.rectangle( imgR, (Rx, Ry), (Rx + Rw, Ry + Rh), (0, 0, 255), 1 )
cv2.imshow( "Right Pole", imgR )
#==================================================
# メイン処理
eyes.main()
# Sleep処理
r.sleep()
def eyes_py(id):
global sidebudsPosX, sidebudsPosY, dispar, sadws
# 初期化宣言 : このソフトウェアは"eyes_py_node"という名前
rospy.init_node('eyes_py_node', anonymous=True)
# インスタンスの作成
eyes = Eyes(id)
# 処理周期の設定
r = rospy.Rate(PUB_RATE)
print("[eyes2] start")
# ctl + Cで終了しない限りwhileループで処理し続ける
while not rospy.is_shutdown():
#==================================================
# Loop
ret0, imgL = RP3_Cap_L.read()
ret1, imgR = RP3_Cap_R.read()
if dispMode == 1:
cv2.imshow("Left Camera", imgL)
cv2.imshow("Right Camera", imgR)
#-- Create DiparityMap --
disparity = GetDisparityMap()
###################################################
#-- Look for SideBuds --
sbsJpg = cv2.imread('./SideBuds_1.JPG', 0)
grayImg = cv2.cvtColor(imgL, cv2.COLOR_BGR2GRAY)
result = cv2.matchTemplate(grayImg, sbsJpg, cv2.TM_CCOEFF_NORMED)
unused_minVal, unused_maxVal, unused_minLoc, maxLoc = cv2.minMaxLoc(
result)
top_left = maxLoc
w, h = sbsJpg.shape[::-1]
bottom_right = (top_left[0] + w, top_left[1] + h)
if dispMode == 1:
cv2.rectangle(imgL, top_left, bottom_right, (0, 255, 0), 1)
cv2.imshow("SideBuds", imgL)
sidebudsPosX = top_left[0] + (w / 2)
sidebudsPosY = top_left[1] + (h / 2)
#-- Cal SideBuds Distance --
sidebudsPosDist = 6.6 * 62 / 0.003 / disparity[sidebudsPosY][
sidebudsPosX]
print sidebudsPosX, sidebudsPosY, sidebudsPosDist
###################################################
#-- Cal weed Center --
targetL = GetGreenDetect(imgL)
targetR = GetGreenDetect(imgR)
# target Position
Lx, Ly, Lw, Lh = cv2.boundingRect(targetL)
Rx, Ry, Rw, Rh = cv2.boundingRect(targetR)
if dispMode == 1:
cv2.rectangle(imgL, (Lx, Ly), (Lx + Lw, Ly + Lh), (0, 0, 255), 1)
cv2.imshow("Weed L", imgL)
cv2.rectangle(imgR, (Rx, Ry), (Rx + Rw, Ry + Rh), (0, 0, 255), 1)
cv2.imshow("Weed R", imgR)
# Xは左右中心の更に中心
Lpos = Lx + (Lw / 2)
Rpos = Rx + (Rw / 2)
weedPosX = (Lpos + Rpos) / 2
# Yは片側基準でOK
weedPosY = Ly + (Lh / 2)
# from disparity to dist
# Z = f x T / um / disparity
weedPosDist = 6.6 * 62 / 0.003 / disparity[weedPosY][weedPosX]
print weedPosX, weedPosY, weedPosDist
#==================================================
# メイン処理
eyes.main()
# Sleep処理
r.sleep()