Welcome to DOFBOT-JetsonNANO repository
10.Voice Control Color Sorting and Stacking
Voice Control Color Sorting and Stacking
Before running the function, you need to close the App and large programs. For the closing method, refer to [4.Preparation] - [1. Manage APP control services].
First, enter the Docker container; then, execute the instructions provided in the tutorial within the container to launch the program.
1. Function Description
Through voice commands for color block sorting order, the robotic arm will sort color blocks according to the given order.
2. Startup and Operation
2.1. Startup
First start the ROS node service. Open the terminal on the main board and enter the following:
ros2 run dofbot_info kinemarics_dofbotThen enter the following command in the terminal to start:
xxxxxxxxxxpython3 ~/Dofbot_2D/src/dofbot_voice/scripts/voice_color_sorting.py2.2. Operation Steps
After the program runs, the voice broadcast module will broadcast the colors of all blocks in the box. For example, in the figure below, it will broadcast "I see red, blue, yellow, green".
Then it will broadcast "The first color to sort is:". At this time, wake up the voice module with the keyword "Hello, yahboom" and say the first color to sort, for example "red". The program will record the first color to sort, then broadcast "The second color to sort is:", then continue to wake up with the keyword and say the color. If there is a color that doesn't need sorting, you can say "No". After sorting is completed, it will broadcast "Sorting completed, whether to start sorting". At this time, say "Start sorting", it will broadcast "ok", then execute the sorting action.
Note: If colors cannot be recognized, you may need to modify the HSV values in the program or adjust the ambient lighting.
3. Core Code Analysis
Source code path: ~/Dofbot_2D/src/dofbot_voice/scripts/voice_color_sorting.py
x#!/usr/bin/env python# coding: utf-8from Speech_Lib import SpeechmySpeech = Speech()import syssys.path.append('/root/Dofbot_2D/src/dofbot_voice/scripts')import cv2 as cvimport threadingfrom time import sleepimport ipywidgets as widgetsfrom IPython.display import displayfrom speech_identify_target import identify_GetTargetfrom dofbot_utils.fps import FPSfrom dofbot_utils.dofbot_config import *## Create target acquisition instancetarget = identify_GetTarget()# Create camera calibration instancecalibration = Arm_Calibration()# Initialize some parametersnum=0dp = []xy=[90,106]msg = {}threshold = 116model = "General"color_list = {}# Initialize HSV valuescolor_hsv = {"red" : ((0, 43, 46), (10, 255, 255)), "green" : ((35, 43, 46), (77, 255, 255)), "blue" : ((100, 43, 46), (124, 255, 255)), "yellow": ((21, 43, 46), (28, 255, 255))}HSV_path="/root/Dofbot_2D/src/dofbot_voice/scripts/HSV_config.txt"# XYT parameter pathXYT_path="/root/Dofbot_2D/src/dofbot_voice/scripts/XYT_config.txt"try: read_HSV(HSV_path,color_hsv)except Exception: print("Read HSV_config Error !!!")try: xy, threshold = read_XYT(XYT_path)except Exception: print("Read XYT_config Error !!!")import Arm_Lib# Create robotic arm drive instanceimport timeArm = Arm_Lib.Arm_Device()joints_0 = [xy[0], xy[1], 0, 0, 90, 30]Arm.Arm_serial_servo_write6_array(joints_0, 1000)fps = FPS()color_list_one = ['red', 'green', 'blue', 'yellow', 'none']# Gripper controldef target_detection_Callback(value): global model model = 'Detection' with output: print(model)def reset_color_list_Callback(value): global model model = 'Reset_list' with output: print(model)def grap_Callback(value): global model model = 'Grap' with output: print(model)def exit_button_Callback(value): global model model = 'Exit' with output: print(model)def camera(): global color_hsv,model,dp,msg,color_list,color_read # Open camera capture = cv.VideoCapture(0, cv.CAP_V4L2) capture.set(cv.CAP_PROP_FRAME_WIDTH, 640) capture.set(cv.CAP_PROP_FRAME_HEIGHT, 480) # Loop execution when camera is properly opened dp = [] while capture.isOpened(): try: result = mySpeech.speech_read() time.sleep(0.01) # Read each frame of the camera _, img = capture.read() fps.update_fps() if model == 'Calibration': time.sleep(0.5) dp, img = calibration.calibration_map(img, xy, threshold) if len(dp) != 0: model="Detection2" continue if len(dp) != 0: img = calibration.Perspective_transform(dp, img) if model == 'calibration_Cancel': dp = [] msg= {} color_read = 0 model="General" if model == 'Detection2': color_read = 0 color_list['1'] = "red" color_list['2'] = "green" color_list['3'] = "blue" color_list['4'] = "yellow" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv, color_list) time.sleep(0.1) mySpeech.void_write(88) time.sleep(2.0) print("-+-+-+-+-+") if coo1 == 'red': color_read += 1 mySpeech.void_write(46) time.sleep(2.0) if coo2 == 'green': color_read += 1 mySpeech.void_write(49) time.sleep(2.0) if coo3 == 'blue': color_read += 1 mySpeech.void_write(48) time.sleep(2.0) if coo4 == 'yellow': color_read += 1 mySpeech.void_write(47) time.sleep(2.0) mySpeech.void_write(89) time.sleep(2.0) model="list1" if model == 'list1' and color_read > 0: if result == 87: color_list['1'] = "red" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist1" elif result == 88: color_list['1'] = "green" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist1" elif result == 89: color_list['1'] = "blue" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist1" elif result == 90: color_list['1'] = "yellow" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist1" elif result == 91: img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist1" if model == 'tolist1': if color_read > 0: mySpeech.void_write(84) model="list2" elif color_read == 0: mySpeech.void_write(92) model="toGrap" if model == 'list2' and color_read > 0: if result == 87: color_list['2'] = "red" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist2" elif result == 88: color_list['2'] = "green" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist2" elif result == 89: color_list['2'] = "blue" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist2" elif result == 90: color_list['2'] = "yellow" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist2" elif result == 91: img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist2" if model == 'tolist2': if color_read > 0: mySpeech.void_write(85) model="list3" elif color_read == 0: mySpeech.void_write(92) model="toGrap" if model == 'list3' and color_read > 0: if result == 87: color_list['3'] = "red" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist3" elif result == 88: color_list['3'] = "green" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist3" elif result == 89: color_list['3'] = "blue" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist3" elif result == 90: color_list['3'] = "yellow" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist3" elif result == 91: img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist3" if model == 'tolist3': if color_read > 0: mySpeech.void_write(86) model="list4" elif color_read == 0: mySpeech.void_write(92) model="toGrap" if model == 'list4' and color_read > 0: if result == 87: color_list['4'] = "red" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist4" elif result == 88: color_list['4'] = "green" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist4" elif result == 89: color_list['4'] = "blue" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist4" elif result == 90: color_list['4'] = "yellow" img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist4" elif result == 91: img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) result = 0 color_read -= 1 model="tolist4" if model == 'tolist4': mySpeech.void_write(92) time.sleep(2.0) model="toGrap" if model == 'toGrap': img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) #coo = {} #img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv,color_list) if result == 92: result = 0 model="Grap" if result == 109: result = 0 msg={} color_list = {} model="Detection2" mySpeech.void_write(45) time.sleep(2.0) if model=="Reset_list": #print(coo1) #print(coo2) #print(coo3) #print(coo4) msg={} color_list = {} model="General" if len(msg)!= 0 and model == 'Grap': threading.Thread(target=target.target_run, args=(msg,xy)).start() msg={} model="General" if model == 'Exit': cv.destroyAllWindows() capture.release() break cv.imshow("img", img) key = cv.waitKey(1) except KeyboardInterrupt:capture.release() # First step is automatic calibrationmodel = 'Calibration'threading.Thread(target=camera, ).start()