Welcome to DOFBOT-Ultra repository

Color Block Tracking and Grasping

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

The web control program backend process must be closed before starting this function.

For all functions involving grasping cube-shaped objects, please use 30x30x30 mm wooden blocks. Non-standard sized objects may cause servo 6 to stall and overheat.

1. Function Description

After the program starts, use the mouse to select an area on the handheld color block to obtain its HSV value. The program will recognize the color block, and the robotic arm will begin tracking it, aligning the center of the color block with the center of the image. In DaBai depth camera mode, when the target stabilizes and the depth distance is valid, it will beep and grasp the color block, then return to the recognition pose after placement. In standard USB camera mode, only tracking is performed; beeping, grasping, and placement actions are not triggered.

The tracking and grasping logic in this tutorial automatically switches based on CAMERA_TYPE, but the camera launch command must be selected according to the actual camera type:

• DaBai depth camera: use dabai_dcw2.launch.py

• Standard USB camera: use usb_cam.launch.py

The behavior differs between the two modes:

• DaBai depth camera: tracks via depth pipeline and grasps;

• Standard USB camera: tracks via RGB-only, performs target following without grasping.

2. Launch and Operation

2.1. Launch Commands

Launch with the following commands in the terminal:

​x
# DaBai depth camera
ros2 launch orbbec_camera dabai_dcw2.launch.py

# Standard USB camera
ros2 launch dofbot_ultra_info usb_cam.launch.py

# Start low-level control:
ros2 run dofbot_ultra_driver arm_driver
# Start inverse kinematics:
ros2 run dofbot_ultra_info kinematics_dofbotultra
# Start color block tracking and grasping:
ros2 run dofbot_ultra_color color_follow

# Set depth camera exposure
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}'

Notes:

1. The camera launch command must be selected according to the actual camera type.

2. Apart from the camera launch command, all other nodes are started in the same way.

2.2. Operation

After the program starts, hold a 3cm x 3cm color block in the camera view. Use the mouse to select a region of the color block to obtain its HSV value. The program begins recognizing the color block. The robotic arm will adjust its pose so that the center of the color block aligns with the center of the image. Slowly move the color block, and the robotic arm will follow it, continuously adjusting its pose. Once the color block center aligns with the image center, the program will automatically select the operating mode based on the current CAMERA_TYPE:

• Depth camera mode: grasps when the center point depth value is valid and within the appropriate working distance;

• Standard USB camera mode: continuously tracks via RGB-only; no grasping is performed even when the target is centered and stable.

After grasping in depth camera mode, the arm places the object at the set position and finally returns to the recognition pose. USB mode maintains the tracking flow.

Note: This issue mainly occurs in depth camera mode. Some solid color blocks have strong reflectivity, which may prevent the depth camera from obtaining distance information, causing grasping failure. Observe the dist data in the upper-left corner of the UI. If dist displays as 0, it means distance capture failed, and you should try a different colored block.

3. Program Flowchart

color_follow.py

4. Core Code Analysis

This tutorial actually launches:

xxxxxxxxxx
ros2 run dofbot_ultra_color color_follow

The actual source code being executed is:

xxxxxxxxxx
Raspberry Pi: /home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_color/dofbot_ultra_color/color_follow.py
RDK: /home/sunrise/dofbot_ultra_ws/src/3d/dofbot_ultra_color/dofbot_ultra_color/color_follow.py

4.1. Color Detection and Tracking Main Node color_follow.py

The current version of color_follow.py supports dual-camera mode:

1. When CAMERA_TYPE=DABAI_DCW2, it subscribes to both color and depth images, and only allows grasping when the depth distance is valid;

2. In standard USB camera mode, it only subscribes to RGB images, performs RGB-only color block tracking, and no longer triggers grasping.

xxxxxxxxxx
if USE_DEPTH_CAMERA:
    self.rgb_image_sub = Subscriber(self, Image, "/camera/color/image_raw", qos_profile=1)
    self.depth_image_sub = Subscriber(self, Image, "/camera/depth/image_raw", qos_profile=1)
    self.TimeSynchronizer = ApproximateTimeSynchronizer(
        [self.rgb_image_sub, self.depth_image_sub], queue_size=5, slop=0.1)
    self.TimeSynchronizer.registerCallback(self.TrackAndGrap)
