Welcome to DOFBOT-JetsonNANO repository

Voice Control AprilTag ID Sorting

1. Function Description

After the program runs, wake up the voice module through "Hello yahboom", then give voice commands "Sorting machine code number one"/"Sorting machine code number two"/"Sorting machine code number three"/"Sorting machine code number four". The robotic arm will grasp the AprilTag wooden 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 AprilTag recognition program node
ros2 run dofbot_voice_ctrl apriltag_detect
# Start grasping node
ros2 run dofbot_sorting grasp

Phenomenon: After the program runs, after waking up with "Hello yahboom", give commands "Sorting machine code number one/Sorting machine code number two/Sorting machine code number three/Sorting machine code number four", the robotic arm will grasp the AprilTag wooden blocks with the corresponding ID on the desktop according to the voice command. After grasping and placement is complete, the voice module will announce "Placement complete". If the target ID AprilTag is not found, the voice module will announce "Stopped".

3. Code Analysis

3.1 speech.launch.py

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

xxxxxxxxxx
/root/dofbot_ws/src/yahboom_speech/launch/speech.launch.py

​x
# Starts two nodes, one is voice_player_node voice broadcast node, one is speech_recognize_node speech recognition node
from launch import LaunchDescription
from launch_ros.actions import Node
import os
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
​
def generate_launch_description():
​
    play_node = Node(
     package='yahboom_speech',
     executable='voice_player',
     name='voice_player_node',
    )
​
    recognize_node = Node(
     package='yahboom_speech',
     executable='speech_recognize',
     name='speech_recognize_node',
    )
    
    return LaunchDescription([play_node,recognize_node])

3.1.1 Speech_Lib Library

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

xxxxxxxxxx
/root/py_install_v0.0.3/Speech_Lib/Speech_Lib.py

xxxxxxxxxx
import time
import threading
import sys
import serial
import os
class Speech(object):
    def __init__(self,path = '~/speech_music/', com="/dev/myspeech"):
        self.ser = serial.Serial(com, 115200)
        self.language = 'cn' # Default is Chinese
        self.path = path
        
        if self.ser.isOpen():
            print("Speech Serial Opened! Baudrate=115200")
        else:
            print("Speech Serial Open Failed!")
​
    def __del__(self):
        self.ser.close()
        print("speech serial Close!")
       
    # Play audio
    def play_voide(self,viocenum): 
        if self.language == 'cn':
            stryy = self.path+'answer'+str(viocenum)+'.mp3'
            try:
                os.system("mpg123 "+stryy+"< /dev/null > /dev/null 2>1 &")
                print("Finish play.")
            except:
                print("音频找不到")
        else:
            stryy = self.path+str(viocenum)+'_en.mp3'
            try:
                os.system("mpg123 "+stryy+"< /dev/null > /dev/null 2>1 &")
            except:
                print("Audio not found!")
    # Select broadcast statement
    def void_write(self, void_data): # Upper layer control audio playback interface
        if self.language == 'cn':
            stryy = self.path+'answer'+str(void_data)+'.mp3'
            try:
                os.system("mpg123 "+stryy+"< /dev/null > /dev/null 2>1 &")
            except:
                print("音频找不到")
        else:
            stryy = self.path+'answer'+str(void_data)+'_en.mp3'
            try:
                os.system("mpg123 "+stryy+"< /dev/null > /dev/null 2>1 &")
            except:
                print("Audio not found!")
    # Read recognized speech
    def speech_read(self):
        count = self.ser.inWaiting()
        #print("data:"+str(count))
        if count:
            speech_data = self.ser.read(count)
            hex_data = speech_data.hex()
            
                # Extract '00' and '03' parts
            byte1 = hex_data[4:6]  # Extract '00'
            byte2 = hex_data[6:8]  # Extract '03'
                # Convert hexadecimal to integer
            value1 = int(byte1, 16)
            value2 = int(byte2, 16)
            
            if value1 != 0 and value1 <4: # Is Chinese wake-up
                self.language = 'cn'
                self.play_voide(0)# Broadcast Chinese audio
                return 0
            elif value1 != 0 and value1 >=4:
                self.language = 'en'
                self.play_voide(0)# Broadcast English audio
                return 0
                
            self.ser.flushInput()
            time.sleep(0.005)
            return value2
        else:
            return 999

Folder path for storing voice broadcast audio files:

xxxxxxxxxx
/root/speech_music

3.1.2 speech_recognize_node Speech Recognition Node

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

xxxxxxxxxx
/root/dofbot_ws/src/yahboom_speech/yahboom_speech/speech_recognize.py

Import necessary libraries:

xxxxxxxxxx
import time
import rclpy
from rclpy.node import Node
from std_msgs.msg import Int8
from Speech_Lib import Speech

Create speech object:

xxxxxxxxxx
mySpeech = Speech()

