给下列代码添加注释： if reached == True:#计算中心线，并根据中心点计算转向角度 done_pub.publish(False) line_xy = np.column_stack(np.where(cropped1 >= 245))#像素值大于等于245 line_x = np.mean(line_xy[:,0])#计算x，y坐标的平均值 line_y = np.mean(line_xy[:,1]) center_x = line_x + 80 center_y = line_y + 20 #计算中点坐标 error_check = 0#错误检测计数器 max_error_check = 5#最大错误检测次数 p_s = cv2.getTrackbarPos('p','image')#滑动条 x_x = cv2.getTrackbarPos('x','image') r_r = cv2.getTrackbarPos('r','image') if np.isnan(center_x) or np.isnan(center_y):#如果有nan的值，按照以前的计算，否则重新计算。 angle = old_angle else: angle = (x_x-center_y)*p_s*0.1 if angle<0: angle = angle*(1+r_r*0.01) angle = 0.7 * angle + 0.3 * old_angle#计算平均角度 print(p_s) print(center_x,center_y) print(angle) ark_contrl.steering_angle = angle ark_contrl.speed = 0.1#设置小车速度 old_angle = angle cmd_vel_pub.publish(ark_contrl)#发布小车控制指令 '''if (np.isnan(line_x) or np.isnan(line_y)) and reached: while True: error_check += 1 print(error_check) if error_check == max_error_check: #ark_contrl.steering_angle = angle #ark_contrl.speed = 0.25 #cmd_vel_pub.publish(ark_contrl) #done_pub.publish(True) error_check = 0 print("done") break'''

# 计算中心线，并根据中心点计算转向角度
if reached == True:
done_pub.publish(False) # 发布False，表示任务未完成
line_xy = np.column_stack(np.where(cropped1 >= 245)) # 获取像素值大于等于245的点的坐标
line_x = np.mean(line_xy[:,0]) # 计算x坐标的平均值
line_y = np.mean(line_xy[:,1]) # 计算y坐标的平均值
center_x = line_x + 80 # 计算中心点的x坐标
center_y = line_y + 20 # 计算中心点的y坐标

error_check = 0 # 错误检测计数器
max_error_check = 5 # 最大错误检测次数

p_s = cv2.getTrackbarPos('p','image') # 获取滑动条p的值
x_x = cv2.getTrackbarPos('x','image') # 获取滑动条x的值
r_r = cv2.getTrackbarPos('r','image') # 获取滑动条r的值

if np.isnan(center_x) or np.isnan(center_y): # 如果中心点的坐标有nan值，使用之前的计算结果
angle = old_angle
else:
angle = (x_x - center_y) * p_s * 0.1 # 根据中心点计算转向角度

if angle < 0:
angle = angle * (1 + r_r * 0.01) # 根据滑动条r的值调整转向角度

angle = 0.7 * angle + 0.3 * old_angle # 计算平均角度

print(p_s)
print(center_x, center_y)
print(angle)

ark_contrl.steering_angle = angle # 设置小车的转向角度
ark_contrl.speed = 0.1 # 设置小车速度

old_angle = angle

cmd_vel_pub.publish(ark_contrl) # 发布小车控制指令

'''
if (np.isnan(line_x) or np.isnan(line_y)) and reached:
while True:
error_check += 1
print(error_check)
if error_check == max_error_check:
error_check = 0
print("done")
break
'''
# 如果线的坐标有nan值且任务已经完成，进入错误检测循环，最多检测max_error_check次，如果超过次数，则输出"done"并退出循环。