Welcome to DOFBOT-JetsonNANO repository

Voice Control Color Block Sorting

1. Function Description

After the program runs, wake up the voice module through "Hello yahboom", then give voice commands "Sort the red block"/"Sort the green block"/"Sort the blue block"/"Sort the yellow block". The robotic arm will grasp the target color blocks on the desktop according to the given command, place them at the set position, and announce "Placement complete".

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 color block recognition program
ros2 run dofbot_voice_ctrl color_sorting
# Start grasping node
ros2 run dofbot_sorting grasp

After the program runs, after waking up with "Hello yahboom", give commands "Sort the red block"/"Sort the green block"/"Sort the blue block"/"Sort the yellow block", the robotic arm will grasp the wooden blocks with corresponding colors on the desktop according to the voice command. After grasping and placement is complete, the voice module will announce "Placement complete". If the target color block is not found, the voice module will announce "Stopped". If the color block recognition effect is not good, you need to recalibrate the color block colors. The calibration method is to run the program in [13.3D Sorting and Grasping Course]-[4.Block color sorting] and perform color calibration according to the tutorial in that section.

3. Code Analysis

3.1 color_sorting

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_sorting.py

Import necessary libraries:

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 dofbot_interface.srv import Kinemarics
from dofbot_interface.msg import *
from std_msgs.msg import Bool,Int16,Int8
import time
from rclpy.node import Node
import rclpy
import threading
from Arm_Lib import Arm_Device

Initialize variables to record target color block color:

xxxxxxxxxx
self.cur_target_color = 0
self.target_color = 0

Load color HSV file paths:

xxxxxxxxxx
package_pwd =  "/root/dofbot_ws/src/dofbot_sorting/dofbot_sorting/"
self.red_hsv_text = os.path.join(package_pwd, 'red_colorHSV.text')
self.green_hsv_text = os.path.join(package_pwd, 'green_colorHSV.text')
self.blue_hsv_text = os.path.join(package_pwd, 'blue_colorHSV.text')
self.yellow_hsv_text = os.path.join(package_pwd, 'yellow_colorHSV.text')

Create publishers and subscribers:

xxxxxxxxxx
# Create subscriber to subscribe to grasp placement completion topic messages
self.sub_grasp_status = self.create_subscription(Bool,"grasp_done",self.get_graspStatusCallBack,100)
# Create publisher to publish recognized position information topic messages
self.pos_info_pub = self.create_publisher(AprilTagInfo,"PosInfo",1)     
# Create publisher to publish calculated servo 5 angle reference value topic messages
self.pub_joint5 = self.create_publisher(Int16, "set_joint5", 10)
# 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)
# Create publisher to publish voice broadcast content topic messages
self.pub_playID = self.create_publisher(Int8,"player_id", 1)

Grasp placement completion topic callback function get_graspStatusCallBack:

xxxxxxxxxx
def get_graspStatusCallBack(self,msg):
    # If the received topic is True, it means the grasp placement action is completed
    if msg.data == True:
        # Create voice broadcast content topic message object
        play_id = Int8()
        # Assign data to voice broadcast content topic message, broadcast audio sequence number 81, "Placement complete"
        play_id.data = 81
        # Publish voice broadcast content topic message
        self.pub_playID.publish(play_id)
        time.sleep(2)
        self.pubPos_flag = True

Speech recognition result topic callback function getVoiceResultCallBack:

xxxxxxxxxx
def getVoiceResultCallback(self,msg): 
    # Change self.target_color and self.cur_target_color values based on speech recognition result, and read HSV values
    if msg.data == 100:
        self.target_color = "red"
        self.cur_target_color = 1
        self.hsv_range = read_HSV(self.red_hsv_text)
        self.pubPos_flag = True
    elif msg.data == 101:
        self.target_color = "green"
        self.cur_target_color = 2
        self.hsv_range = read_HSV(self.green_hsv_text)
        self.pubPos_flag = True
    elif msg.data == 102:
        self.target_color = "blue"
        self.cur_target_color = 3
        self.hsv_range = read_HSV(self.blue_hsv_text)
        self.pubPos_flag = True
    elif msg.data == 103:
        self.target_color = "yellow"
        self.cur_target_color = 4
        self.hsv_range = read_HSV(self.yellow_hsv_text)
        self.pubPos_flag = True
    print("Get the target color is ",self.target_color)
    # Change self.Track_state value to "identify", indicating that color recognition can be performed
    self.Track_state = "identify"

Image topic processing 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 image as parameter to process function for color recognition. For process function content, refer to the code analysis in [3D Space Sorting Grasping]\[4. Color Block Color Sorting].
    result_frame, binary = self.process(rgb_image)
    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 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 :
        cx = int(self.cx)
        cy = int(self.cy)
        # Based on the center coordinates of the target color block's circumcircle, convert coordinates to normalized coefficients a and b, x-axis offset coefficient a, y-axis offset coefficient b, the calculated ba value is the center point coordinates in world coordinate system, this value needs to be sent to the sorting grasp node.
        (a, b) = (round(((320 - cx) / 4000), 5), round(((480 - cy) / 3000) * 0.8+0.15, 5))
        # Calculate servo 5 angle reference value based on corner offset angle
        vx = self.corners[0][0][0] - self.corners[1][0][0]
        vy = self.corners[0][0][1] - self.corners[1][0][1]
        target_joint5 = compute_joint5(vx,vy)
        print("target_joint5: ",target_joint5)
        # If self.pubPos_flag value is True, it means position topic message can be published
        if self.pubPos_flag == True:
            self.pubPos_flag = False
            joint5 = Int16()
            joint5.data = int(target_joint5)
            self.pub_joint5.publish(joint5)
            # Create position information topic message object and assign data
            pos = AprilTagInfo()
            pos.id = self.cur_target_color
            pos.x = float(b)
            pos.y = float(a)
            pos.z = 0.03
            # Publish position information topic
            self.pos_info_pub.publish(pos)  
    # If target color block is not found, then publish voice broadcast content topic, broadcast audio sequence number 1, "Stopped"        
    elif self.circle_r<30 and self.pubPos_flag == True:
        self.Reset()
        self.pubPos_flag = False
        play_id = Int8()
        play_id.data = 1
        self.pub_playID.publish(play_id)

4. Node Communication Graph

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

xxxxxxxxxx
ros2 run rqt_graph rqt_graph