1. 機器人軟件開發
2. 什么是 P、PI 和 PID 控制器？
3. 比例 （P） 控制器
4. 比例積分 （PI） 控制器
5. 比例-積分-微分 （PID） 控制器
6. 橫向控制簡介
7. CARLA ROS2 集成
8. 縱向控制
9. 橫向控制
10. 關鍵要點
11. 結論
12. 引用

機器人軟件開發

機器人技術是一個多學科領域，涉及機械設計和計算機科學的各個領域，例如計算機體系結構、網絡和數據庫。由于它集成了不同的領域，它通常被視為系統工程。機器人技術的核心軟件組件是數學密集型的，必須經過高度優化才能保持低延遲。機器人技術的主要軟件組件是：

• 知覺
• 規劃
• 控制

無論其環境如何，通用的 autonomous stack 將如下所示（見下圖），至少在其初始迭代中是這樣。該堆棧由 4 個子堆棧組成，即傳感器堆棧、感知堆棧、規劃和控制堆棧。傳感器堆棧由前后兩個攝像頭以及車輛頂部的 LiDAR 組成。感知堆棧包括定位和地圖構建（我們在整個系列中一直在討論這一點），高清地圖對于導航也非常重要，但已經提出了一些方法，可以即時創建高清地圖，最后是用于檢測地標和了解可駕駛區域的對象檢測和分割。

規劃模塊分為三個部分。眾所周知，對于許多用例來說，擁有全局和本地規劃器可能就足夠了，但是在處理不同的場景時，機器人需要有一個行為樹。讓我們以自動駕駛汽車為例。可能存在諸如遵循紅綠燈、停車或越野駕駛等場景，在這些場景中，不同的任務需要不同的規劃者。最后是控件 — 通常，經過良好調整的 PID 控制器用于執行器信號。它通常分為兩個步驟：用于航點跟蹤的橫向控制和用于速度管理的縱向控制。在本文中，我們將更詳細地探討控制部分。

如果我們要在 CARLA 中設計一輛基本的自動駕駛汽車，我們將如何做到這一點？強調 “in CARLA”?因為它將位于模擬器內部，這使得在結構化環境中工作變得更加容易。此外，我們可以利用 CARLA 提供的所有?API 優勢來獲得大量預先計算的估計值。下面的系統架構顯示了如何使用?CARLA 和 ROS 2?創建自主路徑跟蹤車輛。

AV系統

我們首先啟動 CARLA 客戶端和 CARLA ROS 橋。這兩個模塊進行通信以將數據從 CARLA 發送到 ROS 2。最初，這將提供有關?CARLA?世界、天氣和狀態的信息。接下來，需要啟動?CARLA ros-bridge 生成車輛節點，以便在 CARLA 世界中生成車輛，這將通過 ROS 2 主題提供有關車輛、傳感器數據和交通的大量信息。之后，啟動Lanelet?和?Point Cloud 地圖，讓我們可以看到可駕駛區域。

Lanelet 地圖和點云地圖可以了解周圍環境和可駕駛區域。在我們了解之后，我們可動?waypoint generator。它采用車輛的起點和目標位置來生成最短路徑，該路徑由航點組成。路徑點本質上是定義全局路徑的一系列 3D 點。車輛的任務是沿著這些航路點到達其目標。為此，使用了?PID 控制器，該控制器采用航路點坐標和車輛的里程計（每個時間戳的當前位置和方向）來生成執行器信號。里程計數據也來自 CARLA。

從 ROS 2 的角度來看，系統是這樣工作的：節點是工作流的核心。它訂閱了開始和結束位置主題，以及為車輛生成的航路點，并使用車輛的里程計來計算控制命令。主題接收這些控制命令并移動車輛。整個系統在?RViz2?中可視化。carla vehicle ctrl/carla/ego_vehicle/vehicle_control_cmd

由于 CARLA 已經處理了里程計和規劃，我們將主要專注于車輛控制。為此，我們決定使用 PID 控制器，但可以探索更高級的控制器。以下是我們將涵蓋的主題：

• 什么是 P、PI 和 PID 控制器？
• 實現 PID 控制 python。
• 什么是縱向控制和橫向控制？

什么是 P、PI 和 PID 控制器？

