Welcome to DOFBOT-Ultra repository

Voice Control Remove Height Abnormal Color Block

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!!!

Before starting this function, you need to close the web control program backend process.

1. Function Description

After the program starts, issue a command via voice for the robotic arm to remove color blocks with abnormal height. Upon receiving the command, the robotic arm will reach down and pick up color blocks exceeding the height threshold. (This section is only for the depth camera version.)

2. Startup and Operation

2.1. Startup

xxxxxxxxxx
# Start camera and inverse kinematics
ros2 launch dofbot_ultra_info camera_arm_kin.launch.py

# Start robotic arm driver
ros2 run dofbot_ultra_driver arm_driver

# Start speech recognition and broadcast
ros2 launch dofbot_yahboom_speech speech.launch.py

# Start voice router
ros2 run dofbot_ultra_voice_ctrl voice_router

# Start height abnormal color block detection
ros2 run dofbot_ultra_color AnomalyHeightRemoval

# Start robotic arm grasping program
ros2 run dofbot_ultra_driver grasp

# Start voice broadcast
ros2 run dofbot_yahboom_speech voice_player

# Depth camera exposure settings
ros2 service call /camera/set_color_exposure orbbec_camera_msgs/srv/SetInt32 '{data: 30}'
ros2 service call /camera/set_depth_auto_exposure std_srvs/srv/SetBool '{data: true}'

For RDK:

xxxxxxxxxx
ros2 launch dofbot_ultra_info camera_arm_kin.launch.py
ros2 run dofbot_ultra_driver arm_driver
ros2 launch dofbot_yahboom_speech speech.launch.py
ros2 run dofbot_ultra_color AnomalyHeightRemoval
ros2 run dofbot_ultra_driver grasp

# Depth camera exposure settings
ros2 service call /camera/set_color_exposure orbbec_camera_msgs/srv/SetInt32 '{data: 30}'
ros2 service call /camera/set_depth_auto_exposure std_srvs/srv/SetBool '{data: true}'

2.2. Operation Steps

2.2.1. Calibrating Color Blocks

Place color blocks of the same color but different heights in the camera frame. Use the mouse to select the top area of any color block, then release the mouse to complete the color calibration. At this point, the height of the color block will also be displayed in the image, as shown below.

2.2.2. Voice Control

After color calibration is completed, say "Hello, yahboom" to the speech recognition module, and the speaker will broadcast "here". Then say "Remove color blocks with abnormal height" to the speech module. The robotic arm will lower its gripper to grasp color blocks higher than 5cm, and the speech module will broadcast "OK". After grasping the height-abnormal color block, it will place it at the designated position. Finally, the robotic arm returns to its initial posture, and the speech broadcast module will announce "Placement complete".

3. Core Code Analysis

1. Voice Routing Distribution (Voice Router)

File: dofbot_ultra_voice_ctrl/voice_router.py

xxxxxxxxxx
# When a voice command is received (e.g., code 11 or 104 corresponding to "remove abnormal")
if code in (11, 104):
    # 1. Set current mode to 'anomaly_height'
    self._set_mode('anomaly_height')
    # 2. Activate grasping node (allow robotic arm movement)
    self._publish_bool(self.pub_grasp_enable, True)
    # 3. Activate anomaly height detection node (start searching for targets)
    self._publish_bool(self.pub_anomaly_height_enable, True)
    return

This is the starting point of the entire flow. When the command is spoken, the code determines the command ID. If it is 11 or 104, it simultaneously "turns on" two switches: one is the visual detection switch (anomaly_height_enable), and the other is the robotic arm grasping switch (grasp_enable), putting the system into standby mode.

2. Anomaly Height Detection (Anomaly Detection)

File: dofbot_ultra_color/AnomalyHeightRemoval.py

xxxxxxxxxx
# Iterate through all detected color block contours
for i in range(len(self.CX_list)):
    # Get the depth value (distance from camera) at the color block center
    dist = depth_image_info[cy, cx] / 1000
    # Convert pixel coordinates + depth to real height (world_z) in robotic arm coordinate system
    pose = self.compute_heigh(cx, cy, dist)
    # ... (coordinate transformation calculation) ...
    
    # Core judgment logic: if the height exceeds 40 mm (0.04 m), it is considered an abnormal target
    if self.enabled and heigh > 40 and self.compute_height == True:
        print("Found the target.")
        # 1. Lock the target, stop subsequent calculations
        self.detect_flag = True
        self.compute_height = False
        
        # 2. Calculate end servo angle (adapt to color block orientation)
        target_joint5 = compute_joint5(vx, vy)
        
        # 3. Publish target position information (PosInfo) to the grasping node
        pos = AprilTagInfo()
        pos.x, pos.y, pos.z = float(self.c_x), float(self.c_y), float(cz)
        self.pub_ColorInfo.publish(pos)

This code is the core of the vision system. It continuously calculates the real height of each color block in the field of view. Once a color block's height exceeds the set threshold (40 mm here), it is determined to be an "abnormal target", its 3D coordinates are calculated, and they are immediately sent to the robotic arm for processing.

3. Grasping Action Execution (Grasp Execution)

def pos_callback(self, msg):
    # Safety check: do not act if the enable signal has not been received
    if not self.enabled: return
    
    # Receive coordinates from vision (x, y, z)
    # Call inverse kinematics (IK) service to calculate the 5 joint angles
    self.get_current_end_pos(current_joints)
    # ... (coordinate transformation and calculation) ...
    
    # Execute the grasping flow
    self.grasp(pose_T)

def grasp(self, pose_T):
    # Send request to the inverse kinematics server to obtain joint angles
    response = future.result()
    # Drive the robotic arm to the target position
    self.pubTargetArm(joints, runtime=1000)
    # Execute a series of actions: close gripper, lift, place, etc.
    self.move()