6. KCF object tracking

1. Start the program

ros2 launch orbbec_camera gemini2.launch.xml
ros2 run yahboomcar_KCFTracker KCF_Tracker_Node

After the program is successfully started, the mouse will select the object to be tracked, and it will be framed when it is released, as shown in the picture below.

Then press the space bar to start tracking the object. The terminal prints out the center coordinates of the tracking object and the distance,

Similarly, since there is no robot chassis driver, the object tracking phenomenon cannot be visually seen, but the object tracking can be reflected by the terminal information and the change of the /cmd_vel topic data. To view the speed topic data, enter the following command,

xxxxxxxxxx
ros2 topic echo /cmd_vel

Move the object being tracked, and the speed data here will also change.

View communication between nodes, terminal input,

xxxxxxxxxx
ros2 run rqt_graph rqt_graph

2. Core code

Code reference path,

xxxxxxxxxx
~/orbbec_ws/src/yahboomcar_KCFTracker/src/KCF_Tracker.cpp

The principle of functional implementation is similar to color tracking, which is based on the central coordinates of the target and the depth information fed by the depth camera to calculate the linear speed and angular speed, and then released to the chassis, part of the code is as follows.

xxxxxxxxxx
//这部分是选择物体后，得出中心坐标，用于计算角速度
// This part is the central coordinate obtained after selecting the object, which is used to calculate the angular velocity
if (bBeginKCF) {
result = tracker.update(rgbimage);
rectangle(rgbimage, result, Scalar(0, 255, 255), 1, 8);
circle(rgbimage, Point(result.x + result.width / 2, result.y + result.height
/ 2), 3, Scalar(0, 0, 255),-1);
} else rectangle(rgbimage, selectRect, Scalar(255, 0, 0), 2, 8, 0);
//这部分是计算出center_x，distance的值，用于计算速度
// This part calculates the value of center_x distance, which is used to calculate the speed
int center_x = (int)(result.x + result.width / 2);
int num_depth_points = 5;
for (int i = 0; i < 5; i++) {
if (dist_val[i] > 0.4 && dist_val[i] < 10.0) distance += dist_val[i];
else num_depth_points--;
}
distance /= num_depth_points;
//计算线速度和角速度
// Calculate linear and angular velocities
if (num_depth_points != 0) {
std::cout<<"minDist: "<<minDist<<std::endl;
if (abs(distance - this->minDist) < 0.1) linear_speed = 0;
else linear_speed = -linear_PID->compute(this->minDist, distance);//-
linear_PID->compute(minDist, distance)
}
rotation_speed = angular_PID->compute(320 / 100.0, center_x /
100.0);//angular_PID->compute(320 / 100.0, center_x / 100.0)