如前所述，我們將使用?PID 控制器通過生成必要的執行器信號，使車輛沿所需軌跡移動。但在此之前，“執行器信號”實際上是什么意思？執行器是將電能轉化為物理運動的機器人部件，通常是機器人關節中的電機或液壓系統。控制器的工作是產生適量的電能以實現所需的運動。例如，如果你有一輛車，希望它從一個地方行駛到另一個地方，或者如果你有一個機械臂，希望它從桌子上抓一個蘋果，規劃器會給你所需的物理運動——它可以表示為速度曲線、軌跡，或兩者兼而有之。控制者的工作是遵循建議的路徑。

現在我們了解了目的，讓我們來探索一下控制器是如何實現的。有不同類型的控制器可用，例如 PID 控制器、模糊邏輯控制器、模型預測控制 （MPC）、非線性模型預測控制?（NLMPC）、神經網絡控制器、基于 RL 的控制器等。但在本文中，我們將重點介紹 PID 控制器，它是最簡單但最高效的控制器之一。傳說中，經過適當調整的 PID 控制器仍然可以讓所有現代控制器物有所值。盡管它是最古老的控制器方法之一（已有 100 多年的歷史），但它仍在工業和學術界使用。

PID 控制器中的 P、I 和 D 分別代表比例、積分和導數。每個術語都有助于控制機器人。讓我們用一個例子來分解一下：假設您有一輛自動駕駛汽車，想在保持 50 m/s 速度的同時從家到辦公室。以恒定速度自動駕駛車輛是一項稱為巡航控制的高級駕駛員輔助系統?（ADAS）?功能。

從控制的角度來看，目標是通過根據當前車速和目標速度之間的差異不斷調整油門來實現這一目標。從圖示上講，它可以表示如下：

在高層次上，你可以把它想象成一個系統，你提供所需的速度，它返回系統響應——實際的速度（見上面的圖 a）。控制器的目標是使實際速度等于所需的速度，這意味著它們之間的差異最終應該達到零。

如果我們放大系統（如圖 a），您將看到（圖 b）控制器和車輛（作為工藝/工廠）。在這里，控制器的工作是將所需速度和實際速度作為輸入，并計算它們之間的差值以產生節流值。向 PID 控制器提供飛行器的響應稱為閉環反饋。

現在我們已經全面了解了 PID 控制器的作用，讓我們糾纏 PID 控制器并了解 P、I 和 D 項的貢獻。

比例 （P） 控制器

P 控制器解決當前速度和所需速度之間的差值。例如，如果當前速度為 0 且目標為 50 m/s，則需要增加節流閥以縮小該間隙。誤差越大，應用的調節就越多，這意味著調節與誤差成正比，表示為：

在數學上，這可以寫成

下面的框圖表示反饋控制系統中使用的比例 （P） 控制器，用于調節車輛的速度。將所需速度輸入到系統中，并與實際速度進行比較以確定誤差。該誤差被饋送到 P 控制器中，該控制器根據誤差的成比例增益調整車輛的油門。輸出是車輛的實際速度，它被反饋到系統中進行連續調整，保持所需的速度。循環通過反饋閉合，確保實時校正飛行器的速度。

下面是簡單 P 控制器的 python 代碼

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.widgets import Slider# Define PID Controller class with resistance
class PIDControllerWithResistance:def __init__(self, Kp, set_point=0, resistance_factor=0.1):self.Kp = Kpself.set_point = set_pointself.resistance_factor = resistance_factor  # Resistance to throttle (e.g., air resistance, friction)def update(self, current_value, dt):# Apply the same PID control logic but factor in resistanceerror = self.set_point - current_valueoutput = self.Kp * errorreturn output - self.resistance_factor * current_value  # Reduce output by a resistance factor# Simulation parameters
dt = 0.1  # Time step
time = np.arange(0, 50, dt)  # Simulation time# Initialize the PID controller with disturbance (resistance)
pid_with_resistance = PIDControllerWithResistance(Kp=1.0, set_point=50, resistance_factor=0.2)# Initial conditions
speed = 0
throttle_with_resistance = []
speed_record_with_resistance = []# Simulate the system with resistance
for t in time:control = pid_with_resistance.update(speed, dt)speed += control * dt  # Speed is affected by throttle control and resistancethrottle_with_resistance.append(control)speed_record_with_resistance.append(speed)# Plot setup
fig, ax = plt.subplots()
plt.subplots_adjust(left=0.1, bottom=0.3)l, = plt.plot(time, speed_record_with_resistance, label="Speed Output (With Resistance)")
plt.axhline(pid_with_resistance.set_point, color='r', linestyle='--', label='Set Point')
plt.xlabel('Time [s]')
plt.ylabel('Speed [m/s]')
plt.title('PID Cruise Control with Resistance')
plt.legend()
plt.show()

