void ControlComply::BiaAngleCalculate(vector<XYZ_COOR_S> path_list, CONTROL_PARAM_IN para_in, robot::control_msg& para_out) { float distance_temp; int new_key_point = 0; XYZ_COOR_S xyz_temp; float delta_x[2], delta_y[2]; float min_distance = 100; int size = path_list.size(); float cur_x = para_in.cur_position.x_axis; float cur_y = para_in.cur_position.y_axis; float cur_head = para_in.cur_position.heading; for (int i = 0; i < size; i++) { xyz_temp = path_list.at(i); distance_temp = sqrt((xyz_temp.x_axis - cur_x) * (xyz_temp.x_axis - cur_x) + (xyz_temp.y_axis - cur_y) * (xyz_temp.y_axis - cur_y)); if (min_distance > distance_temp) { min_distance = distance_temp; new_key_point = i % size; } } // std::cout<<"00000000000000000000000000000 key ="<<new_key_point<<std::endl; // std::cout<<"cur = "<<cur_x<<","<<"y = "<<cur_y<<","<<"xyz = "<<xyz_temp.x_axis<<","<<"y = // "<<xyz_temp.y_axis<<std::endl; mKeyPoint = new_key_point; para_out.preCurve = path_list.at(mKeyPoint).curvature; if (path_list.at(path_list.size() - 3).curvature > para_out.preCurve) para_out.preCurve = path_list.at(path_list.size() - 3).curvature; delta_x[0] = cur_x - path_list.at(new_key_point).x_axis; delta_y[0] = cur_y - path_list.at(new_key_point).y_axis; delta_x[1] = path_list.at((new_key_point + 2) % size).x_axis - path_list.at(new_key_point).x_axis; delta_y[1] = path_list.at((new_key_point + 2) % size).y_axis - path_list.at(new_key_point).y_axis; distance_temp = delta_x[1] * delta_y[0] - delta_y[1] * delta_x[0]; if (distance_temp > 0) para_out.biaDistance = sqrtf(delta_x[0] * delta_x[0] + delta_y[0] * delta_y[0]); else para_out.biaDistance = -1 * sqrtf(delta_x[0] * delta_x[0] + delta_y[0] * delta_y[0]); para_out.preAngleDev = 0; }

这段代码定义了一个名为`BiaAngleCalculate`的函数，它接受三个参数：`path_list`（存储XYZ坐标的向量）、`para_in`（包含控制参数的结构体）和`para_out`（用于输出结果的结构体）。

首先，定义了一些局部变量，包括`distance_temp`（临时距离变量）、`new_key_point`（最近路径点的索引）、`xyz_temp`（临时存储XYZ坐标的结构体）、`delta_x`和`delta_y`（用于计算偏差的变量）、`min_distance`（最小距离值）以及`size`、`cur_x`、`cur_y`和`cur_head`（从输入参数中获取的当前位置和航向角）。

然后，使用一个循环遍历路径点向量中的每个元素。在循环中，计算当前路径点与当前位置的距离，并将其与最小距离值进行比较。如果当前距离更小，则更新最小距离值，并将最近路径点的索引设置为当前索引。

接下来，将最近路径点的索引保存在成员变量`mKeyPoint`中，并将该路径点的曲率值赋值给输出参数`para_out.preCurve`。如果路径向量中倒数第三个路径点的曲率值大于`preCurve`，则将其赋值给`preCurve`。

然后，计算偏差的变量`delta_x`和`delta_y`，用于后续计算。其中，`delta_x[0]`和`delta_y[0]`表示当前位置与最近路径点的X和Y坐标的差值，`delta_x[1]`和`delta_y[1]`表示下一个路径点与最近路径点的X和Y坐标的差值。

接着，计算当前位置与最近路径点连线与最近路径点与下一个路径点连线之间的向量叉乘。如果结果大于0，则表示当前位置在路径的左侧，将距离设置为正值；否则，将距离设置为负值。

最后，将输出参数`para_out.biaDistance`设置为当前位置与最近路径点的距离，并将输出参数`para_out.preAngleDev`设置为0。

总结来说，这段代码定义了一个函数，用于计算当前位置与路径之间的偏差值。它找到离当前位置最近的路径点，并计算当前位置与该路径点之间的距离和角度偏差，并将结果存储在输出参数中。