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
|
# SPARK技术讨论与反馈QQ群：6646169 8346256
|
||||||
#=================================================
|
#=================================================
|
||||||
|
|
||||||
GAME_ENABLE="yes"
|
GAME_ENABLE="no"
|
||||||
sh_ver="2.0"
|
sh_ver="2.0"
|
||||||
filepath=$(cd "$(dirname "$0")"; pwd)
|
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"
|
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} 没有找到雷达，请确认雷达已正确连接！！"
|
echo -e "${Error} 没有找到雷达，请确认雷达已正确连接！！"
|
||||||
fi
|
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(){
|
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(){
|
spark_carry_game(){
|
||||||
echo -e "${Info}"
|
echo -e "${Info}"
|
||||||
echo -e "${Info}夺宝奇兵比赛用示例程序"
|
echo -e "${Info}夺宝奇兵比赛用示例程序"
|
||||||
ROSVER=`/usr/bin/rosversion -d`
|
ROSVER=`/usr/bin/rosversion -d`
|
||||||
PROJECTPATH=$(cd `dirname $0`; pwd)
|
PROJECTPATH=$(cd `dirname $0`; pwd)
|
||||||
source ${PROJECTPATH}/devel/setup.bash
|
source ${PROJECTPATH}/devel/setup.bash
|
||||||
echo -e "${Info}请选择比赛方式：
|
echo -e "${Info}请选择方式：
|
||||||
${Green_font_prefix}1.${Font_color_suffix} 手动模式
|
${Green_font_prefix}1.${Font_color_suffix} 手动模式
|
||||||
${Green_font_prefix}2.${Font_color_suffix} 自动模式
|
${Green_font_prefix}2.${Font_color_suffix} 自动模式
|
||||||
${Green_font_prefix}3.${Font_color_suffix} 退出请输入：Ctrl + c"
|
${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
|
#printf
|
||||||
menu_status(){
|
menu_status(){
|
||||||
echo -e "${Tip} 当前系统版本 ${OSDescription} !"
|
echo -e "${Tip} 当前系统版本 ${OSDescription} !"
|
||||||
|
|
@ -1043,10 +1110,9 @@ echo -e "
|
||||||
${Green_font_prefix} 11.${Font_color_suffix} 语音控制SPARK移动
|
${Green_font_prefix} 11.${Font_color_suffix} 语音控制SPARK移动
|
||||||
${Green_font_prefix} 12.${Font_color_suffix} 给摄像头做标定
|
${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}100.${Font_color_suffix} 问题反馈
|
||||||
${Green_font_prefix}104.${Font_color_suffix} 文件传输
|
${Green_font_prefix}104.${Font_color_suffix} 文件传输
|
||||||
"
|
"
|
||||||
|
|
@ -1093,6 +1159,9 @@ case "$num" in
|
||||||
12)
|
12)
|
||||||
calibrate_camera
|
calibrate_camera
|
||||||
;;
|
;;
|
||||||
|
13)
|
||||||
|
spark_hsv_detection
|
||||||
|
;;
|
||||||
20)
|
20)
|
||||||
spark_carry_game
|
spark_carry_game
|
||||||
;;
|
;;
|
||||||
|
|
|
||||||
|
|
@ -51,7 +51,7 @@
|
||||||
</node>
|
</node>
|
||||||
</group>
|
</group>
|
||||||
<group if="$(eval arg('camera_type_tel')=='astrapro')">
|
<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>
|
</group>
|
||||||
|
|
||||||
</launch>
|
</launch>
|
||||||
|
|
|
||||||
|
|
@ -15,6 +15,7 @@
|
||||||
|
|
||||||
<arg name="camera_type_tel" value="$(arg camera_type_tel)"/>
|
<arg name="camera_type_tel" value="$(arg camera_type_tel)"/>
|
||||||
<arg name="lidar_type_tel" value="$(arg lidar_type_tel)"/>
|
<arg name="lidar_type_tel" value="$(arg lidar_type_tel)"/>
|
||||||
|
<arg name="start_camera" value="true"/>
|
||||||
</include>
|
</include>
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -53,7 +54,7 @@
|
||||||
</node>
|
</node>
|
||||||
</group>
|
</group>
|
||||||
<group if="$(eval arg('camera_type_tel')=='astrapro')">
|
<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>
|
</group>
|
||||||
|
|
||||||
</launch>
|
</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)
|
run_vel_ = rospy.get_param('run_vel', 1.0)
|
||||||
yaw_rate_ = rospy.get_param('yaw_rate', 1.0)
|
yaw_rate_ = rospy.get_param('yaw_rate', 1.0)
|
||||||
|
|
@ -72,12 +72,16 @@ def keyboardLoop():
|
||||||
msg.data='1'
|
msg.data='1'
|
||||||
grasp_pub.publish(msg)
|
grasp_pub.publish(msg)
|
||||||
can_grasp=False
|
can_grasp=False
|
||||||
|
speed = 0
|
||||||
|
turn = 0
|
||||||
elif ch == 'h':
|
elif ch == 'h':
|
||||||
if can_release:
|
if can_release:
|
||||||
msg=String()
|
msg=String()
|
||||||
msg.data='0'
|
msg.data='0'
|
||||||
grasp_pub.publish(msg)
|
grasp_pub.publish(msg)
|
||||||
can_release=False
|
can_release=False
|
||||||
|
speed = 0
|
||||||
|
turn = 0
|
||||||
elif ch == 'w':
|
elif ch == 'w':
|
||||||
max_tv = walk_vel_
|
max_tv = walk_vel_
|
||||||
speed = 1
|
speed = 1
|
||||||
|
|
@ -124,7 +128,7 @@ def keyboardLoop():
|
||||||
pub.publish(cmd)
|
pub.publish(cmd)
|
||||||
rate.sleep()
|
rate.sleep()
|
||||||
#停止机器人
|
#停止机器人
|
||||||
stop_robot()
|
#stop_robot()
|
||||||
|
|
||||||
def stop_robot():
|
def stop_robot():
|
||||||
cmd.linear.x = 0.0
|
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