結果：

此代碼做了一些假設：

1. 我們假設油門和車速之間的關系是線性的，這意味著增加油門會增加速度。這就是為什么我們簡單地做 .通過將 乘以 ，我們確保速度在每個小的時間間隔內逐漸變化，模擬逐漸變化。speed += control * dtcontroldt
2. 為簡單起見，我們假設在這個模型中，阻力與速度成線性比例。

我們首先定義 class，它接受?

?value、set-point 和 resistance 值。在 update 函數中，我們計算誤差并將其乘以

得到節流。我們還包括 .PIDControllerWithResistanceself.set_point - current_valueoutput - self.resistance_factor * current_value

• 將阻力系數乘以當前速度 （） 可創建一個更真實的模型，其中阻力隨速度的增加而增加。current_value
• 通過從輸出節氣門值中減去這個阻力分量，我們模擬了需要更多的節氣門來保持更高的速度，從而使車輛更難以更高的速度加速。

我們從 0 到 50 循環遍歷時間戳，保持 0.1，并調用控制器的 update 函數來獲得結果速度。然而，正如你所看到的，僅使用比例控制是不夠的。盡管速度接近設定點，但它從未完全達到設定點。穩態和設定點之間的這種差值稱為 。此時，誤差變得如此之小，以至于即使將其乘以

也會導致最小的變化，而速度幾乎保持不變。dtSteady State Error

比例積分 （PI） 控制器

正如我們所看到的，響應速度接近設定點，但并沒有完全達到它。為了解決這個問題，我們可以在 Proportional 控制器旁邊添加一個 Integral 控制器。Integral 控制器查看過去的錯誤并隨著時間的推移累積它們。這種正誤差的累積有助于將響應推向更接近設定點。

在數學上，這可以寫成

微分 （D） 控制器現已與 PI 控制器并聯添加，形成比例-積分-微分 （PID） 控制器。該系統用于反饋回路中，以調節車輛的速度，通過考慮現在、過去和預測的未來誤差來提高精度。將所需速度與實際速度進行比較以計算誤差。該誤差被饋送到三個控制器中：比例 （P） 控制器，對當前誤差做出反應，積分 （I） 控制器，解釋隨時間累積的誤差，以及導數 （D） 控制器，通過考慮誤差的變化率來預測未來的誤差。這些控制器的輸出組合在一起以調整車輛的油門，并將實際速度反饋到系統中進行連續校正，確保以更高的精度和穩定性保持所需的速度。

該方程式在編寫時考慮了離散時間。?值通常是通過反復試驗找到的，盡管有一些算法可用于調整增益值。表示導數收益。

以下是 PID 控制器的 python 代碼

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.widgets import Slider# Define PID Controller class with resistance
class PIDControllerWithResistance:def __init__(self, Kp, Ki, Kd, set_point=0, resistance_factor=0.1):self.Kp = Kpself.Ki = Kiself.Kd = Kdself.set_point = set_pointself.prev_error = 0self.integral = 0self.resistance_factor = resistance_factor  # Resistance to throttle (e.g., air resistance, friction)def update(self, current_value, dt):# Apply the same PID control logic but factor in resistanceerror = self.set_point - current_valueself.integral += error * dtderivative = (error - self.prev_error) / dtoutput = self.Kp * error + self.Ki * self.integral + self.Kd * derivativeself.prev_error = errorreturn output - self.resistance_factor * current_value  # Reduce output by a resistance factor# Simulation parameters
dt = 0.1  # Time step
time = np.arange(0, 50, dt)  # Simulation time# Initialize the PID controller with disturbance (resistance)
pid_with_resistance = PIDControllerWithResistance(Kp=1.0, Ki=0.05, Kd=0.01, set_point=50, resistance_factor=0.05)# Initial conditions
speed = 0
throttle_with_resistance = []
speed_record_with_resistance = []# Simulate the system with resistance
for t in time:control = pid_with_resistance.update(speed, dt)speed += control * dt  # Speed is affected by throttle control and resistancethrottle_with_resistance.append(control)speed_record_with_resistance.append(speed)# Plot setup
fig, ax = plt.subplots()
plt.subplots_adjust(left=0.1, bottom=0.3)l, = plt.plot(time, speed_record_with_resistance, label="Velocity Output (With Resistance)")
plt.axhline(pid_with_resistance.set_point, color='r', linestyle='--', label='Set Point')
plt.xlabel('Time [s]')
plt.ylabel('Velocity [m/s]')
plt.title('PID Cruise Control with Resistance')
plt.legend()
plt.show()

