Welcome to DOFBOT-Pi repository

Voice Control Color Block Tracking

1. Function Description

After the program runs, when a color block appears in the program screen, press keys to select the color to track (r-red, g-green, b-blue, y-yellow). If the color recognition effect is not good, you can press the c key to enter color calibration mode, complete color block calibration by selecting colors within the color block area. Then wake up the voice module through "Hello yahboom", followed by the voice command "Track and clip color blocks". When moving the color block slowly, the robotic arm will follow the movement, adjusting its posture to keep the center of the color block within the screen center range. Note: This demo does not include the pick-up function. The voice command “Track and clip” will only perform tracking.

2. Function Startup

After entering the docker container, enter the following commands in the docker terminal:

# Start USB camera node and inverse kinematics node
ros2 launch dofbot_info camera_arm_kin.launch.py
# Start speech recognition and broadcast node
ros2 launch yahboom_speech speech.launch.py
# Start voice control color block tracking node
ros2 run dofbot_voice_ctrl color_follow_2D

After the program runs, press keys to select the color to track (r-red, g-green, b-blue, y-yellow). If the color recognition effect is not good, you can press the c key to enter color calibration mode, complete color block calibration by selecting colors within the color block area. Then after waking up with "Hello yahboom", give the command "Track and clip color blocks". When holding the tracked color block and moving it slowly, the robotic arm will follow the color block movement. After waking up with "Hello yahboom", the stop tracking voice command is "Stop following", the robotic arm stops tracking and returns to the initial tracking posture.

3. Code Analysis

After entering the docker container, the source code path is located at:

xxxxxxxxxx
/root/dofbot_ws/src/dofbot_voice_ctrl/dofbot_voice_ctrl/Color/color_follow_2D.py

Import necessary library files:

xxxxxxxxxx
import cv2
import os
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2 as cv
from dofbot_sorting.color_common import *
from std_msgs.msg import Bool,Int16,Int8
from rclpy.node import Node
import rclpy
import threading
from Arm_Lib import Arm_Device
import dofbot_follow.PID as PID

Create robotic arm control object and control the robotic arm to move to gesture recognition posture:

xxxxxxxxxx
self.Arm = Arm_Device()
self.init_joints = [90, 150, 0, 20, 90, 0]
self.Arm.Arm_serial_servo_write6_array(self.init_joints,2000)

Initialize PID:

xxxxxxxxxx
self.xservo_pid = PID.PositionalPID(0.5, 0.1, 0.1)
self.yservo_pid = PID.PositionalPID(0.5, 0.1, 0.1)

Create subscribers:

xxxxxxxxxx
# Create subscriber to subscribe to image topic
self.subscription =
self.create_subscription(Image,'/image_raw',self.ImageCallback,1)
# Create subscriber to subscribe to speech recognition result topic
self.sub_voice =
self.create_subscription(Int8,"voice_result",self.getVoiceResultCallback,1)

Speech recognition result topic callback function getVoiceResultCallBack:

xxxxxxxxxx
def getVoiceResultCallBack(self,msg):
    # If the received speech recognition result topic message is 106, change self.start_flag value to True, indicating that tracking can be performed
    if msg.data == 106:
        self.start_flag = True
        print("Start tracking.")
    # If the received speech recognition result topic message is 76, change self.start_flag value to False, indicating that tracking cannot be performed
    elif msg.data == 76:
        self.start_flag = False
        # Control the robotic arm to move to initial posture
        self.Arm.Arm_serial_servo_write6_array(self.init_joints,2000)
        self.Reset()

Image topic callback function ImageCallback:

xxxxxxxxxx
def ImageCallback(self,color_frame):
    # Process received image topic messages, convert to image
    rgb_image = self.rgb_bridge.imgmsg_to_cv2(color_frame,'rgb8')
    rgb_image = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2BGR)
    result_image = np.copy(rgb_image)  
    key = cv2.waitKey(10)& 0xFF
    # Pass the image as parameter to the process function for color recognition. For the content of the process function, refer to the code analysis in [3D Space Sorting Grasping]\[4. Color Block Color Sorting].
    result_frame, binary = self.process(rgb_image,key)
    show_frame = threading.Thread(target=self.img_out, args=(result_frame,binary,))
    show_frame.start()
    show_frame.join()
    # If the center coordinates of the target color block's circumcircle are not 0 and the radius is greater than 30, it means the target color block is found
    if self.cx!=0 and self.cy!=0 and self.circle_r>30:
        center_x = self.cx
        center_y = self.cy
        # If the center coordinates of the target color block are not within the screen center range and self.start_flag value is True, indicating that tracking can be performed
        if (abs(center_x-320) >10 or abs(center_y-240)>10) and self.start_flag == True:
            self.XY_track(center_x,center_y)
            print("Tracking")
            print("-------------------------------------" )