else:
    self.rgb_image_sub = self.create_subscription(
        Image, "/camera/color/image_raw", self._on_usb_image, make_usb_image_qos())

TrackAndGrap() currently has three main responsibilities:

1. Extract the target color block based on the calibrated HSV range;

2. Call DofbotTrack.XY_track() to bring the color block center closer to the image center;

3. Only allow grasping when depth camera mode conditions (stability and distance) are satisfied.

xxxxxxxxxx
can_grasp = (
    USE_DEPTH_CAMERA
    and (not self.grasping)
    and (self.cur_distance != 0.0)
    and (self.align_count >= self.align_required_count)
    and ((cur_time - self.last_grasp_time) >= self.grasp_cooldown_s)
)

This is the actual implementation behind "depth camera can grasp, USB only tracks" in the current tutorial:

• Depth camera mode: proceeds to grasp once the color block is stable;

• Standard USB camera mode: USE_DEPTH_CAMERA is false, the node only tracks and does not grasp.

Additionally, process() handles color selection and HSV threshold updates:

1. In init state, allows mouse selection of a color region;

2. Computes and saves the hsv_range;

3. In run state, continuously extracts the target color block and updates the center coordinates and corner orientation.

4.2. Robotic Arm Grasping Execution Dofbot_Track.py

The low-level grasping implementation is located at:

xxxxxxxxxx
Raspberry Pi: /home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_color/dofbot_ultra_color/Dofbot_Track.py
RDK: /home/sunrise/dofbot_ultra_ws/src/3d/dofbot_ultra_color/dofbot_ultra_color/Dofbot_Track.py

Note: The actual path is no longer the old scripts/Dofbot_Track.py, but dofbot_ultra_color/Dofbot_Track.py in the current package directory.

In depth camera mode, color_follow.py passes the color block center point and depth value to DofbotTrack.Clamping(), which is responsible for:

1. Converting pixel coordinates and depth values to the camera coordinate system;

2. Combining with the hand-eye matrix and current end-effector pose to transform to the robotic arm world coordinate system;

3. Calling inverse kinematics for solving;

4. Executing descending, grasping, lifting, placing, and returning to zero.

xxxxxxxxxx
camera_location = self.pixel_to_camera_depth((cx, cy), cz)
PoseEndMat = np.matmul(self.EndToCamMat, self.xyz_euler_to_mat(camera_location, (0, 0, 0)))
EndPointMat = self.get_end_point_mat()
WorldPose = np.matmul(EndPointMat, PoseEndMat)
pose_T, pose_R = self.mat_to_xyz_euler(WorldPose)

Therefore, the actual execution chain in this section is:

color_follow.py handles color selection, detection, centering, and grasping trigger determination;

Dofbot_Track.py handles coordinate transformation, inverse kinematics solving, and robotic arm action execution.

    request.kin_name = "ik"
    # Target Roll value of the robotic arm end effector, in radians, representing the current roll value of the arm end
    request.Roll = self.CurEndPos[3]
    print("calcutelate_request: ",request)
    try:
        response = self.client.call(request)
        joints = [0.0, 0.0, 0.0, 0.0, 0.0,0.0]
        # Assign the joint1-joint6 values returned by the service to joints
        joints[0] = response.joint1 #response.joint1
        joints[1] = response.joint2
        joints[2] = response.joint3
        if response.joint4>90:
            joints[3] = 90
        else:
            joints[3] = response.joint4
        joints[4] = 90 
        joints[5] = 20
        # Calculate the distance from the AprilTag to the robotic arm base coordinate system
        dist = math.sqrt(request.tar_y ** 2 + request.tar_x** 2)
        # If the distance is between 18 cm and 30 cm, control the arm to move to the grasping point
        if dist>0.18 and dist<0.30:
            self.Buzzer()
            print("compute_joints: ",joints)
            # Execute pubTargetArm function, passing the calculated joints values as parameters
            self.pub_arm(joints)
            # Execute grasping, movement, and placement actions
            self.move()
        else:
            print("It's too far to catch it!Please move it forward a bit. ")
    except Exception:
       rospy.loginfo("run error")