該代碼與 PI 控制器代碼非常相似，只是在 PI 控制器輸出中添加了導數項。它的計算公式為 。derivative = (error - self.prev_error) / dt

總結 PID 控制器，P 查看當前錯誤，I 查看過去的錯誤，D 預測未來的錯誤。它們共同調節輸出響應以達到所需的結果。

橫向控制簡介

當將機器人從一個位置移動到另一個位置時，僅使用速度控制（通常稱為縱向控制）不足以確保準確的路徑跟蹤。雖然縱向控制控制機器人的速度，但規劃器通常會生成一個由從機器人當前位置到目標的 3D 航路點組成的軌跡。為了有效地遵循這一軌跡，機器人還需要橫向控制，調整其轉向角度或方向以保持所需的路徑。橫向控制對于處理偏差、確保平穩導航和實現精確定位至關重要，尤其是在復雜環境中移動時。

有各種類型的側向控制器旨在處理此任務，每種類型都適用于不同的應用和條件。這些控制器可以大致分為幾何控制器和動態控制器。

• 幾何控制器：
  • Pure Pursuit （胡蘿卜追隨）
  • 赤 柱
  • PID （PID）

• 動態控制器：
  • MPC 控制器
  • 其他控制系統
    • 滑動模式、反饋線性化等

CARLA ROS2 集成

現在我們已經了解了 PID 控制器，讓我們使用它在 CARLA 中移動我們的車輛。只有兩個主文件用于執行此作，并且 ;我們將一一介紹它們。vehicle_ctrl.pylat_lon_ctrl.py

首先了解 中的縱向和橫向控制代碼。下面的代碼取自 CARLA 本身，略有修改。lat_lon_ctrl.py

