Welcome to DOFBOT-Pi repository

Voice Control AprilTag Tracking

1. Function Description

After the program runs, wake up the voice module through "Hello yahboom", then give the voice command "Track and clip machine code". The robotic arm will track the AprilTag according to the given command. When the AprilTag is moved slowly, the robotic arm will follow the movement. Wake up the voice module again through "Hello yahboom", give the voice command "stop following", and the robotic arm will stop tracking the AprilTag.

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 AprilTag tracking node
ros2 run dofbot_voice_ctrl apriltag_follow_2D

Phenomenon: After the program runs, after waking up with "Hello yahboom", give the command "Track and clip machine code". When holding the AprilTag wooden block and moving it slowly, the robotic arm will follow the AprilTag 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/AprilTag/apriltag_follow_2D.py

Import necessary library files:

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

Create robotic arm control object and control the robotic arm to move to initial 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)

Create publishers and subscribers:

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

Speech recognition result topic callback function getVoiceResultCallBack:

xxxxxxxxxx
def getVoiceResultCallBack(self,msg):
    # If the received speech recognition result topic message is 105, change self.start_flag value to True, indicating that tracking can be performed
    if msg.data == 105:
        self.start_flag = True
    # 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)

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')
    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) 
    draw_tags(rgb_image, tags, corners_color=(0, 0, 255), center_color=(0, 255, 0))
    frame1 = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2BGR)
    cv2.imshow("result_image", frame1)
    key = cv2.waitKey(1)
    # AprilTag recognized
    if len(tags) > 0 :
        cur_id = tags[0].tag_id
        # Get the center point coordinates of the AprilTag
        center_x, center_y = tags[0].center
        # If the AprilTag center point coordinates 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:
            # Call XY_track function, pass center point coordinates, start tracking
            self.XY_track(center_x,center_y)
            print("Tracking")
            print("-------------------------------------")