Human Body Follow

This tutorial is specific to CM5 version; it cannot be used directly in CM4 version.

1. Experiment Objective

Learn to use the RiderPi camera for human body perspective positioning and follow the human body.

2. Experiment Steps

Enter the RiderPi system, terminate the RiderPi main program, and enter "ip (IP is the RiderPi's IP):8888" in your browser. After logging in, enter the password "yahboom".

Then log in and navigate to the path cd ~/Rider-pi_class/6.AI Visual Interaction Course/8. Human Body Follows and run follow_person.ipynb.

Finally, run the program.

3. Experimental Phenomena

After running the program, RiderPi will detect the human body and follow it forward, backward, left, and right based on the distance to the human body.

4. Experimental Source Code Analysis

xxxxxxxxxx
# 显示输出区域
display(image_widget)
display(output_area)
​
from track import HumanTracker
from xgolib import XGO
​
button = Button()
dog = XGO(port="/dev/ttyAMA0", version="xgolite")
dog.attitude('p', -10)
​
​
tracker = HumanTracker()
from PIL import Image, ImageDraw
import xgoscreen.LCD_2inch as LCD_2inch
splash_theme_color = (255,255,255)
mydisplay = LCD_2inch.LCD_2inch()
mydisplay.Init()
mydisplay.clear()
# Init Splash
splash = Image.new("RGB", (mydisplay.height, mydisplay.width), splash_theme_color)
draw = ImageDraw.Draw(splash)
mydisplay.ShowImage(splash)
detector = HumanDetector()
last_move_x_speed = 0.0
last_turn_speed = 0.0
filter_coefficient = 0.3  # 滤波系数 filter coefficient
while True:
    with output_area:
        frame = picam2.capture_array()
        humans = detector.detect_humans(frame)
        for human in humans:
            if human:
                print("\n=== update  ===")
                center_x, distance = human['center_x'], human['distance'] if human else (None, None)
                #print(f"距离最近的人在 ({human['bbox']}), center_x: {human['center_x']}, distance: {human['distance']} cm")
                tracking_params = tracker.update(human)
                if tracking_params['is_detected'] and tracking_params['is_moving']:
                    prediction = tracker.predict_future_position(tracking_params, 0.06)
                    if prediction:
                        pass
                raw_move_x_speed = tracking_params['velocity_x'] * 0.05 + (distance - 100) * 0.1
                if -20 <distance - 70 <  20:
                    raw_move_x_speed = (distance - 70) * 0.3
                else: 
                    raw_move_x_speed = (distance - 70) * 0.2
                raw_turn_speed = tracking_params['velocity_x'] * 0.02
                if 145<center_x<175:
                    raw_turn_speed = 0
                elif 130<center_x<145 or 175< center_x <190:
                    raw_turn_speed = (160 - center_x) * 0.2
                else:
                    raw_turn_speed = (160 - center_x) * 0.15
                move_x_speed = filter_coefficient * last_move_x_speed + (1 - filter_coefficient) * raw_move_x_speed
                turn_speed = filter_coefficient * last_turn_speed + (1 - filter_coefficient) * raw_turn_speed
                max_move_speed = 10.0
                max_turn_speed = 15.0
                move_x_speed = max(-max_move_speed, min(max_move_speed, move_x_speed))
                turn_speed = max(-max_turn_speed, min(max_turn_speed, turn_speed))
                # 更新上一时刻的速度 Update the speed from the previous moment
                last_move_x_speed = move_x_speed
                last_turn_speed = turn_speed
                if la =="cn":
                    print(f'距离:{distance}, 中心点偏移：{160-center_x}')
                    print(f"移动速度: {move_x_speed:.2f}, 转向角度: {turn_speed:.2f}")
                else:
                    print(f'distance:{distance}, center_x：{160-center_x}')
                    print(f"move_x_speed: {move_x_speed:.2f}, turn_angle: {turn_speed:.2f}")
        
                #dog.move_x(move_x_speed)
                #dog.turn(turn_speed)
                # #dog.move_x(0.02 * tracking_params['velocity_y'])
                # #dog.turn(tracking_params['velocity_x'])
                
                
        # 如果没有检测到人体，逐渐减速 If no human body is detected, gradually slow down
        if humans == []:
            move_x_speed = 0.03*filter_coefficient * last_move_x_speed 
            turn_speed = 0.03*filter_coefficient * last_turn_speed 
            max_move_speed = 3.0
            max_turn_speed = 3.0
            move_x_speed = max(-max_move_speed, min(max_move_speed, move_x_speed))
            turn_speed = max(-max_turn_speed, min(max_turn_speed, turn_speed))
            dog.move_x(move_x_speed)
            dog.turn(turn_speed)
​
            if la =="cn":
                print(f"移动速度: {move_x_speed:.2f}, 转向角度: {turn_speed:.2f}")
            else:
                print(f"move_x_speed: {move_x_speed:.2f}, turn_angle: {turn_speed:.2f}")
            
            last_move_x_speed = move_x_speed
            last_turn_speed = turn_speed
            
        b, g, r = cv2.split(frame)
        image_widget.value = bgr8_to_jpeg(frame)
        #cv2.imshow('Human Detection', frame)
        
        img = cv2.merge((r, g, b))
        imgok = Image.fromarray(img)
        mydisplay.ShowImage(imgok)
​
        if button.press_b():
            dog.reset()
            break

if -20 <distance - 70 <  20:
    raw_move_x_speed = (distance - 70) * 0.3
else: 
    raw_move_x_speed = (distance - 70) * 0.2
raw_turn_speed = tracking_params['velocity_x'] * 0.02
if 145<center_x<175:
    raw_turn_speed = 0
elif 130<center_x<145 or 175< center_x <190:
    raw_turn_speed = (160 - center_x) * 0.2
else:
    raw_turn_speed = (160 - center_x) * 0.15

• If you want to change the following distance and the left/right turn size, you can change the range here.
• detector.detect_humans(frame): This function mainly detects the position of human bodies in the image.