diff --git a/onekey.sh b/onekey.sh
index f1c5545..b7edf8c 100755
--- a/onekey.sh
+++ b/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
;;
diff --git a/src/3rd_app/move2grasp/launch/move2grasp.launch b/src/3rd_app/move2grasp/launch/move2grasp.launch
index e7d4207..dcd279a 100755
--- a/src/3rd_app/move2grasp/launch/move2grasp.launch
+++ b/src/3rd_app/move2grasp/launch/move2grasp.launch
@@ -51,7 +51,7 @@
-
+
diff --git a/src/3rd_app/move2grasp/launch/teleop2grasp.launch b/src/3rd_app/move2grasp/launch/teleop2grasp.launch
index 10234bf..8673fd8 100755
--- a/src/3rd_app/move2grasp/launch/teleop2grasp.launch
+++ b/src/3rd_app/move2grasp/launch/teleop2grasp.launch
@@ -15,6 +15,7 @@
+
@@ -53,7 +54,7 @@
-
+
diff --git a/src/3rd_app/move2grasp/scripts/grasp_pro_new.py b/src/3rd_app/move2grasp/scripts/grasp_pro_new.py
new file mode 100755
index 0000000..fd1db65
--- /dev/null
+++ b/src/3rd_app/move2grasp/scripts/grasp_pro_new.py
@@ -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")
+
diff --git a/src/3rd_app/move2grasp/scripts/teleop.py b/src/3rd_app/move2grasp/scripts/teleop.py
index d3ea438..222ce46 100755
--- a/src/3rd_app/move2grasp/scripts/teleop.py
+++ b/src/3rd_app/move2grasp/scripts/teleop.py
@@ -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
diff --git a/src/spark_app/spark_carry/nodes/hsv_detection.py b/src/spark_app/spark_carry/nodes/hsv_detection.py
new file mode 100755
index 0000000..86b5f49
--- /dev/null
+++ b/src/spark_app/spark_carry/nodes/hsv_detection.py
@@ -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()