Welcome to DOFBOT-Ultra repository

Voice Control Color Sorting And Stacking

If using a depth camera (DABAI_DCW2), before running this example, be sure to execute ros2 run dofbot_ultra_driver calibrate_6point for a six-point calibration, otherwise the robot arm cannot accurately locate the target.

Please ensure that the web control program backend process is not running in the background. To close it, refer to the [Get Started Quickly - 2. Access the Terminal] section in [Disable the Auto-Start Web Control Backend Process]. After closing, please restart DofbotUltra!!!

This case requires placing building blocks within the camera's recognizable range, using 30x30x30mm building blocks, otherwise the arm cannot accurately grasp them.

1. Operation Instructions

1.1. Startup

Before running the function, you need to shut down the web control program backend process. For the shutdown method, please refer to the [4. Preparation - 1. Manage Web control services].

Raspberry Pi version (within Docker), after starting the ROS2 core node, run the following commands:

Step 1 - Start the ROS2 core node:

ros2 run dofbot_ultra_info kinematics_dofbotultra

HSV calibration:

xxxxxxxxxx
Raspberry Pi: python3 /home/yahboom/dofbot_ultra/2d/dofbot_color_identify/scripts/hsv_calibration_gui.py
RDK X5: python3 /home/sunrise/dofbot_ultra_ws/src/2d/dofbot_color_identify/scripts/hsv_calibration_gui.py

Step 2 - Start the color sorting program:

xxxxxxxxxx
python3 /home/yahboom/dofbot_ultra/2d/dofbot_voice/scripts/voice_color_sorting.py

1.2. Operation Description

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.

2. Code Logic

This case integrates voice control, color recognition, and inverse kinematics, implementing the "voice command -> color recognition -> inverse kinematics solution -> arm motion" workflow.

Workflow:

1. Voice module receives and parses the user's command

2. The identify_GetTarget module detects and locates the colored block within the camera range (center point coordinates)

3. Color detection uses HSV threshold segmentation to identify blocks of different colors

4. Coordinates are sent to the ROS2 inverse kinematics service (dofbot_kinematics) which calculates joint angles

5. The arm executes the grasping and sorting motion, completing one color per cycle

In the program, the kinematics_dofbotultra node provides an inverse kinematics service, communicating via the Kinematics service type (dofbot_ultra_interface.srv.Kinematics).

3. Source Code

Raspberry Pi: /home/yahboom/dofbot_ultra/2d/dofbot_voice/scripts/voice_color_sorting.py

RDK X5: /home/sunrise/dofbot_ultra_ws/src/2d/dofbot_voice/scripts/voice_color_sorting.py

Identification module:

Raspberry Pi: /home/yahboom/dofbot_ultra/2d/dofbot_voice/scripts/speech_identify_target.py

RDK X5: /home/sunrise/dofbot_ultra_ws/src/2d/dofbot_voice/scripts/speech_identify_target.py

​x
from speech_identify_target import identify_GetTarget

target = identify_GetTarget()
img, msg, coo1, coo2, coo3, coo4 = target.select_color(img, color_hsv, color_list)
threading.Thread(target=target.target_run, args=(msg, xy)).start()

In speech_identify_target:

xxxxxxxxxx
def select_color(self, image, color_hsv, color_list):
    self.image = image.copy()
    msg = {}
    if len(color_list) == 0:
        return self.image, msg
    if '1' in color_list:
        self.color_name = color_list['1']
        pos = self.get_Sqaure(color_hsv[self.color_name])
        if pos != None:
            msg[self.color_name] = pos
    return self.image, msg

def target_run(self, msg, xy=None):
    if xy != None:
        self.xy = xy
    for name, pos in msg.items():
        joints = self.server_joint(pos)
        self.grap.identify_move(str(name), joints)

def server_joint(self, posxy):
    request = Kinematics.Request()
    request.tar_x = posxy[0]
    request.tar_y = posxy[1]
    request.tar_z = 0.02
    request.kin_name = "ik"
    future = self.client.call_async(request)
    rclpy.spin_until_future_complete(self, future)
    response = future.result()
    if response is not None:
        joints = [0.0, 0.0, 0.0, 0.0, 0.0]
        joints[0] = response.joint1
        joints[1] = response.joint2
        joints[2] = response.joint3
        joints[3] = response.joint4
        joints[4] = posxy[2] + 90
        return joints

• select_color() - Select the specified color block by color list, return position coordinates

• target_run() - Execute target sorting, call inverse kinematics and grasping motion

• server_joint() - Call the ROS2 Kinematics inverse kinematics service, convert (x, y, z) coordinates to 5 joint angles