from collections import deque
import math
import numpy as np
import carla
from agents.tools.misc import get_speed...class PIDLongitudinalController():"""PIDLongitudinalController implements longitudinal control using a PID."""def __init__(self, vehicle, K_P=1.0, K_I=0.0, K_D=0.0, dt=0.03):"""Constructor method.:param vehicle: actor to apply to local planner logic onto:param K_P: Proportional term:param K_D: Differential term:param K_I: Integral term:param dt: time differential in seconds"""self._vehicle = vehicleself._k_p = K_Pself._k_i = K_Iself._k_d = K_Dself._dt = dtself._error_buffer = deque(maxlen=10)def run_step(self, target_speed, debug=False):"""Execute one step of longitudinal control to reach a given target speed.:param target_speed: target speed in Km/h:param debug: boolean for debugging:return: throttle control"""current_speed = get_speed(self._vehicle)if debug:print('Current speed = {}'.format(current_speed))return self._pid_control(target_speed, current_speed)def _pid_control(self, target_speed, current_speed):"""Estimate the throttle/brake of the vehicle based on the PID equations:param target_speed:  target speed in Km/h:param current_speed: current speed of the vehicle in Km/h:return: throttle/brake control"""error = target_speed - current_speedself._error_buffer.append(error)if len(self._error_buffer) >= 2:_de = (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt_ie = sum(self._error_buffer) * self._dtelse:_de = 0.0_ie = 0.0return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0)def change_parameters(self, K_P, K_I, K_D, dt):"""Changes the PID parameters"""self._k_p = K_Pself._k_i = K_Iself._k_d = K_Dself._dt = dtclass PIDLateralController():"""PIDLateralController implements lateral control using a PID."""def __init__(self, vehicle, offset=0, K_P=1.0, K_I=0.0, K_D=0.0, dt=0.03):"""Constructor method.:param vehicle: actor to apply to local planner logic onto:param offset: distance to the center line. If might cause issues if the valueis large enough to make the vehicle invade other lanes.:param K_P: Proportional term:param K_D: Differential term:param K_I: Integral term:param dt: time differential in seconds"""self._vehicle = vehicleself._k_p = K_Pself._k_i = K_Iself._k_d = K_Dself._dt = dtself._offset = offsetself._e_buffer = deque(maxlen=10)def run_step(self, waypoint):"""Execute one step of lateral control to steerthe vehicle towards a certain waypoin.:param waypoint: target waypoint:return: steering control in the range [-1, 1] where:-1 maximum steering to left+1 maximum steering to right"""return self._pid_control(waypoint, self._vehicle.get_transform())def _pid_control(self, waypoint, vehicle_transform):"""Estimate the steering angle of the vehicle based on the PID equations:param waypoint: target waypoint:param vehicle_transform: current transform of the vehicle:return: steering control in the range [-1, 1]"""# Get the ego's location and forward vectorego_loc = vehicle_transform.locationv_vec = vehicle_transform.get_forward_vector()v_vec = np.array([v_vec.x, v_vec.y, 0.0])# Get the vector vehicle-target_wpif self._offset != 0:# Displace the wp to the sidew_tran = waypointr_vec = w_tran.get_right_vector()w_loc = w_tran.location + carla.Location(x=self._offset*r_vec.x,y=self._offset*r_vec.y)else:w_loc = waypoint.locationw_vec = np.array([w_loc.x - ego_loc.x,w_loc.y - ego_loc.y,0.0])wv_linalg = np.linalg.norm(w_vec) * np.linalg.norm(v_vec)if wv_linalg == 0:_dot = 1else:_dot = math.acos(np.clip(np.dot(w_vec, v_vec) / (wv_linalg), -1.0, 1.0))_cross = np.cross(v_vec, w_vec)if _cross[2] < 0:_dot *= -1.0self._e_buffer.append(_dot)if len(self._e_buffer) >= 2:_de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt_ie = sum(self._e_buffer) * self._dtelse:_de = 0.0_ie = 0.0return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0)def change_parameters(self, K_P, K_I, K_D, dt):"""Changes the PID parameters"""self._k_p = K_Pself._k_i = K_Iself._k_d = K_Dself._dt = dt

縱向控制

PIDLongitudinalControllerclass 包含縱向控制的代碼，我們首先初始化 PID 增益、Carla Vehicle 實例和誤差緩沖區（用于 D-Control）。 是從外部函數迭代調用 PID 更新的函數的函數。真正的魔力在于函數內部。實現非常簡單，類似于上面的 PID 實現。我們首先計算誤差，填充列表。如果有 2 個以上的元素，那么我們計算導數誤差 （） 和積分誤差 （），最后通過將其從 -1 裁剪到 1 來返回總控制輸出為 。run_step_pid_control_pid_controlerror = target_speed - current_speedself._error_bufferself._error_buffer_de_ienp.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0)

橫向控制

橫向控制有點棘手。在跳入代碼之前，我們先看看橫向誤差是如何計算的。誤差表示為無人機前向矢量和航路點矢量之間的有符號角。角度的標志是根據車輛是在所需路徑的左側還是右側來決定。角度應為正 （向右） 或負 （向左）。

在自動駕駛汽車的橫向控制中，主要目標是使車輛保持在正確的軌跡或路徑上。車輛的橫向控制通常與最小化與該所需路徑的偏差有關，這是通過控制轉向角來實現的。您提供的代碼使用 PID （Proportional， Integral， Derivative） 控制器根據此偏差調整車輛的轉向角。

讓我們來看看代碼。 是定義橫向控制的位置。以下是誤差角的計算方法，PIDLateralController

1.?車輛的前向矢量?（v_vec）：

正向向量表示無人機當前移動的方向，該方向源自無人機的變換 （）。它是一個 3D 向量，但由于車輛在 2D 平面上移動，因此僅使用 x 和 y 分量。vehicle_transform.get_forward_vector()

2.?航點矢量?（）：w_vec

航路點向量表示從機體的當前位置 （） 到目標航路點 （） 的方向：ego_locw_loc

3.?點積和角度：

機體航向與目標航點之間的誤差由機體的前向矢量與航點矢量之間的角度來測量。這是使用點積和這些向量的大小計算的：

