🔷 Wooden Block Shape Recognition and Sorting

1. Course Overview

The function implemented in this lesson can be simply understood as: camera distinguishes shapes, judges whether it's a square or cylinder based on contours, automatically places them in corresponding slots.

The program first uses mouse box selection for color, locking onto the target block's HSV color range. Then for each frame, it performs color segmentation to extract contours, using polygon fitting vertex count + aspect ratio + circularity three geometric features combined to determine shape (square/rectangle/cylinder/triangle), and uses multi-frame voting mechanism to stabilize shape labels. Finally, the robotic arm automatically sorts and unloads based on shape.

2. Preparation and Running

This case requires opening 3 terminals to start the system sequentially:

2.1 Start Camera Driver and Kinematics Solver Environment

Run in Terminal 1:

ros2 launch dofbot_pro_driver A1pro_camera_arm.launch.py

2.2 Start Robotic Arm Desktop Grasping Action Execution Node

Run in Terminal 2:

​x
# Depth camera version (Nuwa)
ros2 run dofbot_pro_driver grasp_desktop

# Or USB camera version
ros2 run dofbot_pro_usb grasp_usb

2.3 Start Geometric Shape Recognition and Sorting Master Node

Run in Terminal 3:

xxxxxxxxxx
# Depth camera version (Nuwa)
ros2 run dofbot_pro_driver shape_recognize

# Or USB camera version
ros2 run dofbot_pro_usb shape_usb_recognize

3. Operation Demo and Sorting Interaction

1. Place square, rectangular, or circular wooden blocks steadily below the robotic arm field of view;

2. Press C to enter color selection mode (Note: depth camera version does not require color selection), use mouse to drag and select the target wooden block area to sample HSV. The image will display real-time detection and annotation of each block's geometric shape name (Square / Rectangle / Cylinder / Triangle);

3. Left-click on the specific target block to grasp. The selected block's border will change to highlighted 🔴 red;

4. Press spacebar or enter (Space / Enter), chassis automatically and smoothly aligns, robotic arm descends to grasp the target block and places it in the fixed slot.

4. Core Source Code Analysis

The system is completed by "geometric shape recognition decision" and "IK grasping execution" two nodes working in coordination:

The core business logic of both remains unified: HSV Segmentation -> Extract Geometric Features (vertices/aspect ratio/circularity) -> Historical queue multi-frame voting lock -> Mouse click to select target -> Calculate 3D coordinates and gripper angle -> IK execution bin grasping.

The main technical implementation comparison between the two:

4.1 USB Monocular Camera Core Source Code Analysis (shape_usb_recognize.py)

1. Geometric Feature Combined Recognition Algorithm

After extracting contours, the program calculates circularity (circularity), aspect ratio (aspect_ratio), and polygon fitting vertex count (vertices), using four-dimensional feature joint arbitration:

xxxxxxxxxx
# shape_usb_recognize.py —— Geometric joint discrimination
perimeter = cv2.arcLength(obj, True)
circularity = 4 * math.pi * area / (perimeter * perimeter)
aspect_ratio = max(rect_w, rect_h) / min(rect_w, rect_h)
approx = cv2.approxPolyDP(obj, 0.035 * perimeter, True)
vertices = len(approx)

if circularity > 0.78 and vertices > 6:
    obj_type = "Cylinder"     # High circularity + multi-vertex fitting -> Cylinder
elif 4 <= vertices <= 6:
    if aspect_ratio < 1.15:
        obj_type = "Square"   # Vertices 4-6 and aspect ratio close to 1 -> Cube
    else:
        obj_type = "Rectangle" # Clearly elongated -> Rectangular prism
elif vertices == 3:
    obj_type = "Triangle"     # 3 vertices -> Triangle

2. Multi-Frame Voting and Mouse Polygon Collision Detection

Maintains a historical queue with capacity of 10; the most frequent shape needs to accumulate 6 occurrences before locking display; when mouse clicking, uses cv2.pointPolygonTest to determine whether the click point is within the polygon bounding box:

