add no.20
This commit is contained in:
litian.zhuang
parent
acb097794d
commit
e29d15e6eb
83
onekey.sh
83
onekey.sh
|
|
@ -9,7 +9,7 @@ export PATH
|
|||
# SPARK技术讨论与反馈QQ群：6646169 8346256
|
||||
#=================================================
|
||||
|
||||
GAME_ENABLE="yes"
|
||||
GAME_ENABLE="no"
|
||||
sh_ver="2.0"
|
||||
filepath=$(cd "$(dirname "$0")"; pwd)
|
||||
Green_font_prefix="\033[32m" && Red_font_prefix="\033[31m" && Green_background_prefix="\033[42;37m" && Red_background_prefix="\033[41;37m" && Yellow_background_prefix="\033[43;37m" && Font_color_suffix="\033[0m" && Yellow_font_prefix="\e[1;33m" && Blue_font_prefix="\e[0;34m"
|
||||
|
|
@ -98,6 +98,21 @@ check_lidar(){
|
|||
echo -e "${Error} 没有找到雷达，请确认雷达已正确连接！！"
|
||||
fi
|
||||
}
|
||||
|
||||
check_camera_no_print(){
|
||||
#检查使用哪种设备
|
||||
if [ -n "$(lsusb -d 2bc5:0403)" ]; then
|
||||
CAMERATYPE="astrapro"
|
||||
fi
|
||||
if [ -n "$(lsusb -d 2bc5:0401)" ]; then
|
||||
CAMERATYPE="astra"
|
||||
fi
|
||||
if [ -n "$(lsusb -d 8086:0b07)" ]; then
|
||||
CAMERATYPE="d435"
|
||||
fi
|
||||
}
|
||||
|
||||
|
||||
#检查摄像头设备
|
||||
check_camera(){
|
||||
|
||||
|
|
@ -752,14 +767,49 @@ spark_dock(){
|
|||
}
|
||||
|
||||
|
||||
#让SPARK夺宝奇兵比赛用示例程序
|
||||
#hsv范围值查找
|
||||
spark_hsv_detection(){
|
||||
echo -e "${Info}"
|
||||
echo -e "${Info}hsv值自动查找"
|
||||
ROSVER=`/usr/bin/rosversion -d`
|
||||
PROJECTPATH=$(cd `dirname $0`; pwd)
|
||||
source ${PROJECTPATH}/devel/setup.bash
|
||||
|
||||
echo -e "${Info}"
|
||||
echo -e "${Info}请确定："
|
||||
echo -e "${Info} A.摄像头已反向向下安装好。机械臂正常上电。"
|
||||
echo -e "${Info} B.${Red_font_prefix}红色${Font_color_suffix}标定物已贴好在吸盘固定头正上方。"
|
||||
echo -e "${Info} C.机械臂正常上电。"
|
||||
echo -e "${Info}退出请输入：Ctrl + c "
|
||||
echo -e "${Info}"
|
||||
|
||||
echo -e "${Info}请选择比赛方式：
|
||||
${Green_font_prefix}1.${Font_color_suffix} 获取吸盘上方hsv值
|
||||
${Green_font_prefix}2.${Font_color_suffix} 获取颜色块hsv值,需要将颜色块放在图像的矩形框中
|
||||
${Green_font_prefix}3.${Font_color_suffix} 退出请输入：Ctrl + c"
|
||||
echo && stty erase ^? && read -p "请输入数字 [1-2]：" armnum
|
||||
case "$armnum" in
|
||||
1)
|
||||
roslaunch spark_carry_object hsv_detection.launch camera_type_tel:=${CAMERATYPE} color:=${calibration}
|
||||
;;
|
||||
2)
|
||||
roslaunch spark_carry_object hsv_detection.launch camera_type_tel:=${CAMERATYPE} color:=${color_block}
|
||||
;;
|
||||
*)
|
||||
echo -e "${Error} 错误，请填入正确的数字"
|
||||
;;
|
||||
esac
|
||||
|
||||
}
|
||||
|
||||
#方块抓取示例程序
|
||||
spark_carry_game(){
|
||||
echo -e "${Info}"
|
||||
echo -e "${Info}夺宝奇兵比赛用示例程序"
|
||||
ROSVER=`/usr/bin/rosversion -d`
|
||||
PROJECTPATH=$(cd `dirname $0`; pwd)
|
||||
source ${PROJECTPATH}/devel/setup.bash
|
||||
echo -e "${Info}请选择比赛方式：
|
||||
echo -e "${Info}请选择方式：
|
||||
${Green_font_prefix}1.${Font_color_suffix} 手动模式
|
||||
${Green_font_prefix}2.${Font_color_suffix} 自动模式
|
||||
${Green_font_prefix}3.${Font_color_suffix} 退出请输入：Ctrl + c"
|
||||
|
|
@ -992,6 +1042,23 @@ coming_soon(){
|
|||
}
|
||||
|
||||
|
||||
|
||||
check_camera_game(){
|
||||
if [[ "${GAME_ENABLE}" == "yes" ]]; then
|
||||
check_camera_no_print
|
||||
if [[ "${CAMERATYPE}" == "d435" ]]; then
|
||||
aa='a'
|
||||
else
|
||||
echo -e "
|
||||
${Green_font_prefix} 13.${Font_color_suffix} hsv值自动查找"
|
||||
fi
|
||||
echo -e "
|
||||
${Green_font_prefix} 20.${Font_color_suffix} 竞赛示例程序
|
||||
|
||||
————————————"
|
||||
fi
|
||||
}
|
||||
|
||||
#printf
|
||||
menu_status(){
|
||||
echo -e "${Tip} 当前系统版本 ${OSDescription} !"
|
||||
|
|
@ -1043,10 +1110,9 @@ echo -e "
|
|||
${Green_font_prefix} 11.${Font_color_suffix} 语音控制SPARK移动
|
||||
${Green_font_prefix} 12.${Font_color_suffix} 给摄像头做标定
|
||||
|
||||
${Green_font_prefix} 20.${Font_color_suffix} 比赛程序
|
||||
|
||||
————————————
|
||||
|
||||
————————————"
|
||||
check_camera_game
|
||||
echo -e "
|
||||
${Green_font_prefix}100.${Font_color_suffix} 问题反馈
|
||||
${Green_font_prefix}104.${Font_color_suffix} 文件传输
|
||||
"
|
||||
|
|
@ -1093,6 +1159,9 @@ case "$num" in
|
|||
12)
|
||||
calibrate_camera
|
||||
;;
|
||||
13)
|
||||
spark_hsv_detection
|
||||
;;
|
||||
20)
|
||||
spark_carry_game
|
||||
;;
|
||||
|
|
|
|||
|
|
@ -51,7 +51,7 @@
|
|||
</node>
|
||||
</group>
|
||||
<group if="$(eval arg('camera_type_tel')=='astrapro')">
|
||||
<node pkg="move2grasp" type="grasp_pro.py" name="grasp" />
|
||||
<node pkg="move2grasp" type="grasp_pro_new.py" name="grasp" />
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -15,6 +15,7 @@
|
|||
|
||||
<arg name="camera_type_tel" value="$(arg camera_type_tel)"/>
|
||||
<arg name="lidar_type_tel" value="$(arg lidar_type_tel)"/>
|
||||
<arg name="start_camera" value="true"/>
|
||||
</include>
|
||||
|
||||
|
||||
|
|
@ -53,7 +54,7 @@
|
|||
</node>
|
||||
</group>
|
||||
<group if="$(eval arg('camera_type_tel')=='astrapro')">
|
||||
<node pkg="move2grasp" type="grasp_pro.py" name="grasp" />
|
||||
<node pkg="move2grasp" type="grasp_pro_new.py" name="grasp" output="screen"/>
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,263 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
import os
|
||||
import time
|
||||
import random
|
||||
import ctypes
|
||||
import roslib
|
||||
import rospy
|
||||
import smach
|
||||
import smach_ros
|
||||
import threading
|
||||
import string
|
||||
import math
|
||||
import cv2
|
||||
import numpy as np
|
||||
from geometry_msgs.msg import Twist
|
||||
from std_msgs.msg import String
|
||||
from geometry_msgs.msg import Pose, Point, Quaternion
|
||||
from sensor_msgs.msg import Image
|
||||
from cv_bridge import CvBridge, CvBridgeError
|
||||
from spark_carry_object.msg import *
|
||||
|
||||
|
||||
class GraspObject():
|
||||
'''
|
||||
监听主控，用于物品抓取功能
|
||||
'''
|
||||
|
||||
def __init__(self):
|
||||
'''
|
||||
初始化
|
||||
'''
|
||||
|
||||
global xc, yc, xc_prev, yc_prev, found_count
|
||||
xc = 0
|
||||
yc = 0
|
||||
xc_prev = xc
|
||||
yc_prev = yc
|
||||
found_count = 0
|
||||
self.is_found_object = False
|
||||
# self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
|
||||
# 订阅机械臂抓取指令
|
||||
self.sub2 = rospy.Subscriber(
|
||||
'/grasp', String, self.grasp_cp, queue_size=1)
|
||||
# 发布机械臂位姿
|
||||
self.pub1 = rospy.Publisher(
|
||||
'position_write_topic', position, queue_size=10)
|
||||
# 发布机械臂吸盘
|
||||
self.pub2 = rospy.Publisher('pump_topic', status, queue_size=1)
|
||||
# 发布机械臂状态
|
||||
self.grasp_status_pub = rospy.Publisher(
|
||||
'grasp_status', String, queue_size=1)
|
||||
# 发布TWist消息控制机器人底盘
|
||||
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
|
||||
pos = position()
|
||||
pos.x = 120
|
||||
pos.y = 0
|
||||
pos.z = 35
|
||||
self.pub1.publish(pos)
|
||||
|
||||
def grasp_cp(self, msg):
|
||||
if msg.data == '1':
|
||||
# 订阅摄像头话题,对图像信息进行处理
|
||||
self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
|
||||
self.is_found_object = False
|
||||
rate = rospy.Rate(10)
|
||||
times=0
|
||||
steps=0
|
||||
while not self.is_found_object:
|
||||
rate.sleep()
|
||||
times+=1
|
||||
# 转一圈没有发现可抓取物体,退出抓取
|
||||
if steps>=5:
|
||||
self.sub.unregister()
|
||||
print("stop grasp\n")
|
||||
status=String()
|
||||
status.data='-1'
|
||||
self.grasp_status_pub.publish(status)
|
||||
return
|
||||
# 旋转一定角度扫描是否有可供抓取的物体
|
||||
if times>=30:
|
||||
times=0
|
||||
steps+=1
|
||||
self.turn_body()
|
||||
print("not found\n")
|
||||
print("unregisting sub\n")
|
||||
self.sub.unregister()
|
||||
print("unregisted sub\n")
|
||||
# 抓取检测到的物体
|
||||
self.grasp()
|
||||
status=String()
|
||||
status.data='1'
|
||||
self.grasp_status_pub.publish(status)
|
||||
if msg.data=='0':
|
||||
# 放下物体
|
||||
self.is_found_object = False
|
||||
self.release_object()
|
||||
status=String()
|
||||
status.data='0'
|
||||
self.grasp_status_pub.publish(status)
|
||||
|
||||
# 执行抓取
|
||||
def grasp(self):
|
||||
print("start to grasp\n")
|
||||
global xc, yc, found_count
|
||||
# stop function
|
||||
|
||||
filename = os.environ['HOME'] + "/thefile.txt"
|
||||
file_pix = open(filename, 'r')
|
||||
s = file_pix.read()
|
||||
file_pix.close()
|
||||
print(s)
|
||||
arr=s.split()
|
||||
a1=arr[0]
|
||||
a2=arr[1]
|
||||
a3=arr[2]
|
||||
a4=arr[3]
|
||||
a = [0]*2
|
||||
b = [0]*2
|
||||
a[0]=float(a1)
|
||||
a[1]=float(a2)
|
||||
b[0]=float(a3)
|
||||
b[1]=float(a4)
|
||||
print('k and b value:',a[0],a[1],b[0],b[1])
|
||||
r1 = rospy.Rate(0.095)
|
||||
r2 = rospy.Rate(10)
|
||||
pos = position()
|
||||
# 物体所在坐标+标定误差
|
||||
pos.x = a[0] * yc + a[1]
|
||||
pos.y = b[0] * xc + b[1]
|
||||
pos.z = 20
|
||||
# pos.z = 20
|
||||
print("z = 20\n")
|
||||
self.pub1.publish(pos)
|
||||
r2.sleep()
|
||||
# go down -100
|
||||
pos.z = -50
|
||||
self.pub1.publish(pos)
|
||||
print("z = -83\n")
|
||||
r2.sleep()
|
||||
|
||||
# 开始吸取物体
|
||||
self.pub2.publish(1)
|
||||
r2.sleep()
|
||||
|
||||
# 提起物体
|
||||
pos.x = 250 #160
|
||||
pos.y = 0
|
||||
pos.z = 150 #55
|
||||
self.pub1.publish(pos)
|
||||
r1.sleep()
|
||||
# 使用CV检测物体
|
||||
def image_cb(self, data):
|
||||
global xc, yc, xc_prev, yc_prev, found_count
|
||||
# change to opencv
|
||||
try:
|
||||
cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8")
|
||||
except CvBridgeError as e:
|
||||
print('error')
|
||||
|
||||
# change rgb to hsv
|
||||
cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
|
||||
|
||||
# 蓝色物体颜色检测范围
|
||||
LowerBlue = np.array([95, 70, 60])
|
||||
UpperBlue = np.array([140, 255, 255])
|
||||
mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)
|
||||
cv_image3 = cv2.bitwise_and(cv_image2, cv_image2, mask=mask)
|
||||
|
||||
# gray process
|
||||
cv_image4 = cv_image3[:, :, 0]
|
||||
|
||||
# smooth and clean noise
|
||||
blurred = cv2.blur(cv_image4, (9, 9))
|
||||
(_, thresh) = cv2.threshold(blurred, 90, 255, cv2.THRESH_BINARY)
|
||||
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25))
|
||||
cv_image5 = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)
|
||||
cv_image5 = cv2.erode(cv_image5, None, iterations=4)
|
||||
cv_image5 = cv2.dilate(cv_image5, None, iterations=4)
|
||||
|
||||
# detect contour
|
||||
# cv2.imshow("win1", cv_image3)
|
||||
# cv2.imshow("win2", cv_image5)
|
||||
cv2.waitKey(1)
|
||||
contours, hier = cv2.findContours(cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
|
||||
|
||||
# if find contours, pick the biggest box
|
||||
if len(contours) > 0:
|
||||
size = []
|
||||
size_max = 0
|
||||
for i, c in enumerate(contours):
|
||||
rect = cv2.minAreaRect(c)
|
||||
box = cv2.boxPoints(rect)
|
||||
box = np.int0(box)
|
||||
x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4
|
||||
y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4
|
||||
w = math.sqrt((box[0][0] - box[1][0]) ** 2 + (box[0][1] - box[1][1]) ** 2)
|
||||
h = math.sqrt((box[0][0] - box[3][0]) ** 2 + (box[0][1] - box[3][1]) ** 2)
|
||||
size.append(w * h)
|
||||
if size[i] > size_max:
|
||||
size_max = size[i]
|
||||
index = i
|
||||
xc = x_mid
|
||||
yc = y_mid
|
||||
# if box is not moving for 20 times
|
||||
# print found_count
|
||||
if found_count >= 30:
|
||||
self.is_found_object = True
|
||||
cmd_vel = Twist()
|
||||
self.cmd_vel_pub.publish(cmd_vel)
|
||||
else:
|
||||
# if box is not moving
|
||||
if abs(xc - xc_prev) <= 2 and abs(yc - yc_prev) <= 2:
|
||||
found_count = found_count + 1
|
||||
else:
|
||||
found_count = 0
|
||||
else:
|
||||
found_count = 0
|
||||
xc_prev = xc
|
||||
yc_prev = yc
|
||||
# 释放物体
|
||||
def release_object(self):
|
||||
r1 = rospy.Rate(0.15) # 5s
|
||||
r2 = rospy.Rate(1) # 1s
|
||||
pos = position()
|
||||
# go forward
|
||||
pos.x = 200
|
||||
pos.y = 0
|
||||
pos.z = -40 #-80
|
||||
self.pub1.publish(pos)
|
||||
r1.sleep()
|
||||
|
||||
# stop pump
|
||||
self.pub2.publish(0)
|
||||
r2.sleep()
|
||||
#r1.sleep()
|
||||
pos.x = 120
|
||||
pos.y = 0
|
||||
pos.z = 35
|
||||
self.pub1.publish(pos)
|
||||
r1.sleep()
|
||||
return 'succeeded'
|
||||
# 转动机器人到一定角度
|
||||
def turn_body(self):
|
||||
cmd_vel = Twist()
|
||||
cmd_vel.angular.z = 0.25
|
||||
rate = rospy.Rate(10)
|
||||
for i in range(40):
|
||||
self.cmd_vel_pub.publish(cmd_vel)
|
||||
rate.sleep()
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
rospy.init_node('GraspObject', anonymous=False)
|
||||
rospy.loginfo("Init GraspObject main")
|
||||
GraspObject()
|
||||
rospy.spin()
|
||||
except rospy.ROSInterruptException:
|
||||
rospy.loginfo("End spark GraspObject main")
|
||||
|
||||
|
|
@ -33,7 +33,7 @@ def keyboardLoop():
|
|||
|
||||
#速度变量
|
||||
# 慢速
|
||||
walk_vel_ = rospy.get_param('walk_vel', 0.3)
|
||||
walk_vel_ = rospy.get_param('walk_vel', 0.2)
|
||||
# 快速
|
||||
run_vel_ = rospy.get_param('run_vel', 1.0)
|
||||
yaw_rate_ = rospy.get_param('yaw_rate', 1.0)
|
||||
|
|
@ -72,12 +72,16 @@ def keyboardLoop():
|
|||
msg.data='1'
|
||||
grasp_pub.publish(msg)
|
||||
can_grasp=False
|
||||
speed = 0
|
||||
turn = 0
|
||||
elif ch == 'h':
|
||||
if can_release:
|
||||
msg=String()
|
||||
msg.data='0'
|
||||
grasp_pub.publish(msg)
|
||||
can_release=False
|
||||
speed = 0
|
||||
turn = 0
|
||||
elif ch == 'w':
|
||||
max_tv = walk_vel_
|
||||
speed = 1
|
||||
|
|
@ -124,7 +128,7 @@ def keyboardLoop():
|
|||
pub.publish(cmd)
|
||||
rate.sleep()
|
||||
#停止机器人
|
||||
stop_robot()
|
||||
#stop_robot()
|
||||
|
||||
def stop_robot():
|
||||
cmd.linear.x = 0.0
|
||||
|
|
|
|||
|
|
@ -0,0 +1,153 @@
|
|||
#!/usr/bin/env python
|
||||
# -*- coding: utf-8 -*-
|
||||
import cv2
|
||||
import numpy as np
|
||||
import math
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
from PIL import Image, ImageDraw,ImageFont
|
||||
import rospy
|
||||
from spark_carry_object.msg import *
|
||||
|
||||
def mean_hsv(img,HSV_value):
|
||||
HSV = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
|
||||
HSV_value[0]+=np.mean(HSV[:, :, 0])
|
||||
HSV_value[1]+=np.mean(HSV[:, :, 1])
|
||||
HSV_value[2]+=np.mean(HSV[:, :, 2])
|
||||
return HSV_value
|
||||
|
||||
|
||||
def hsv_range(HSV_value):
|
||||
# 设置HSV颜色值的范围
|
||||
H_range = 12
|
||||
S_range = 120
|
||||
V_range = 120
|
||||
|
||||
lower_H = int(HSV_value[0] - H_range)
|
||||
upper_H = int(HSV_value[0] + H_range)
|
||||
|
||||
lower_S = int(HSV_value[1] - S_range)
|
||||
upper_S = int(HSV_value[1] + S_range)
|
||||
|
||||
lower_V = int(HSV_value[2] - V_range)
|
||||
upper_V = int(HSV_value[2] + V_range)
|
||||
|
||||
if lower_H<0:
|
||||
lower_H=0
|
||||
if upper_H>180:
|
||||
upper_H=180
|
||||
|
||||
if lower_S<50:
|
||||
lower_S=50
|
||||
if upper_S>255:
|
||||
upper_S=255
|
||||
|
||||
if lower_V<50:
|
||||
lower_V=50
|
||||
if upper_V>255:
|
||||
upper_V=255
|
||||
|
||||
lower_HSV = np.array([lower_H, lower_S, lower_V])
|
||||
upper_HSV = np.array([upper_H, upper_S, upper_V])
|
||||
return lower_HSV, upper_HSV
|
||||
|
||||
|
||||
def test(lower_HSV, upper_HSV, image):
|
||||
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
|
||||
mask = cv2.inRange(hsv, lower_HSV, upper_HSV)
|
||||
mask = cv2.erode(mask, None, iterations=2)
|
||||
mask = cv2.dilate(mask, None, iterations=2)
|
||||
mask = cv2.GaussianBlur(mask, (3, 3), 0)
|
||||
cv2.putText(mask, 'Done! Press q to exit!', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.2,
|
||||
(255, 255, 255), 2, cv2.LINE_AA)
|
||||
cv2.imshow("HSV_img", mask)
|
||||
|
||||
def arm_init():
|
||||
pub1 = rospy.Publisher('position_write_topic', position, queue_size=1)
|
||||
|
||||
r1 = rospy.Rate(1)
|
||||
r1.sleep()
|
||||
pos = position()
|
||||
pos.x = 120
|
||||
pos.y = 0
|
||||
pos.z = 35
|
||||
pub1.publish(pos)
|
||||
r1.sleep()
|
||||
|
||||
def save_hsv(name, lower_HSV, upper_HSV):
|
||||
|
||||
content = str(lower_HSV[0]) + ',' +str(lower_HSV[1])+ ',' + str(lower_HSV[2]) \
|
||||
+ ' ' + str(upper_HSV[0])+ ',' + str(upper_HSV[1])+ ',' + str(upper_HSV[2]) + '\n'
|
||||
# 将HSV值写入文件，文件在spark目录下
|
||||
if name == 'color_block':
|
||||
content = "block_HSV is :" + content
|
||||
filename = os.environ['HOME'] + "/color_block_HSV.txt"
|
||||
elif name == 'calibration':
|
||||
content = "calibration_HSV is :" + content
|
||||
filename = os.environ['HOME'] + "/calibration_HSV.txt"
|
||||
|
||||
with open(filename, "w") as f:
|
||||
f.write(content)
|
||||
|
||||
print("HSV value has saved in" + filename)
|
||||
print(content)
|
||||
|
||||
def main():
|
||||
name = rospy.get_param("/spark_hsv_detection/color")
|
||||
arm_init()
|
||||
time.sleep(3)
|
||||
HSV_value = [0,0,0]
|
||||
count = 0
|
||||
capture = cv2.VideoCapture(0)
|
||||
# 颜色块中心点上下左右扩大60个像素
|
||||
box_w = 60
|
||||
# 吸盘上方颜色像素范围
|
||||
cali_w = 20
|
||||
cali_h = 30
|
||||
# 收集300次取平均值
|
||||
collect_times = 300
|
||||
while True:
|
||||
ret, img = capture.read()
|
||||
if img is not None:
|
||||
# 200次以内先做提醒，将颜色块放在矩形框中
|
||||
if count < 200:
|
||||
cv2.putText(img, 'please put the color being', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.2,
|
||||
(0, 255, 0), 2, cv2.LINE_AA)
|
||||
cv2.putText(img, 'tested in rectangle box!', (30, 80), cv2.FONT_HERSHEY_SIMPLEX, 1.2,
|
||||
(0, 255, 0), 2, cv2.LINE_AA)
|
||||
# 如果在200-500次以内开始收集hsv值，并求出平均值
|
||||
elif count > 200 and count < 200+collect_times:
|
||||
cv2.putText(img, 'HSV_value is collecting!', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.2,
|
||||
(0, 255, 0), 2, cv2.LINE_AA)
|
||||
if name == 'color_block':
|
||||
frame = img[120:120 + box_w, 300:300 + box_w]
|
||||
elif name == 'calibration':
|
||||
frame = img[310:310 + cali_h, 355:355 + cali_w]
|
||||
HSV_value = mean_hsv(frame, HSV_value)
|
||||
# 500次以后开始检测，查看是否有提取到矩形框中颜色的HSV值
|
||||
elif count==200+collect_times:
|
||||
for i in range(len(HSV_value)):
|
||||
HSV_value[i] = HSV_value[i] / collect_times
|
||||
lower_HSV, upper_HSV = hsv_range(HSV_value)
|
||||
save_hsv(name, lower_HSV, upper_HSV)
|
||||
|
||||
elif count > 200+collect_times:
|
||||
test(lower_HSV, upper_HSV, img)
|
||||
|
||||
count += 1
|
||||
if name == 'color_block':
|
||||
cv2.rectangle(img, (300, 120), (300+box_w, 120+box_w), (0, 255, 0), 3)
|
||||
elif name == 'calibration':
|
||||
cv2.rectangle(img, (355, 310), (355 + cali_w, 310 + cali_h), (0, 255, 0), 3)
|
||||
cv2.imshow("RGB_img", img)
|
||||
cv2.waitKey(1)
|
||||
if cv2.waitKey(10) == ord('q'):
|
||||
break
|
||||
cv2.destroyAllWindows()
|
||||
return lower_HSV, upper_HSV
|
||||
|
||||
if __name__ == '__main__':
|
||||
rospy.init_node('hsv_detection', anonymous=True)
|
||||
main()
|
||||
rospy.spin()
|
||||
Loading…
Reference in New Issue