此角度 （） 表示以弧度為單位的橫向誤差。_dot

4.?Sign 的叉積：

為了確定無人機是在所需路徑的左側還是右側（即誤差的符號），計算前向矢量和航點矢量之間的叉積：

叉積的第三個分量（z 分量）的符號確定角度應該是正 （向右） 還是負 （向左）。如果 z 分量為負，則角度反轉：

因此，橫向誤差 （） 是一個有符號角度，表示無人機需要轉向多少才能與航點對齊。_dot

PID 控制應用

錯誤 （） 被傳遞到 PID 控制器中，PID 控制器使用以下公式：_dot

現在我們已經完成了 PID 控制，讓我們看看如何使用 ROS 2 運行它。

現在讓我們來看看 .vehicle_ctrl.py

class CarlaVehicleControl(Node):def __init__(self):super().__init__('carla_route_planner')# Subscribersself.initialpose_sub = self.create_subscription(PoseWithCovarianceStamped,'/initialpose',self.initialpose_callback,10)self.goal_pose_sub = self.create_subscription(PoseStamped,'/goal_pose',self.goal_pose_callback,10)self.waypt_sub = self.create_subscription(Path, '/carla/ego_vehicle/waypoints', self.waypoints_callback, 10)# Subscriber to the /carla/ego_vehicle/odometry topicself.odom_sub = self.create_subscription(Odometry,'/carla/ego_vehicle/odometry',self.odometry_callback,10)self.vehicle_control_publisher = self.create_publisher(CarlaEgoVehicleControl, '/carla/ego_vehicle/vehicle_control_cmd', 10)# Initialize Carla client and mapself.client = Client('localhost', 2000)self.client.set_timeout(10.0)# Get the current worldself.world = self.client.get_world()# Check if Town01 is already loadedif 'Town01' not in self.world.get_map().name:print("Town01 is not loaded. Loading Town01...")self.world = self.client.load_world('Town01')print("Done!")else:print("Town01 is already loaded.")self.map = self.world.get_map()# Initialize GlobalRoutePlannerself.route_planner = GlobalRoutePlanner(self.map, 2.0)# Get all actors (vehicles, pedestrians, etc.) in the worldself.actors = self.world.get_actors()# Filter to get only the vehicles get the 0-th veh as there is only one vehself.vehicle = self.actors.filter('vehicle.*')[0]# Placeholders for start and end posesself.start_pose = Noneself.end_pose = Noneself.waypoints_list = []self.odom = None# TF2 listener and buffer# self.tf_buffer = Buffer()# self.tf_listener = TransformListener(self.tf_buffer, self)# self.vehicle_loc = Nonedef odometry_callback(self, msg):self.get_logger().info(f"Received odometry data: {msg.pose.pose.position.x}, {msg.pose.pose.position.y}, {msg.pose.pose.position.z}")# Extract position and orientation from Odometry messagex = msg.pose.pose.position.xy = -msg.pose.pose.position.yz = msg.pose.pose.position.zprint(" ^^^^ ODOM XYZ: ", x,y,z )orientation_q = msg.pose.pose.orientationroll, pitch, yaw = euler_from_quaternion([orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w])# Create a carla.Location objectlocation = carla.Location(x=x, y=y, z=z)# Create a carla.Rotation objectrotation = carla.Rotation(roll=roll, pitch=pitch, yaw=yaw)# Create a carla.Transform objecttransform = carla.Transform(location, rotation)self.odom = transformdef waypoints_callback(self, msg):# self.waypoints_list.clear()  # Clear the list before storing new waypoints# Iterate through all the waypoints in the Path messagefor pose in msg.poses:# Extract the position from the posex = pose.pose.position.xy = -pose.pose.position.yz = pose.pose.position.z# Extract the orientation (quaternion) from the poseorientation_q = pose.pose.orientationroll, pitch, yaw = euler_from_quaternion([orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w])# Create a carla.Location objectlocation = carla.Location(x=x, y=y, z=z)# Create a carla.Rotation objectrotation = carla.Rotation(roll=roll, pitch=pitch, yaw=yaw)# Create a carla.Transform objecttransform = carla.Transform(location, rotation)# Store the Waypoint in the global listself.waypoints_list.append(transform)self.get_logger().info(f"Stored {len(self.waypoints_list)} waypoints as carla.libcarla.Waypoint objects.")def create_ctrl_msg(self, throttle, steer, brake):control_msg = CarlaEgoVehicleControl()control_msg.throttle = throttlecontrol_msg.steer = steercontrol_msg.brake = brakereturn control_msgdef initialpose_callback(self, msg):self.get_logger().info("Received initialpose")self.start_pose = msg.pose.posedef goal_pose_callback(self, msg):self.get_logger().info("Received goal_pose")self.end_pose = msg.pose# Clear the waypoints list for the new goalself.waypoints_list.clear()def get_transform(self, vehicle_location, angle, d=6.4):a = math.radians(angle)location = carla.Location(d * math.cos(a), d * math.sin(a), 2.0) + vehicle_locationreturn carla.Transform(location, carla.Rotation(yaw=180 + angle, pitch=-15))def setup_PID(self, vehicle):"""This function creates a PID controller for the vehicle passed to it """args_lateral_dict = {'K_P': 0.5,  # Reduced proportional gain for smoother steering'K_D': 0.1,  # Small derivative gain to dampen oscillations'K_I': 0.01, # Small integral gain to correct for long-term drift'dt': 0.05}args_long_dict = {'K_P': 0.2,  # Slightly lower gain for acceleration control'K_D': 0.3,  # Moderate derivative gain'K_I': 0.01, # Small integral gain'dt': 0.05}PID= VehiclePIDController(vehicle,args_lateral=args_lateral_dict,args_longitudinal=args_long_dict)return PIDdef find_dist_veh(self, vehicle_loc,target):dist = math.sqrt( (target.location.x - vehicle_loc.x)**2 + \(target.location.y - vehicle_loc.y)**2 )return distdef drive_through_plan(self, planned_route, vehicle, speed, PID):"""This function drives throught the planned_route with the speed passed in the argument"""i=0waypt_cnt = len(planned_route)-1target=planned_route[0]cnt = 0while True:self.world.get_spectator().set_transform(self.get_transform(vehicle.get_location() +carla.Location(z=1, x=0.5), vehicle.get_transform().rotation.yaw-180))# vehicle_loc =  vehicle.get_location()vehicle_loc =  self.odom.locationdistance_v = self.find_dist_veh(vehicle_loc,target)control = PID.run_step(speed,target)# vehicle.apply_control(control)ctrl_msg = self.create_ctrl_msg(control.throttle,control.steer,control.brake)self.vehicle_control_publisher.publish(ctrl_msg)if i==(len(planned_route)-1):print("last waypoint reached")breakif (distance_v<3.5):control = PID.run_step(speed,target)# vehicle.apply_control(control)ctrl_msg = self.create_ctrl_msg(control.throttle,control.steer,control.brake)self.vehicle_control_publisher.publish(ctrl_msg)i=i+1target=planned_route[i]if cnt%5==0:print("=----------------------------------------------------------")print(f"\n{GREEN} ***** from current loc to {i}/{waypt_cnt} waypoint distance: {distance_v}{RESET}\n")print("ROS2 vehilce location: ", self.odom.location)print("CARLA vehilce location: ", vehicle.get_location())print("target location: ", target.location)rclpy.spin_once(self)# time.sleep(0.1)  # Add a slight delay to reduce control frequency# time.sleep(1)  # Add a 1-second delay# print("throttle: ", control.throttle)cnt+=1control = PID.run_step(0,planned_route[len(planned_route)-1])# vehicle.apply_control(control)ctrl_msg = self.create_ctrl_msg(control.throttle,control.steer,control.brake)self.vehicle_control_publisher.publish(ctrl_msg)def run(self):desired_velocity=10 #Km/hwhile rclpy.ok():rclpy.spin_once(self)if self.start_pose is None or self.end_pose is None:self.get_logger().info(f'Start pose: {self.start_pose}, End pose: {self.end_pose}')elif not self.waypoints_list:self.get_logger().info('Waiting for waypoints to be generated...')else:# Delay to ensure waypoints are populatedself.get_logger().info('Waiting a bit for waypoints to be fully populated...')time.sleep(1)  # Add a 1-second delayself.get_logger().info(f'Generated {len(self.waypoints_list)} waypoints from start to end pose')# calculating life time of the markertotal_dist = self.find_dist_veh(self.waypoints_list[0].location, self.waypoints_list[len(self.waypoints_list)-1])marker_life_time = (total_dist/desired_velocity) * 3.6# Draw waypoints on the Carla mapfor w in self.waypoints_list:# print("self.waypoints_list: ",w.location)self.world.debug.draw_string(w.location, 'O', draw_shadow=False,color=Color(r=255, g=0, b=0), life_time=5000000.0,persistent_lines=True)# drive the vehiclePID=self.setup_PID(self.vehicle)self.drive_through_plan(self.waypoints_list,self.vehicle,desired_velocity,PID)# After processing, break the loop if neededbreakdef main(args=None):rclpy.init(args=args)route_planner = CarlaVehicleControl()route_planner.run()rclpy.shutdown()