Create publisher to publish speech recognition results:

xxxxxxxxxx
self.pub_res = self.create_publisher(Int8,'voice_result',1)

Create speech recognition result topic message object:

xxxxxxxxxx
self.voice_command = Int8()

Enter loop to detect speech events:

xxxxxxxxxx
while True:
    # Call speech object's speech_read function to get speech recognition result
    result = mySpeech.speech_read()
    #print("resulr",result)
    # If speech recognition result is not 999 and 0, it means the speech recognition result is valid and not a wake word
    if result!=999 and result!=0:
        # Assign data to speech recognition result object
        self.voice_command.data = result
        # Publish speech recognition result topic message
        self.pub_res.publish(self.voice_command) 
        print("result: ",result)

3.1.3 voice_player Voice Broadcast Node

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

xxxxxxxxxx
/root/dofbot_ws/src/yahboom_speech/yahboom_speech/voice_player.py

Import necessary libraries:

xxxxxxxxxx
import time
import rclpy
from rclpy.node import Node
from std_msgs.msg import Int8
from Speech_Lib import Speech

Create speech object:

xxxxxxxxxx
mySpeech = Speech()

Create subscriber to subscribe to voice broadcast content topic:

xxxxxxxxxx
self.sub_playID = self.create_subscription(Int8,'player_id',self.playercallback,1000)

Voice broadcast content topic callback function playercallback:

xxxxxxxxxx
def playercallback(self,msg):
    ID_value = msg.data
    print("ID_value: ",ID_value)
    if ID_value!=0:
        play_id = ID_value
        print("play_id = ",play_id)
        # Call speech object's void_write function to broadcast voice content
        mySpeech.void_write(int(play_id))

3.2 apriltag_detect

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

xxxxxxxxxx
/root/dofbot_ws/src/dofbot_voice_ctrl/dofbot_voice_ctrl/AprilTag/apriltag_detect.py

Import necessary libraries:

xxxxxxxxxx
import rclpy
from rclpy.node import Node
import cv2 
import numpy as np
from sensor_msgs.msg import Image
from std_msgs.msg import Float32, Bool,Int16,Int8
from cv_bridge import CvBridge
import cv2 as cv
from dt_apriltags import Detector
from dofbot_sorting.vutils import draw_tags
from dofbot_interface.msg import *
import time
import math
from Arm_Lib import Arm_Device
import os
from dofbot_sorting.compute_joint5 import *

Initialize target sorting AprilTag ID:

xxxxxxxxxx
self.set_id = 0

Create publishers and subscribers:

xxxxxxxxxx
# Create publisher to publish recognized position information topic messages
self.pos_info_pub = self.create_publisher(AprilTagInfo, "PosInfo", qos_profile=10)
# 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 grasp placement completion topic messages
self.subscription = self.create_subscription(Bool,'grasp_done',self.GraspStatusCallback,qos_profile=1)
# 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 GraspStatusCallback:

xxxxxxxxxx
def GraspStatusCallback(self,msg):
    print("**")
    # 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.0)
        self.pubPos_flag = True
        self.detect_flag = False

Speech recognition result topic callback function getVoiceResultCallBack:

xxxxxxxxxx
def getVoiceResultCallBack(self,msg):
    # Change target AprilTag ID value based on speech recognition result
    if msg.data == 95:
        self.set_id = 1 
    elif msg.data == 96:
        self.set_id = 2
    elif msg.data == 97:
        self.set_id = 3
    elif msg.data == 98:
        self.set_id = 4       
    print("id:",self.set_id)
    # Change pubPos_flag value to True, indicating that position message topic can be published
    self.pubPos_flag = True

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')
    result_image = np.copy(rgb_image)
    tags = self.at_detector.detect(cv2.cvtColor(rgb_image, cv2.COLOR_RGB2GRAY), False, None, 0.025)
    tags = sorted(tags, key=lambda tag: tag.tag_id) # Seems to be sorted in ascending order by default, no need for manual sorting
    draw_tags(result_image, tags, corners_color=(0, 0, 255), center_color=(0, 255, 0))
    key = cv2.waitKey(10)
    # Find AprilTag
    if len(tags) > 0 :
        # Traverse AprilTag list
        for i in range(len(tags)):    
            # If currently recognized AprilTag id equals set target sorting AprilTag id
            if self.pubPos_flag == True and tags[i].tag_id == self.set_id:
                # Change self.detect_flag value to True, indicating target AprilTag found
                self.detect_flag = True
                # Change self.detect_flag value to False, prevent repeated publishing of position information topic
                self.pubPos_flag = False
                # Get AprilTag center coordinates
                center_x, center_y = tags[i].center                
                cx = center_x
                cy = center_y
                # Based on AprilTag center coordinates, 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))
                print("a: ",a)
                print("b: ",b)
 KCF Object Tracking
Voice Control Color Block Sorting