xxxxxxxxxx
# shape_usb_recognize.py —— Mouse click detection
for t_obj in self.tracked_objects:
    if cv2.pointPolygonTest(t_obj['box'], (float(x), float(y)), False) >= 0:
        clicked_targets.append((distance, t_obj))

# Lock selected target ID and change border to red
self.selected_track_id = selected['track_id']
self.Target_Shape = selected['locked_shape']

4.2 3D Depth Camera Core Source Code Analysis (shape_recognize.py)

1. Depth Map Direct Extraction and World Absolute Coordinate Solving

The depth camera solution obtains aligned point cloud depth matrix through ApproximateTimeSynchronizer. When the user mouse-selects a target, it directly reads the true distance at the block center pixel and calculates the robotic arm base world coordinates:

xxxxxxxxxx
# shape_recognize.py —— Target depth extraction and hand-eye transform
cx = int(target['center'][0])
cy = int(target['center'][1])
# Directly read true depth (meters)
dist = depth_image[cy, cx] / 1000.0

# Combine hand-eye matrix and robotic arm current forward kinematics to compute world coordinates pose
pose = self.compute_heigh(cx, cy, dist)
# Superimpose offset_value.yaml 3D spatial calibration compensation
pose[0] += self.x_offset
pose[1] += self.y_offset
pose[2] += self.z_offset


   # 50 is the grayscale threshold, used to remove darker areas: grayscale range is 0~255, this sets pixels below 50 to black, keeping the rest. When doing subsequent binarization and contour recognition, areas set to zero usually won't be treated as targets
   # Increasing 50: filters more areas, may reduce interference but may also filter out objects.
   # Decreasing 50: keeps more areas, but may increase noise and extra contours.
    black_image = np.zeros_like(gray_image)
    black_image[0:320, 0:640] = gray_image[0:320, 0:640]
    black_image[black_image < 50] = 0

2. Gripper Parallel Alignment Rotation Angle Calculation

Using the minimum bounding rectangle two vertex difference vectors from the selected block's contour fitting, real-time solve joint 5 pose angle to ensure the gripper closes parallel to the square or rectangular block's side wall:

xxxxxxxxxx
# shape_recognize.py —— Gripper adaptive deflection angle computation
vx = target['box'][0][0] - target['box'][1][0]
vy = target['box'][0][1] - target['box'][1][1]
target_joint5 = compute_joint5(vx, vy)
self.joint5.data = int(target_joint5)

# Publish adaptive deflection angle and 3D pose
self.TargetJoint5_pub.publish(self.joint5)
self.pos_info_pub.publish(pos)

3. Shape Classification Placement

After the grasping execution node receives target information, it maps to different joint rotation target angles based on the recognized geometric shape (Target_Shape), driving rotation to different unloading stations for cubes, rectangular prisms, and cylinders, completing the entire shape sorting automation closed-loop.

4.3 Grasping Offset Tuning (Both Camera Models)

If position deviation occurs during actual grasping (such as missing, knocking over objects, offset left/right, or hitting the table), you can directly modify the spatial offset configuration file for the corresponding camera:

1. Where to Modify

Configuration files are located in the ~/yahboomcar_ros2_ws/dofbot_pro_ws/src/dofbot_pro_driver/config/ directory. Select based on camera type:

• 3D Depth Camera (Nuwa-HP60C): offset_value.yaml

• USB Camera: usb_offset_value.yaml

2. How to Modify

The file contains offset parameters in X, Y, Z directions (unit: meters, recommended adjustment per tweak is 0.005~0.01, i.e., 5~10mm):

• x_offset (forward/backward direction, positive toward vehicle front):

  • Gripper extends too far and knocks over object decrease value (adjust smaller or set negative)

  • Gripper doesn't extend far enough and misses increase value (adjust larger)

• y_offset (left/right direction, positive toward vehicle left):

  • Gripper deviates right increase value (correct left)

  • Gripper deviates left decrease value (correct right)

• z_offset (up/down height, positive toward ceiling):

  • Gripper too high in air and can't grasp decrease value (probe deeper)

  • Gripper hits or scrapes table increase value (raise higher)