如果您一直在關注 robotics 系列，上面的代碼應該很容易理解。代碼的核心在于 and 函數。run()drive_through_plan()

• 調用 main 函數時，回調將轉到該函數。您會注意到我們沒有使用 ，而是使用 .這是因為 Continuous 旋轉以收集所有已發布的數據。然而，我們不能僅僅為了這個目的而旋轉——我們還需要根據輸入來移動車輛。如果我們只關注這一點，我們可能會錯過已發布的數據，導致車輛失控并可能撞墻。
  
  這可以通過以下方式處理：run()rclpy.spin()rclpy.spin_once(self)rclpy.spin()
  • 在單獨的線程中運行這兩個任務。
  • 運行 with 來收集已發布的數據，在循環中時，定期使用 來更新 subscriber 變量。rclpy.spin_once()while rclpy.ok()drive_through_planrclpy.spin_once()
• 工作原理的解釋如下：它首先設置一個目標軌跡點并進入一個 while 循環。此循環一直持續到飛機到達其目標位置。在循環中，它首先計算車輛位置與目標航點之間的距離，然后使用 PID 控制器根據所需的速度和目標航點計算轉向和油門值。這些值將應用于車輛。此過程將重復，直到機體與目標航點之間的距離小于 3.5。此時，它會重新計算油門和轉向值，應用它們，并更新目標航點。如果迭代到達最后一個航點，則車輛會根據所需的速度 0 制動并應用控制。此外，在該條件中，它會執行以更新里程計值。drive_through_planif cnt%5==0:rclpy.spin_once(self)

一切就緒后，剩下的工作就是運行代碼并觀察它的運行情況。

# in a new terminal, run carla first
# ./CarlaUE4.sh  # or ./CarlaUE4.sh -prefernvidia # $ ~/carla_simulator/PythonAPI/util/config.py --map Town01
$CARLA_ROOT/CarlaUE4.sh -quality-level=Low -prefernvidia -nosound# in a new terminal, get inside the `carla-ros-bridge/colcon_ws` folder and source the workspace; launch the `carla ros-bridge`
cd ~/carla-ros-bridge/colcon_ws && source install/setup.bash
ros2 launch carla_ros_bridge carla_ros_bridge.launch.py synchronous_mode:=True town:=Town01 # <town number, eg: 03># in a new terminal, launch the objects.json; launch ros-bridge
# cd ~/carla-ros-bridge/colcon_ws
cd ~/carla-ros-bridge/colcon_ws && source install/setup.bashros2 launch carla_spawn_objects carla_example_ego_vehicle.launch.py spawn_sensors_only:=False objects_definition_file:=<absolute path to>/src/vehicle_ctrl/vehicle_ctrl/config/objects.json# load the town1 lanelet map
python src/vehicle_ctrl/vehicle_ctrl/map.py# in new terminal, launch the rviz2 [set the global frame to map in rviz2]
rviz2 -d /src/vehicle_ctrl/rviz2/carla_map_spawn_anywherev2.rviz# in a new terminal, get inside the `carla-ros-bridge/colcon_ws` folder and source the workspace; waypoint publisher 
cd ~/carla-ros-bridge/colcon_ws && source install/setup.bash
ros2 launch carla_waypoint_publisher carla_waypoint_publisher.launch.py# goal remap
python src/vehicle_ctrl/vehicle_ctrl/remap_goal.py # waypoint following using carls ros-bridge
python src/vehicle_ctrl/vehicle_ctrl/simple_ctrl.py