From f03aa0ca542c4fafcf7cf936222edc22650bf703 Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Mon, 20 Oct 2025 10:13:27 +0800 Subject: [PATCH 01/13] =?UTF-8?q?=E5=B0=86=E7=AC=AC=E5=9B=9B=E9=A2=98?= =?UTF-8?q?=E4=B8=AD=E7=9A=84random((3,2))=E6=94=B9=E6=88=90=E4=BA=86rand(?= =?UTF-8?q?3,2),=E4=BB=A5=E5=8F=8A=E7=AC=AC=E4=BA=8C=E5=8D=81=E4=BA=94?= =?UTF-8?q?=E9=A2=98=E4=B8=ADarange=E6=94=B9=E6=88=90=E4=BA=86linspace?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/chap01_warmup/numpy_tutorial.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/chap01_warmup/numpy_tutorial.py b/src/chap01_warmup/numpy_tutorial.py index 93d587cfcb..37e51cb973 100644 --- a/src/chap01_warmup/numpy_tutorial.py +++ b/src/chap01_warmup/numpy_tutorial.py @@ -47,7 +47,7 @@ def question_4(): print("(3) 单位矩阵:\n", c) # 随机数矩阵,3x2:设置随机种子(42)确保结果可复现,生成0-1之间的浮点数 np.random.seed(42) # 固定随机种子,使随机结果可复现 - d = np.random.random((3, 2))# 生成一个形状为(3, 2)的NumPy数组,其中每个元素都是0到1之间的随机浮点数 + d = np.random.rand(3, 2)# 生成一个形状为(3, 2)的NumPy数组,其中每个元素都是0到1之间的随机浮点数 print("(4) 随机矩阵:\n", d) # 5. 建立一个数组 a,(值为 [[1, 2, 3, 4], [5, 6, 7, 8], [9, 10, 11, 12]] ) ,(1) 打印 a; (2) 输出数组中下标为 (2,3),(0,0) 这两个元素的值 @@ -253,7 +253,7 @@ def question_24(): #绘制二次函数 y = x^2 的图像。 def question_25(): print("第二十五题:绘制正弦和余弦函数") # 改用linspace确保包含端点 - x = np.arange(0, 3 * np.pi, 0.1) ## 生成从 0 到 3π 的 x 值,步长为 0.1 + x = np.linspace(0, 3 * np.pi, 100) ## 生成从 0 到 3π 的 x 值,步长为 0.1 y_sin = np.sin(x) # 计算对应的正弦值 y_cos = np.cos(x) # 计算对应的余弦值 From 69074b3c40eed350e211e2eae7d8de37f69a223f Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Tue, 21 Oct 2025 08:30:55 +0800 Subject: [PATCH 02/13] =?UTF-8?q?=E5=B0=86dw=E5=92=8Cdb=E7=9A=84=E8=AE=A1?= =?UTF-8?q?=E7=AE=97=E6=94=B9=E4=B8=BA=E4=BA=86=E6=9D=A1=E4=BB=B6=E8=A1=A8?= =?UTF-8?q?=E8=BE=BE=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/chap03_SVM/svm.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/chap03_SVM/svm.py b/src/chap03_SVM/svm.py index da1deeb2d1..122f9b9dd9 100644 --- a/src/chap03_SVM/svm.py +++ b/src/chap03_SVM/svm.py @@ -73,8 +73,8 @@ def train(self, data_train): # 计算梯度:正则化项梯度 + 误分类样本梯度 # L2正则化:减小权重,防止过拟合 # hinge loss梯度:只对误分类和边界样本计算梯度 - dw = (2 * self.reg_lambda * self.w) - np.mean(y[idx].reshape(-1, 1) * X[idx], axis=0) - db = -np.mean(y[idx]) + dw = (2 * self.reg_lambda * self.w) - (np.mean(y[idx].reshape(-1, 1) * X[idx], axis=0) if len(idx) > 0 else np.zeros_like(self.w)) + db = -np.mean(y[idx]) if len(idx) > 0 else 0 # 梯度下降更新参数 self.w -= self.learning_rate * dw # 权重更新:w = w - η*dw/dw From fa17f6f3ad2ea0668d5acfe1096dcb1036389c8f Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Mon, 27 Oct 2025 08:20:57 +0800 Subject: [PATCH 03/13] =?UTF-8?q?=E4=BD=BF=E7=94=A8np.sum=E4=BB=A3?= =?UTF-8?q?=E6=9B=BFeinsum,=E9=81=BF=E5=85=8D=E4=BA=86=E8=AF=AD=E6=B3=95?= =?UTF-8?q?=E5=A4=8D=E6=9D=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/chap11_gaussian_mixture/GMM.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/chap11_gaussian_mixture/GMM.py b/src/chap11_gaussian_mixture/GMM.py index 02714d3027..1867e627d5 100644 --- a/src/chap11_gaussian_mixture/GMM.py +++ b/src/chap11_gaussian_mixture/GMM.py @@ -266,7 +266,7 @@ def _log_gaussian(self, X, mu, sigma): # 计算二次型:(x-μ)^T·Σ^(-1)·(x-μ) # 使用einsum高效计算多个样本的二次型 - exponent = -0.5 * np.einsum('...i,...i->...', X_centered @ inv, X_centered) + exponent = -0.5 * np.sum(X_centered @ inv * X_centered, axis=1) # 返回对数概率密度 # 公式:log_p(x) = -0.5*D*log(2π) - 0.5*log|Σ| - 0.5*(x-μ)^T·Σ^(-1)·(x-μ) From f23f149e0c7b2b36ac680a3b435db376b5a1ecbe Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Mon, 3 Nov 2025 08:42:29 +0800 Subject: [PATCH 04/13] =?UTF-8?q?np.einsum=E6=9B=BF=E6=8D=A2=E4=B8=BAnp.do?= =?UTF-8?q?t=EF=BC=8C=E5=90=8C=E6=97=B6=E4=BF=9D=E6=8C=81=E4=BA=86?= =?UTF-8?q?=E6=AD=A3=E5=88=99=E5=8C=96=E4=B8=80=E8=87=B4=EF=BC=8C=E6=8F=90?= =?UTF-8?q?=E9=AB=98=E4=BB=A3=E7=A0=81=E4=B8=80=E8=87=B4=E6=80=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/chap11_gaussian_mixture/.idea/.gitignore | 5 +++++ .../.idea/chap11_gaussian_mixture.iml | 12 ++++++++++++ .../.idea/inspectionProfiles/profiles_settings.xml | 6 ++++++ src/chap11_gaussian_mixture/.idea/misc.xml | 7 +++++++ src/chap11_gaussian_mixture/.idea/modules.xml | 8 ++++++++ src/chap11_gaussian_mixture/.idea/vcs.xml | 6 ++++++ src/chap11_gaussian_mixture/GMM.py | 9 ++++----- 7 files changed, 48 insertions(+), 5 deletions(-) create mode 100644 src/chap11_gaussian_mixture/.idea/.gitignore create mode 100644 src/chap11_gaussian_mixture/.idea/chap11_gaussian_mixture.iml create mode 100644 src/chap11_gaussian_mixture/.idea/inspectionProfiles/profiles_settings.xml create mode 100644 src/chap11_gaussian_mixture/.idea/misc.xml create mode 100644 src/chap11_gaussian_mixture/.idea/modules.xml create mode 100644 src/chap11_gaussian_mixture/.idea/vcs.xml diff --git a/src/chap11_gaussian_mixture/.idea/.gitignore b/src/chap11_gaussian_mixture/.idea/.gitignore new file mode 100644 index 0000000000..10b731c518 --- /dev/null +++ b/src/chap11_gaussian_mixture/.idea/.gitignore @@ -0,0 +1,5 @@ +# 默认忽略的文件 +/shelf/ +/workspace.xml +# 基于编辑器的 HTTP 客户端请求 +/httpRequests/ diff --git a/src/chap11_gaussian_mixture/.idea/chap11_gaussian_mixture.iml b/src/chap11_gaussian_mixture/.idea/chap11_gaussian_mixture.iml new file mode 100644 index 0000000000..94219bfb57 --- /dev/null +++ b/src/chap11_gaussian_mixture/.idea/chap11_gaussian_mixture.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/src/chap11_gaussian_mixture/.idea/inspectionProfiles/profiles_settings.xml b/src/chap11_gaussian_mixture/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000000..105ce2da2d --- /dev/null +++ b/src/chap11_gaussian_mixture/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/src/chap11_gaussian_mixture/.idea/misc.xml b/src/chap11_gaussian_mixture/.idea/misc.xml new file mode 100644 index 0000000000..db8786c06e --- /dev/null +++ b/src/chap11_gaussian_mixture/.idea/misc.xml @@ -0,0 +1,7 @@ + + + + + + \ No newline at end of file diff --git a/src/chap11_gaussian_mixture/.idea/modules.xml b/src/chap11_gaussian_mixture/.idea/modules.xml new file mode 100644 index 0000000000..eaf08a999b --- /dev/null +++ b/src/chap11_gaussian_mixture/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/src/chap11_gaussian_mixture/.idea/vcs.xml b/src/chap11_gaussian_mixture/.idea/vcs.xml new file mode 100644 index 0000000000..b2bdec2d71 --- /dev/null +++ b/src/chap11_gaussian_mixture/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/src/chap11_gaussian_mixture/GMM.py b/src/chap11_gaussian_mixture/GMM.py index 1867e627d5..a0f4f3c6d0 100644 --- a/src/chap11_gaussian_mixture/GMM.py +++ b/src/chap11_gaussian_mixture/GMM.py @@ -197,11 +197,10 @@ def fit(self, X): # 使用einsum高效计算协方差矩阵 # 等价于: new_sigma_k = (X_centered.T @ diag(gamma[:,k]) @ X_centered) / Nk[k] - new_sigma_k = np.einsum('ni,nj->ij', X_centered, weighted_X) / Nk[k] - - # 正则化:添加小的对角矩阵,防止协方差矩阵奇异 - eps = 1e-6 # 正则化系数 - new_sigma_k += np.eye(n_features) * eps # 添加单位矩阵的eps倍 + new_sigma_k = np.dot(weighted_X.T, X_centered) / Nk[k] + + # 统一正则化处理,确保协方差矩阵正定 + new_sigma_k += np.eye(n_features) * 1e-6 new_sigma[k] = new_sigma_k # 存储更新后的协方差矩阵 From 605a28eaba6e50118339afa0b021beeafa47c1e5 Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Tue, 4 Nov 2025 08:08:48 +0800 Subject: [PATCH 05/13] =?UTF-8?q?=E4=BD=BF=E7=94=A8=E6=9B=B4=E5=8A=A0?= =?UTF-8?q?=E7=AE=80=E4=BE=BF=E7=9A=84=E6=9D=A1=E4=BB=B6=E8=A1=A8=E8=BE=BE?= =?UTF-8?q?=E5=BC=8F=EF=BC=8C=E5=87=8F=E5=B0=91=E4=BA=86=E5=B5=8C=E5=A5=97?= =?UTF-8?q?=E5=B1=82=E6=AC=A1?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/chap03_SVM/.idea/.gitignore | 5 ----- src/chap03_SVM/.idea/.name | 1 - src/chap03_SVM/.idea/chap03_SVM.iml | 12 ------------ .../.idea/inspectionProfiles/profiles_settings.xml | 6 ------ src/chap03_SVM/.idea/misc.xml | 7 ------- src/chap03_SVM/.idea/modules.xml | 8 -------- src/chap03_SVM/.idea/vcs.xml | 6 ------ src/chap03_SVM/svm.py | 4 ++-- 8 files changed, 2 insertions(+), 47 deletions(-) delete mode 100644 src/chap03_SVM/.idea/.gitignore delete mode 100644 src/chap03_SVM/.idea/.name delete mode 100644 src/chap03_SVM/.idea/chap03_SVM.iml delete mode 100644 src/chap03_SVM/.idea/inspectionProfiles/profiles_settings.xml delete mode 100644 src/chap03_SVM/.idea/misc.xml delete mode 100644 src/chap03_SVM/.idea/modules.xml delete mode 100644 src/chap03_SVM/.idea/vcs.xml diff --git a/src/chap03_SVM/.idea/.gitignore b/src/chap03_SVM/.idea/.gitignore deleted file mode 100644 index 10b731c518..0000000000 --- a/src/chap03_SVM/.idea/.gitignore +++ /dev/null @@ -1,5 +0,0 @@ -# 默认忽略的文件 -/shelf/ -/workspace.xml -# 基于编辑器的 HTTP 客户端请求 -/httpRequests/ diff --git a/src/chap03_SVM/.idea/.name b/src/chap03_SVM/.idea/.name deleted file mode 100644 index 826e26f052..0000000000 --- a/src/chap03_SVM/.idea/.name +++ /dev/null @@ -1 +0,0 @@ -svm.py \ No newline at end of file diff --git a/src/chap03_SVM/.idea/chap03_SVM.iml b/src/chap03_SVM/.idea/chap03_SVM.iml deleted file mode 100644 index 07abf20250..0000000000 --- a/src/chap03_SVM/.idea/chap03_SVM.iml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/src/chap03_SVM/.idea/inspectionProfiles/profiles_settings.xml b/src/chap03_SVM/.idea/inspectionProfiles/profiles_settings.xml deleted file mode 100644 index 105ce2da2d..0000000000 --- a/src/chap03_SVM/.idea/inspectionProfiles/profiles_settings.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - \ No newline at end of file diff --git a/src/chap03_SVM/.idea/misc.xml b/src/chap03_SVM/.idea/misc.xml deleted file mode 100644 index db8786c06e..0000000000 --- a/src/chap03_SVM/.idea/misc.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/src/chap03_SVM/.idea/modules.xml b/src/chap03_SVM/.idea/modules.xml deleted file mode 100644 index dd5df7cb59..0000000000 --- a/src/chap03_SVM/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/src/chap03_SVM/.idea/vcs.xml b/src/chap03_SVM/.idea/vcs.xml deleted file mode 100644 index b2bdec2d71..0000000000 --- a/src/chap03_SVM/.idea/vcs.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/src/chap03_SVM/svm.py b/src/chap03_SVM/svm.py index 6e05d32743..6bbcfc6692 100644 --- a/src/chap03_SVM/svm.py +++ b/src/chap03_SVM/svm.py @@ -73,8 +73,8 @@ def train(self, data_train): # 计算梯度:正则化项梯度 + 误分类样本梯度 # L2正则化:减小权重,防止过拟合 # hinge loss梯度:只对误分类和边界样本计算梯度 - dw = (2 * self.reg_lambda * self.w) - (np.sum(y[idx, None] * X[idx], axis=0) / m if len(idx) > 0 else 0) - + dw = (2 * self.reg_lambda * self.w) - np.sum(y[idx, None] * X[idx], axis=0) / m if len( + idx) > 0 else 2 * self.reg_lambda * self.w db = -np.mean(y[idx]) if len(idx) > 0 else 0 # 梯度下降更新参数 From 91913f6fcfd870d49453205bbe6e61b4f36ddcbd Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Tue, 4 Nov 2025 09:16:58 +0800 Subject: [PATCH 06/13] Delete src/chap11_gaussian_mixture/.idea directory --- src/chap11_gaussian_mixture/.idea/.gitignore | 5 ----- .../.idea/chap11_gaussian_mixture.iml | 12 ------------ .../.idea/inspectionProfiles/profiles_settings.xml | 6 ------ src/chap11_gaussian_mixture/.idea/misc.xml | 7 ------- src/chap11_gaussian_mixture/.idea/modules.xml | 8 -------- src/chap11_gaussian_mixture/.idea/vcs.xml | 6 ------ 6 files changed, 44 deletions(-) delete mode 100644 src/chap11_gaussian_mixture/.idea/.gitignore delete mode 100644 src/chap11_gaussian_mixture/.idea/chap11_gaussian_mixture.iml delete mode 100644 src/chap11_gaussian_mixture/.idea/inspectionProfiles/profiles_settings.xml delete mode 100644 src/chap11_gaussian_mixture/.idea/misc.xml delete mode 100644 src/chap11_gaussian_mixture/.idea/modules.xml delete mode 100644 src/chap11_gaussian_mixture/.idea/vcs.xml diff --git a/src/chap11_gaussian_mixture/.idea/.gitignore b/src/chap11_gaussian_mixture/.idea/.gitignore deleted file mode 100644 index 10b731c518..0000000000 --- a/src/chap11_gaussian_mixture/.idea/.gitignore +++ /dev/null @@ -1,5 +0,0 @@ -# 默认忽略的文件 -/shelf/ -/workspace.xml -# 基于编辑器的 HTTP 客户端请求 -/httpRequests/ diff --git a/src/chap11_gaussian_mixture/.idea/chap11_gaussian_mixture.iml b/src/chap11_gaussian_mixture/.idea/chap11_gaussian_mixture.iml deleted file mode 100644 index 94219bfb57..0000000000 --- a/src/chap11_gaussian_mixture/.idea/chap11_gaussian_mixture.iml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/src/chap11_gaussian_mixture/.idea/inspectionProfiles/profiles_settings.xml b/src/chap11_gaussian_mixture/.idea/inspectionProfiles/profiles_settings.xml deleted file mode 100644 index 105ce2da2d..0000000000 --- a/src/chap11_gaussian_mixture/.idea/inspectionProfiles/profiles_settings.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - \ No newline at end of file diff --git a/src/chap11_gaussian_mixture/.idea/misc.xml b/src/chap11_gaussian_mixture/.idea/misc.xml deleted file mode 100644 index db8786c06e..0000000000 --- a/src/chap11_gaussian_mixture/.idea/misc.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/src/chap11_gaussian_mixture/.idea/modules.xml b/src/chap11_gaussian_mixture/.idea/modules.xml deleted file mode 100644 index eaf08a999b..0000000000 --- a/src/chap11_gaussian_mixture/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/src/chap11_gaussian_mixture/.idea/vcs.xml b/src/chap11_gaussian_mixture/.idea/vcs.xml deleted file mode 100644 index b2bdec2d71..0000000000 --- a/src/chap11_gaussian_mixture/.idea/vcs.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file From cb9d05973e18955633bb82187e5c3b8976c02d74 Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Mon, 17 Nov 2025 08:18:52 +0800 Subject: [PATCH 07/13] =?UTF-8?q?=E4=BC=98=E5=8C=96=E5=8D=8F=E6=96=B9?= =?UTF-8?q?=E5=B7=AE=E7=9F=A9=E9=98=B5=E8=AE=A1=E7=AE=97=EF=BC=8C=E6=9B=B4?= =?UTF-8?q?=E6=B8=85=E6=99=B0=E8=A1=A8=E8=BE=BE=E5=BC=A0=E9=87=8F=E8=AE=A1?= =?UTF-8?q?=E7=AE=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/chap11_gaussian_mixture/GMM.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/chap11_gaussian_mixture/GMM.py b/src/chap11_gaussian_mixture/GMM.py index a0f4f3c6d0..54d8949a66 100644 --- a/src/chap11_gaussian_mixture/GMM.py +++ b/src/chap11_gaussian_mixture/GMM.py @@ -197,7 +197,8 @@ def fit(self, X): # 使用einsum高效计算协方差矩阵 # 等价于: new_sigma_k = (X_centered.T @ diag(gamma[:,k]) @ X_centered) / Nk[k] - new_sigma_k = np.dot(weighted_X.T, X_centered) / Nk[k] + # 更稳定的协方差计算方式 + new_sigma_k = np.einsum('ki,kj->ij', gamma[:, k, None] * X_centered, X_centered) / Nk[k] # 统一正则化处理,确保协方差矩阵正定 new_sigma_k += np.eye(n_features) * 1e-6 From fae69415ec9dd67f576e6d41bb1de80ab1f8670f Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Tue, 18 Nov 2025 08:48:50 +0800 Subject: [PATCH 08/13] =?UTF-8?q?=E8=BF=9B=E8=A1=8C=E9=80=89=E9=A2=98?= =?UTF-8?q?=EF=BC=8C=E5=B9=B6=E5=A2=9E=E5=8A=A0README?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Object_Tracking_Planning/README.md | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 src/Object_Tracking_Planning/README.md diff --git a/src/Object_Tracking_Planning/README.md b/src/Object_Tracking_Planning/README.md new file mode 100644 index 0000000000..72ed89f513 --- /dev/null +++ b/src/Object_Tracking_Planning/README.md @@ -0,0 +1,2 @@ +这是一个关于利用yolov3和carla模拟器进行·自主车辆目标检测与轨迹规划,与模拟器相结合,以提升车辆的感知与决策能力。 + From 0671a4e707d70a411ce68d876afe56d441c6c55a Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Mon, 1 Dec 2025 08:17:34 +0800 Subject: [PATCH 09/13] =?UTF-8?q?=E9=81=BF=E5=85=8D=E9=87=8D=E5=A4=8D?= =?UTF-8?q?=E8=AE=A1=E7=AE=97=E6=A2=AF=E5=BA=A6=EF=BC=8C=E6=8F=90=E9=AB=98?= =?UTF-8?q?=E6=95=88=E7=8E=87?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../tutorial_minst_fnn-tf2.0-exercise.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/chap04_simple_neural_network/tutorial_minst_fnn-tf2.0-exercise.py b/src/chap04_simple_neural_network/tutorial_minst_fnn-tf2.0-exercise.py index c3827b718c..c4cda955ba 100644 --- a/src/chap04_simple_neural_network/tutorial_minst_fnn-tf2.0-exercise.py +++ b/src/chap04_simple_neural_network/tutorial_minst_fnn-tf2.0-exercise.py @@ -115,12 +115,13 @@ def train_one_step(model, optimizer, x, y): 1. 使用梯度裁剪防止梯度爆炸 2. 所有计算都在GradientTape上下文中进行自动微分 """ - with tf.GradientTape() as tape: # 记录计算图以计算梯度 - logits = model(x) # 前向传播 - loss = compute_loss(logits, y) # 计算损失 - grads = tape.gradient(loss,model.trainable_variables) - #添加梯度裁剪 控制在1.0以内 - grads = tape.gradient(loss, model.trainable_variables) # 计算梯度 + with tf.GradientTape() as tape: + logits = model(x) + loss = compute_loss(logits, y) + # 移出with块,只计算一次梯度,并添加梯度裁剪 + grads = tape.gradient(loss, model.trainable_variables) + grads = [tf.clip_by_value(grad, -1.0, 1.0) for grad in grads] # 添加梯度裁剪 + # 计算梯度 optimizer.apply_gradients(zip(grads, model.trainable_variables)) # 更新参数 accuracy = compute_accuracy(logits, y) # 计算准确率 From 4b4d3a6756543589ac56f50b5750c046a57e3c54 Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Wed, 17 Dec 2025 22:10:17 +0800 Subject: [PATCH 10/13] =?UTF-8?q?=E8=B0=83=E8=AF=95=E7=8E=AF=E5=A2=83?= =?UTF-8?q?=E6=88=90=E5=8A=9F=EF=BC=8C=E7=A8=8B=E5=BA=8F=E5=8F=AF=E4=BB=A5?= =?UTF-8?q?=E6=AD=A3=E5=B8=B8=E8=BF=90=E8=A1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../object_detection.py | 1842 +++++++++++++++++ 1 file changed, 1842 insertions(+) create mode 100644 src/Object_Tracking_Planning/object_detection.py diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py new file mode 100644 index 0000000000..285fd97222 --- /dev/null +++ b/src/Object_Tracking_Planning/object_detection.py @@ -0,0 +1,1842 @@ +"""Example of automatic vehicle control from client side.""" + +from __future__ import print_function + +import argparse +import collections +import datetime +import glob +import logging +import math +import os +import cv2 +import random +import re +import sys +import weakref +from enum import Enum +from collections import deque + + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import K_ESCAPE + from pygame.locals import K_q +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError( + 'cannot import numpy, make sure numpy package is installed') + +# ============================================================================== +# -- Find CARLA module --------------------------------------------------------- +# ============================================================================== +try: + sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( + sys.version_info.major, + sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) +except IndexError: + pass + +# ============================================================================== +# -- Add PythonAPI for release mode -------------------------------------------- +# ============================================================================== +try: + sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + '/carla') +except IndexError: + pass + +import carla +from carla import ColorConverter as cc +from torch.utils.tensorboard import SummaryWriter +import torch +#import tensorflow as tf + +# Create a SummaryWriter object to write the metrics to Tensorboard +writer = SummaryWriter() + +from agents.navigation.global_route_planner import GlobalRoutePlanner +from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO +from agents.navigation.types_behavior import Cautious, Aggressive, Normal + +from agents.tools.misc import get_speed, positive, draw_waypoints, distance_vehicle + + +# ============================================================================== +# -- Global functions ---------------------------------------------------------- +# ============================================================================== + + +def find_weather_presets(): + """Method to find weather presets""" + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') + def name(x): return ' '.join(m.group(0) for m in rgx.finditer(x)) + presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + """Method to get actor display name""" + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name + + +# ============================================================================== +# -- World --------------------------------------------------------------- +# ============================================================================== + +class World(object): + """ Class representing the surrounding environment """ + + def __init__(self, carla_world, hud, args): + """Constructor method""" + self.world = carla_world + try: + self.map = self.world.get_map() + except RuntimeError as error: + print('RuntimeError: {}'.format(error)) + print(' The server could not send the OpenDRIVE (.xodr) file:') + print(' Make sure it exists, has the same name of your town, and is correct.') + sys.exit(1) + self.hud = hud + self.player = None + self.collision_sensor = None + self.lane_invasion_sensor = None + self.gnss_sensor = None + self.camera_manager = None + self._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = args.filter + self._gamma = args.gamma + self.restart(args) + self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False + self.recording_start = 0 + + def restart(self, args): + """Restart the world""" + # Keep same camera config if the camera manager exists. + cam_index = self.camera_manager.index if self.camera_manager is not None else 0 + cam_pos_id = self.camera_manager.transform_index if self.camera_manager is not None else 0 + # Set the seed if requested by user + if args.seed is not None: + random.seed(args.seed) + + # KK Get a vehicle blueprint + #blueprint = self.world.get_blueprint_library().filter('vehicle.lincoln.mkz2017')[0] # KK + blueprint = self.world.get_blueprint_library().filter('model3')[0] + blueprint.set_attribute('role_name', 'hero') + if blueprint.has_attribute('color'): + color = '236,102,17' # KK Packt logo color + blueprint.set_attribute('color', color) + # Spawn the player. + print("Spawning the player") + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + + while self.player is None: + if not self.map.get_spawn_points(): + print('There are no spawn points available in your map/town.') + print('Please add some Vehicle Spawn Point to your UE4 scene.') + sys.exit(1) + spawn_points = self.map.get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + #spawn_point = spawn_points[0] if spawn_points else carla.Transform() # KK + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + # Set up the sensors. + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud, self._gamma) + self.camera_manager.transform_index = cam_pos_id + self.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.player) + self.hud.notification(actor_type) + + def next_weather(self, reverse=False): + """Get next weather setting""" + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.player.get_world().set_weather(preset[0]) + + def tick(self, clock): + """Method for every tick""" + self.hud.tick(self, clock) + + def render(self, display): + """Render world""" + self.camera_manager.render(display) + self.hud.render(display) + + def destroy_sensors(self): + """Destroy sensors""" + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None + + def destroy(self): + """Destroys all actors""" + actors = [ + self.camera_manager.sensor, + self.collision_sensor.sensor, + self.lane_invasion_sensor.sensor, + self.gnss_sensor.sensor, + self.player] + for actor in actors: + if actor is not None: + actor.destroy() + + +# ============================================================================== +# -- KeyboardControl ----------------------------------------------------------- +# ============================================================================== + + +class KeyboardControl(object): + def __init__(self, world): + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def parse_events(self): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + if event.type == pygame.KEYUP: + if self._is_quit_shortcut(event.key): + return True + + @staticmethod + def _is_quit_shortcut(key): + """Shortcut for quitting""" + return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) + +# ============================================================================== +# -- HUD ----------------------------------------------------------------------- +# ============================================================================== + + +class HUD(object): + """Class for HUD text""" + + def __init__(self, width, height): + """Constructor method""" + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 20) + font_name = 'courier' if os.name == 'nt' else 'mono' + fonts = [x for x in pygame.font.get_fonts() if font_name in x] + default_font = 'ubuntumono' + mono = default_font if default_font in fonts else fonts[0] + mono = pygame.font.match_font(mono) + self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + self.help = HelpText(pygame.font.Font(mono, 24), width, height) + self.server_fps = 0 + self.frame = 0 + self.simulation_time = 0 + self._show_info = True + self._info_text = [] + self._server_clock = pygame.time.Clock() + + def on_world_tick(self, timestamp): + """Gets informations from the world at every tick""" + self._server_clock.tick() + self.server_fps = self._server_clock.get_fps() + self.frame = timestamp.frame_count + self.simulation_time = timestamp.elapsed_seconds + + def tick(self, world, clock): + """HUD method for every tick""" + self._notifications.tick(world, clock) + if not self._show_info: + return + transform = world.player.get_transform() + vel = world.player.get_velocity() + control = world.player.get_control() + heading = 'N' if abs(transform.rotation.yaw) < 89.5 else '' + heading += 'S' if abs(transform.rotation.yaw) > 90.5 else '' + heading += 'E' if 179.5 > transform.rotation.yaw > 0.5 else '' + heading += 'W' if -0.5 > transform.rotation.yaw > -179.5 else '' + colhist = world.collision_sensor.get_collision_history() + collision = [colhist[x + self.frame - 200] for x in range(0, 200)] + max_col = max(1.0, max(collision)) + collision = [x / max_col for x in collision] + vehicles = world.world.get_actors().filter('vehicle.*') + + self._info_text = [ + 'Server: % 16.0f FPS' % self.server_fps, + 'Client: % 16.0f FPS' % clock.get_fps(), + '', + 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), + 'Map: % 20s' % world.map.name, + 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), + '', + 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)), + u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (transform.rotation.yaw, heading), + 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (transform.location.x, transform.location.y)), + 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), + 'Height: % 18.0f m' % transform.location.z, + ''] + if isinstance(control, carla.VehicleControl): + self._info_text += [ + ('Throttle:', control.throttle, 0.0, 1.0), + ('Steer:', control.steer, -1.0, 1.0), + ('Brake:', control.brake, 0.0, 1.0), + ('Reverse:', control.reverse), + ('Hand brake:', control.hand_brake), + ('Manual:', control.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(control.gear, control.gear)] + elif isinstance(control, carla.WalkerControl): + self._info_text += [ + ('Speed:', control.speed, 0.0, 5.556), + ('Jump:', control.jump)] + self._info_text += [ + '', + 'Collision:', + collision, + '', + 'Number of vehicles: % 8d' % len(vehicles)] + + if len(vehicles) > 1: + self._info_text += ['Nearby vehicles:'] + + def dist(l): + return math.sqrt((l.x - transform.location.x)**2 + (l.y - transform.location.y) + ** 2 + (l.z - transform.location.z)**2) + vehicles = [(dist(x.get_location()), x) for x in vehicles if x.id != world.player.id] + + for dist, vehicle in sorted(vehicles): + if dist > 200.0: + break + vehicle_type = get_actor_display_name(vehicle, truncate=22) + self._info_text.append('% 4dm %s' % (dist, vehicle_type)) + + def toggle_info(self): + """Toggle info on or off""" + self._show_info = not self._show_info + + def notification(self, text, seconds=2.0): + """Notification text""" + self._notifications.set_text(text, seconds=seconds) + + def error(self, text): + """Error text""" + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display): + """Render for HUD class""" + if self._show_info: + info_surface = pygame.Surface((220, self.dim[1])) + info_surface.set_alpha(100) + display.blit(info_surface, (0, 0)) + v_offset = 4 + bar_h_offset = 100 + bar_width = 106 + for item in self._info_text: + if v_offset + 18 > self.dim[1]: + break + if isinstance(item, list): + if len(item) > 1: + points = [(x + 8, v_offset + 8 + (1 - y) * 30) for x, y in enumerate(item)] + pygame.draw.lines(display, (255, 136, 0), False, points, 2) + item = None + v_offset += 18 + elif isinstance(item, tuple): + if isinstance(item[1], bool): + rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) + pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) + else: + rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect_border, 1) + fig = (item[1] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect( + (bar_h_offset + fig * (bar_width - 6), v_offset + 8), (6, 6)) + else: + rect = pygame.Rect((bar_h_offset, v_offset + 8), (fig * bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect) + item = item[0] + if item: # At this point has to be a str. + surface = self._font_mono.render(item, True, (255, 255, 255)) + display.blit(surface, (8, v_offset)) + v_offset += 18 + self._notifications.render(display) + self.help.render(display) + +# ============================================================================== +# -- FadingText ---------------------------------------------------------------- +# ============================================================================== + + +class FadingText(object): + """ Class for fading text """ + + def __init__(self, font, dim, pos): + """Constructor method""" + self.font = font + self.dim = dim + self.pos = pos + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + + def set_text(self, text, color=(255, 255, 255), seconds=2.0): + """Set fading text""" + text_texture = self.font.render(text, True, color) + self.surface = pygame.Surface(self.dim) + self.seconds_left = seconds + self.surface.fill((0, 0, 0, 0)) + self.surface.blit(text_texture, (10, 11)) + + def tick(self, _, clock): + """Fading text method for every tick""" + delta_seconds = 1e-3 * clock.get_time() + self.seconds_left = max(0.0, self.seconds_left - delta_seconds) + self.surface.set_alpha(500.0 * self.seconds_left) + + def render(self, display): + """Render fading text method""" + display.blit(self.surface, self.pos) + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + """ Helper class for text render""" + + def __init__(self, font, width, height): + """Constructor method""" + lines = __doc__.split('\n') + self.font = font + self.dim = (680, len(lines) * 22 + 12) + self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + self.surface.fill((0, 0, 0, 0)) + for i, line in enumerate(lines): + text_texture = self.font.render(line, True, (255, 255, 255)) + self.surface.blit(text_texture, (22, i * 22)) + self._render = False + self.surface.set_alpha(220) + + def toggle(self): + """Toggle on or off the render help""" + self._render = not self._render + + def render(self, display): + """Render help text method""" + if self._render: + display.blit(self.surface, self.pos) + +# ============================================================================== +# -- CollisionSensor ----------------------------------------------------------- +# ============================================================================== + + +class CollisionSensor(object): + """ Class for collision sensors""" + + def __init__(self, parent_actor, hud): + """Constructor method""" + self.sensor = None + self.history = [] + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + blueprint = world.get_blueprint_library().find('sensor.other.collision') + self.sensor = world.spawn_actor(blueprint, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) + + def get_collision_history(self): + """Gets the history of collisions""" + history = collections.defaultdict(int) + for frame, intensity in self.history: + history[frame] += intensity + return history + + @staticmethod + def _on_collision(weak_self, event): + """On collision method""" + self = weak_self() + if not self: + return + actor_type = get_actor_display_name(event.other_actor) + self.hud.notification('Collision with %r' % actor_type) + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x ** 2 + impulse.y ** 2 + impulse.z ** 2) + self.history.append((event.frame, intensity)) + if len(self.history) > 4000: + self.history.pop(0) + +# ============================================================================== +# -- LaneInvasionSensor -------------------------------------------------------- +# ============================================================================== + + +class LaneInvasionSensor(object): + """Class for lane invasion sensors""" + + def __init__(self, parent_actor, hud): + """Constructor method""" + self.sensor = None + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.lane_invasion') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) + + @staticmethod + def _on_invasion(weak_self, event): + """On invasion method""" + self = weak_self() + if not self: + return + lane_types = set(x.type for x in event.crossed_lane_markings) + text = ['%r' % str(x).split()[-1] for x in lane_types] + self.hud.notification('Crossed line %s' % ' and '.join(text)) + +# ============================================================================== +# -- GnssSensor -------------------------------------------------------- +# ============================================================================== + + +class GnssSensor(object): + """ Class for GNSS sensors""" + + def __init__(self, parent_actor): + """Constructor method""" + self.sensor = None + self._parent = parent_actor + self.lat = 0.0 + self.lon = 0.0 + world = self._parent.get_world() + blueprint = world.get_blueprint_library().find('sensor.other.gnss') + self.sensor = world.spawn_actor(blueprint, carla.Transform(carla.Location(x=1.0, z=2.8)), + attach_to=self._parent) + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) + + @staticmethod + def _on_gnss_event(weak_self, event): + """GNSS method""" + self = weak_self() + if not self: + return + self.lat = event.latitude + self.lon = event.longitude + +# ============================================================================== +# -- CameraManager ------------------------------------------------------------- +# ============================================================================== + + +class CameraManager(object): + """ Class for camera management""" + + def __init__(self, parent_actor, hud, gamma_correction): + """Constructor method""" + self.sensor = None + self.surface = None + self._parent = parent_actor + self.hud = hud + self.recording = False + bound_y = 0.5 + self._parent.bounding_box.extent.y + attachment = carla.AttachmentType + self._camera_transforms = [ + (carla.Transform( + carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8.0)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=1.6, z=1.7)), attachment.Rigid), + (carla.Transform( + carla.Location(x=5.5, y=1.5, z=1.5)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=-8.0, z=6.0), carla.Rotation(pitch=6.0)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=-1, y=-bound_y, z=0.5)), attachment.Rigid)] + self.transform_index = 1 + self.sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, + 'Camera Semantic Segmentation (CityScapes Palette)'], + ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + blp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + blp.set_attribute('image_size_x', str(hud.dim[0])) + blp.set_attribute('image_size_y', str(hud.dim[1])) + if blp.has_attribute('gamma'): + blp.set_attribute('gamma', str(gamma_correction)) + elif item[0].startswith('sensor.lidar'): + blp.set_attribute('range', '50') + item.append(blp) + self.index = None + + def toggle_camera(self): + """Activate a camera""" + self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) + self.set_sensor(self.index, notify=False, force_respawn=True) + + def set_sensor(self, index, notify=True, force_respawn=False): + """Set a sensor""" + index = index % len(self.sensors) + needs_respawn = True if self.index is None else ( + force_respawn or (self.sensors[index][0] != self.sensors[self.index][0])) + if needs_respawn: + if self.sensor is not None: + self.sensor.destroy() + self.surface = None + self.sensor = self._parent.get_world().spawn_actor( + self.sensors[index][-1], + self._camera_transforms[self.transform_index][0], + attach_to=self._parent, + attachment_type=self._camera_transforms[self.transform_index][1]) + + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) + if notify: + self.hud.notification(self.sensors[index][2]) + self.index = index + + def next_sensor(self): + """Get the next sensor""" + self.set_sensor(self.index + 1) + + def toggle_recording(self): + """Toggle recording on or off""" + self.recording = not self.recording + self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + + def render(self, display): + """Render method""" + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self.sensors[self.index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 3), 3)) + lidar_data = np.array(points[:, :2]) + lidar_data *= min(self.hud.dim) / 100.0 + lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) + lidar_data = np.fabs(lidar_data) # pylint: disable=assignment-from-no-return + lidar_data = lidar_data.astype(np.int32) + lidar_data = np.reshape(lidar_data, (-1, 2)) + lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) + lidar_img = np.zeros(lidar_img_size) + lidar_img[tuple(lidar_data.T)] = (255, 255, 255) + self.surface = pygame.surfarray.make_surface(lidar_img) + else: + image.convert(self.sensors[self.index][1]) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + ######################################################################################################################################### +############################################################ yolov3 #################################################################### + def calculate_iou(box1, box2): + """ + Calculate the Intersection over Union (IoU) between two bounding boxes. + + Args: + box1 (list): List containing the coordinates of the first bounding box [x, y, w, h]. + box2 (list): List containing the coordinates of the second bounding box [x, y, w, h]. + + Returns: + float: The IoU value. + + """ + # Extract box coordinates + x1, y1, w1, h1 = box1 + x2, y2, w2, h2 = box2 + + # Calculate box coordinates + x_left = max(x1, x2) + y_top = max(y1, y2) + x_right = min(x1 + w1, x2 + w2) + y_bottom = min(y1 + h1, y2 + h2) + + # Calculate intersection area + intersection_area = max(0, x_right - x_left) * max(0, y_bottom - y_top) + + # Calculate union area + box1_area = w1 * h1 + box2_area = w2 * h2 + union_area = box1_area + box2_area - intersection_area + + # Calculate IoU + iou = intersection_area / union_area + + return iou + + + test_im = np.array(image.raw_data) + test_im = test_im.copy() + test_im = test_im.reshape((image.height, image.width, 4)) + test_im = test_im[:, :, :3] + + # Load YOLO + model_weights = 'yolov3.cfg' + model_cfg = 'yolov3.weights' + #frameWidth = 1200 + #frameHeight = 720 + frameWidth = 640 + frameHeight = 480 + net = cv2.dnn.readNet(model_weights, model_cfg) + + # Define your loss function + #loss_fn = tf.keras.losses.BinaryCrossentropy() + + classes = [] + with open("coco.names", "r") as f: + classes = [line.strip() for line in f.readlines()] + + layers_names = net.getLayerNames() + #output_layers = [layers_names[i[0] - 1] for i in net.getUnconnectedOutLayers()] + output_layers = [layers_names[i - 1] for i in net.getUnconnectedOutLayers()] + colors = np.random.uniform(0, 255, size=(len(classes), 3)) + + # Loading image + img = cv2.resize(test_im, (frameWidth, frameHeight), None) + height, width, channels = img.shape + img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + + # Detect objects + blob = cv2.dnn.blobFromImage(img, 1/255, (416, 416), (0, 0, 0), True, crop=False) + net.setInput(blob) + outs = net.forward(output_layers) + + # Perform object detection + class_ids = [] + confidences = [] + boxes = [] + + for out in outs: + for detection in out: + scores = detection[5:] + class_id = np.argmax(scores) + confidence = scores[class_id] + + if confidence > 0.5: + # Object detected + center_x = int(detection[0] * width) + center_y = int(detection[1] * height) + w = int(detection[2] * width) + h = int(detection[3] * height) + + # Rectangle coordinates + x = int(center_x - w / 2) + y = int(center_y - h / 2) + + boxes.append([x, y, w, h]) + confidences.append(float(confidence)) + class_ids.append(class_id) + + indexes = cv2.dnn.NMSBoxes(boxes, confidences, 0.5, 0.4) + font = cv2.FONT_HERSHEY_SIMPLEX + colors = np.random.uniform(0, 255, size=(len(boxes), 3)) + + detected_labels = [] + detected_boxes = [] + predicted_confidences = [] + predicted_class_probs = [] + + for i in range(len(boxes)): + if i in indexes: + x, y, w, h = boxes[i] + label = str(classes[class_ids[i]]) + confidence = str(round(confidences[i], 2)) + color = colors[i] + cv2.rectangle(img, (x, y), (x + w, y + h), color, 2) + cv2.putText(img, label + " " + confidence, (x + 60, y + 60), font, 0.8, (255, 255, 200), 1) + detected_labels.append(label) + detected_boxes.append([x, y, w, h]) + predicted_confidences.append(float(confidence)) + predicted_class_probs.append(label) + + # Ground truth annotations + ground_truth_labels = ['person', 'bicycle', 'car', 'traffic light', 'fire hydrant', 'pottedplant', 'stop sign', 'bench'] + ground_truth_boxes = [[250, 220, 160, 190], [200, 200, 80, 80], [200, 200, 80, 80], [450, 90, 20, 50], [200, 200, 80, 80], [200, 200, 80, 80], [200, 200, 80, 80], [200, 200, 80, 80]] # [x, y, w, h] format + + #def normalize_boxes(boxes, image_width, image_height): + # Convert box coordinates to numpy array + # boxes = np.array(boxes) + + # Extract box coordinates + #x, y, w, h = np.split(boxes, 4, axis=1) + + # Normalize box coordinates + #normalized_x = x / image_width + #normalized_y = y / image_height + #normalized_w = w / image_width + #normalized_h = h / image_height + + # Concatenate normalized coordinates + # normalized_boxes = np.concatenate((normalized_x, normalized_y, normalized_w, normalized_h), axis=1) + + #return normalized_boxes + ''' + def preprocess_ground_truth(ground_truth_labels, ground_truth_boxes): + # Define a dictionary to map class labels to class IDs + label_to_id = { + 'person': 0, + 'bicycle': 1, + 'car': 2, + 'traffic light': 3, + 'fire hydrant': 4, + 'pottedplant': 5, + 'stop sign': 6, + 'bench': 7 + } + ''' + + # Convert class labels to class IDs# + ''' + class_ids = [label_to_id[label] for label in ground_truth_labels] + + # Convert class IDs to one-hot encoding + num_classes = len(label_to_id) + one_hot_labels = tf.one_hot(class_ids, num_classes) + + # Normalize bounding box coordinates + normalized_boxes = normalize_boxes(ground_truth_boxes, width, height) # Implement 'normalize_boxes' function to scale box coordinates between 0 and 1 + + print('Normalized Boxes Shape:', normalized_boxes.shape) + print('One-Hot Labels Shape:', one_hot_labels.shape) + print('Normalized Boxes Data Type:', normalized_boxes.dtype) + print('One-Hot Labels Data Type:', one_hot_labels.dtype) + + if normalized_boxes.shape != one_hot_labels.shape: + print('Error: Shapes of normalized_boxes and one_hot_labels do not match.') + return None + + if normalized_boxes.dtype != one_hot_labels.dtype: + print('Error: Data types of normalized_boxes and one_hot_labels do not match.') + return None + + # Combine labels and boxes into a single tensor + ground_truth_targets = tf.concat([normalized_boxes, one_hot_labels], axis=-1) + + return ground_truth_targets, normalized_boxes, one_hot_labels, label_to_id + # Call the function and store the returned values + #ground_truth_targets, normalized_boxes, one_hot_labels, label_to_id = preprocess_ground_truth(ground_truth_labels, ground_truth_boxes) + + #num_classes = len(label_to_id) + ''' + + + # Perform object matching for ground truth and detected objects + TP = 0 # True positives + FP = 0 # False positives + FN = 0 # False negatives + threshold = 0.5 + print("Detected Labels: ", detected_labels) + print("Detected boxes: ", detected_boxes) + + for i, (gt_label, gt_box) in enumerate(zip(ground_truth_labels, ground_truth_boxes)): + max_iou = 0 + max_iou_index = -1 + + for j, (det_label, det_box) in enumerate(zip(detected_labels, detected_boxes)): + iou = calculate_iou(gt_box, det_box) + if iou > max_iou: + max_iou = iou + max_iou_index = j + + if max_iou >= threshold: + # Matched object found (True positive) + TP += 1 + # Remove the matched object from detected objects + if max_iou_index != -1: + detected_labels.pop(max_iou_index) + detected_boxes.pop(max_iou_index) + else: + # No match found (False negative) + FN += 1 + + # Any remaining unmatched detected objects are false positives + FP = len(detected_labels) + + eps = 1e-7 # Small value to avoid division by zero + + # Calculate performance metrics + accuracy = TP / (TP + FP + FN + eps) + precision = TP / (TP + FP + eps) + recall = TP / (TP + FN + eps) + matt_corr_coeff = (TP - (FP * FN)) / math.sqrt((TP + FP + eps) * (TP + FN + eps) * (FP + eps) * (FN + eps)) + + + # Calculate the loss + #ground_truth_targets = preprocess_ground_truth(ground_truth_labels, ground_truth_boxes) # Preprocess the ground truth targets + + # localization_loss = loss_fn(detected_boxes, ground_truth_targets) # Calculate the localization loss + # confidence_loss = loss_fn(predicted_confidences, ground_truth_targets) # Calculate the confidence loss + # class_prob_loss = loss_fn(predicted_class_probs, ground_truth_targets) # Calculate the class probability loss + + # Log the metrics to Tensorboard + log_dir = "./logs3" # Specify the log directory her + writer = SummaryWriter(log_dir=log_dir) + writer.add_scalar('Accuracy', accuracy, image.frame) + writer.add_scalar('Precision', precision, image.frame) + writer.add_scalar('Recall', recall, image.frame) + writer.add_scalar('IOU', max_iou, image.frame) + #writer.add_scalar('Matthews Correlation Coefficient', matt_corr_coeff) + #writer.add_scalar('Localization Loss', localization_loss, image.frame) + #writer.add_scalar('Confidence Loss', confidence_loss, image.frame) + #writer.add_scalar('Class Probability Loss', class_prob_loss, image.frame) + + # Show the annotated image + #cv2.imshow("Annotated Image", img) + #cv2.waitKey(0) + #cv2.destroyAllWindows() + + self.surface = pygame.surfarray.make_surface(img.swapaxes(0, 1)) + if self.recording: + image.save_to_disk('output/%08d' % image.frame) + + +# ============================================================================== +# -- KK PID Controllers --------------------------------------------------------- +# ============================================================================== + +class VehiclePIDController(): + """ + VehiclePIDController is the combination of two PID controllers + (lateral and longitudinal) to perform the + low level control a vehicle from client side + """ + + + def __init__(self, vehicle, args_lateral, args_longitudinal, max_throttle=0.75, max_brake=0.3, max_steering=0.8): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param args_lateral: dictionary of arguments to set the lateral PID controller + using the following semantics: + K_P -- Proportional term + K_D -- Differential term + K_I -- Integral term + :param args_longitudinal: dictionary of arguments to set the longitudinal + PID controller using the following semantics: + K_P -- Proportional term + K_D -- Differential term + K_I -- Integral term + """ + + self.max_brake = max_brake + self.max_throt = max_throttle + self.max_steer = max_steering + + self._vehicle = vehicle + self._world = self._vehicle.get_world() + self.past_steering = self._vehicle.get_control().steer + self._lon_controller = PIDLongitudinalController(self._vehicle, **args_longitudinal) + self._lat_controller = PIDLateralController(self._vehicle, **args_lateral) + + def run_step(self, target_speed, waypoint): + """ + Execute one step of control invoking both lateral and longitudinal + PID controllers to reach a target waypoint + at a given target_speed. + + :param target_speed: desired vehicle speed + :param waypoint: target location encoded as a waypoint + :return: distance (in meters) to the waypoint + """ + + acceleration = self._lon_controller.run_step(target_speed) + current_steering = self._lat_controller.run_step(waypoint) + control = carla.VehicleControl() + if acceleration >= 0.0: + control.throttle = min(acceleration, self.max_throt) + control.brake = 0.0 + else: + control.throttle = 0.0 + control.brake = min(abs(acceleration), self.max_brake) + + # Steering regulation: changes cannot happen abruptly, can't steer too much. + + if current_steering > self.past_steering + 0.1: + current_steering = self.past_steering + 0.1 + elif current_steering < self.past_steering - 0.1: + current_steering = self.past_steering - 0.1 + + if current_steering >= 0: + steering = min(self.max_steer, current_steering) + else: + steering = max(-self.max_steer, current_steering) + + control.steer = steering + control.hand_brake = False + control.manual_gear_shift = False + self.past_steering = steering + + return control + + +class PIDLongitudinalController(): + """ + PIDLongitudinalController implements longitudinal control using a PID. + """ + + + def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=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 = vehicle + self._k_p = K_P + self._k_d = K_D + self._k_i = K_I + self._dt = dt + self._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) / 2 + target_speed = target_speed / 2 + 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_speed + self._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._dt + else: + _de = 0.0 + _ie = 0.0 + + return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) + + +class PIDLateralController(): + """ + PIDLateralController implements lateral control using a PID. + """ + + def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=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 = vehicle + self._k_p = K_P + self._k_d = K_D + self._k_i = K_I + self._dt = dt + self._e_buffer = deque(maxlen=10) + + def run_step(self, waypoint): + """ + Execute one step of lateral control to steer + the 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] + """ + v_begin = vehicle_transform.location + v_end = v_begin + carla.Location(x=math.cos(math.radians(vehicle_transform.rotation.yaw)), + y=math.sin(math.radians(vehicle_transform.rotation.yaw))) + + v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0]) + w_vec = np.array([waypoint.transform.location.x - + v_begin.x, waypoint.transform.location.y - + v_begin.y, 0.0]) + _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / + (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0)) + + _cross = np.cross(v_vec, w_vec) + + if _cross[2] < 0: + _dot *= -1.0 + + self._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._dt + else: + _de = 0.0 + _ie = 0.0 + + return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) + + +# ============================================================================== +# -- KK Local Planner --------------------------------------------------------- +# ============================================================================== + +class RoadOption(Enum): + """ + RoadOption represents the possible topological configurations + when moving from a segment of lane to other. + """ + VOID = -1 + LEFT = 1 + RIGHT = 2 + STRAIGHT = 3 + LANEFOLLOW = 4 + CHANGELANELEFT = 5 + CHANGELANERIGHT = 6 + + +class LocalPlanner(object): + """ + LocalPlanner implements the basic behavior of following a trajectory + of waypoints that is generated on-the-fly. + The low-level motion of the vehicle is computed by using two PID controllers, + one is used for the lateral control + and the other for the longitudinal control (cruise speed). + + When multiple paths are available (intersections) + this local planner makes a random choice. + """ + + # Minimum distance to target waypoint as a percentage + # (e.g. within 80% of total distance) + + # FPS used for dt + FPS = 20 + + def __init__(self, agent): + """ + :param agent: agent that regulates the vehicle + :param vehicle: actor to apply to local planner logic onto + """ + self._vehicle = agent.vehicle + self._map = agent.vehicle.get_world().get_map() + + self._target_speed = None + self.sampling_radius = None + self._min_distance = None + self._current_waypoint = None + self.target_road_option = None + self._next_waypoints = None + self.target_waypoint = None + self._vehicle_controller = None + self._global_plan = None + self._pid_controller = None + self.waypoints_queue = deque(maxlen=20000) # queue with tuples of (waypoint, RoadOption) + self._buffer_size = 5 + self._waypoint_buffer = deque(maxlen=self._buffer_size) + + self._init_controller() # initializing controller + + def get_distance_to_nearest_obstacle(self): + ego_vehicle_transform = self._vehicle.get_transform() + ego_vehicle_location = ego_vehicle_transform.location + + # Get the bounding box of the ego vehicle + ego_vehicle_bbox = self._vehicle.bounding_box + + # Calculate the minimum distance to other vehicles + distance_to_nearest_obstacle = None + + for actor in self._vehicle.get_world().get_actors().filter('vehicle.*'): + if actor.id != self._vehicle.id: + actor_transform = actor.get_transform() + actor_location = actor_transform.location + + # Calculate the distance between the ego vehicle and the other vehicle + distance = ego_vehicle_location.distance(actor_location) + + # Calculate the minimum distance to the other vehicle's bounding box + distance_to_obstacle = distance - ego_vehicle_bbox.extent.x - actor.bounding_box.extent.x + + if distance_to_nearest_obstacle is None or distance_to_obstacle < distance_to_nearest_obstacle: + distance_to_nearest_obstacle = distance_to_obstacle + + + return distance_to_nearest_obstacle + + def reset_vehicle(self): + """Reset the ego-vehicle""" + self._vehicle = None + print("Resetting ego-vehicle!") + + def _init_controller(self): + """ + Controller initialization. + + dt -- time difference between physics control in seconds. + This is can be fixed from server side + using the arguments -benchmark -fps=F, since dt = 1/F + + target_speed -- desired cruise speed in km/h + + min_distance -- minimum distance to remove waypoint from queue + + lateral_dict -- dictionary of arguments to setup the lateral PID controller + {'K_P':, 'K_D':, 'K_I':, 'dt'} + + longitudinal_dict -- dictionary of arguments to setup the longitudinal PID controller + {'K_P':, 'K_D':, 'K_I':, 'dt'} + """ + # Default parameters + self.args_lat_hw_dict = { + 'K_P': 0.75, + 'K_D': 0.02, + 'K_I': 0.4, + 'dt': 1.0 / self.FPS} + self.args_lat_city_dict = { + 'K_P': 0.58, + 'K_D': 0.02, + 'K_I': 0.5, + 'dt': 1.0 / self.FPS} + self.args_long_hw_dict = { + 'K_P': 0.37, + 'K_D': 0.024, + 'K_I': 0.032, + 'dt': 1.0 / self.FPS} + self.args_long_city_dict = { + 'K_P': 0.15, + 'K_D': 0.05, + 'K_I': 0.07, + 'dt': 1.0 / self.FPS} + + self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) + + self._global_plan = False + + self._target_speed = self._vehicle.get_speed_limit() + + self._min_distance = 3 + + def set_speed(self, speed): + """ + Request new target speed. + + :param speed: new target speed in km/h + """ + + self._target_speed = speed + + def set_global_plan(self, current_plan): + """ + Sets new global plan. + + :param current_plan: list of waypoints in the actual plan + """ + for elem in current_plan: + self.waypoints_queue.append(elem) + self._global_plan = True + + def get_incoming_waypoint_and_direction(self, steps=3): + """ + Returns direction and waypoint at a distance ahead defined by the user. + + :param steps: number of steps to get the incoming waypoint. + """ + if len(self.waypoints_queue) > steps: + return self.waypoints_queue[steps] + + else: + try: + wpt, direction = self.waypoints_queue[-1] + return wpt, direction + except IndexError as i: + print(i) + return None, RoadOption.VOID + return None, RoadOption.VOID + + def run_step(self, target_speed=None, debug=False): + """ + Execute one step of local planning which involves + running the longitudinal and lateral PID controllers to + follow the waypoints trajectory. + + :param target_speed: desired speed + :param debug: boolean flag to activate waypoints debugging + :return: control + """ + + if target_speed is not None: + self._target_speed = target_speed + else: + self._target_speed = self._vehicle.get_speed_limit() + + if len(self.waypoints_queue) == 0: + control = carla.VehicleControl() + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 1.0 + control.hand_brake = False + control.manual_gear_shift = False + return control + + # Buffering the waypoints + if not self._waypoint_buffer: + for i in range(self._buffer_size): + if self.waypoints_queue: + self._waypoint_buffer.append( + self.waypoints_queue.popleft()) + else: + break + + # Current vehicle waypoint + self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) + + # Target waypoint + self.target_waypoint, self.target_road_option = self._waypoint_buffer[0] + + if target_speed > 50: + args_lat = self.args_lat_hw_dict + args_long = self.args_long_hw_dict + else: + args_lat = self.args_lat_city_dict + args_long = self.args_long_city_dict + + self._pid_controller = VehiclePIDController(self._vehicle, + args_lateral=args_lat, + args_longitudinal=args_long) + + control = self._pid_controller.run_step(self._target_speed, self.target_waypoint) + + # Purge the queue of obsolete waypoints + vehicle_transform = self._vehicle.get_transform() + max_index = -1 + + for i, (waypoint, _) in enumerate(self._waypoint_buffer): + if distance_vehicle( + waypoint, vehicle_transform) < self._min_distance: + max_index = i + if max_index >= 0: + for i in range(max_index + 1): + self._waypoint_buffer.popleft() + + if debug: + draw_waypoints(self._vehicle.get_world(), + [self.target_waypoint], 1.0) + return control + + +# ============================================================================== +# -- KK Agent Class --------------------------------------------------------- +# ============================================================================== + +class AgentState(Enum): + """ + AGENT_STATE represents the possible states of a roaming agent + """ + NAVIGATING = 1 + BLOCKED_BY_VEHICLE = 2 + BLOCKED_RED_LIGHT = 3 + + +class Agent(object): + """Base class to define agents in CARLA""" + + def __init__(self, vehicle): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + """ + self._vehicle = vehicle + self._proximity_tlight_threshold = 5.0 # meters + self._proximity_vehicle_threshold = 10.0 # meters + self._local_planner = None + self._world = self._vehicle.get_world() + try: + self._map = self._world.get_map() + except RuntimeError as error: + print('RuntimeError: {}'.format(error)) + print(' The server could not send the OpenDRIVE (.xodr) file:') + print(' Make sure it exists, has the same name of your town, and is correct.') + sys.exit(1) + self._last_traffic_light = None + + def get_local_planner(self): + """Get method for protected member local planner""" + return self._local_planner + + @staticmethod + def run_step(debug=False): + """ + Execute one step of navigation. + + :param debug: boolean flag for debugging + :return: control + """ + control = carla.VehicleControl() + + if debug: + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 0.0 + control.hand_brake = False + control.manual_gear_shift = False + + return control + + @staticmethod + def emergency_stop(): + """ + Send an emergency stop command to the vehicle + + :return: control for braking + """ + control = carla.VehicleControl() + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 1.0 + control.hand_brake = False + + return control + + +# ============================================================================== +# -- KK Behavior Agent Class --------------------------------------------------------- +# ============================================================================== + +class BehaviorAgent(Agent): + """ + BehaviorAgent implements an agent that navigates scenes to reach a given + target destination, by computing the shortest possible path to it. + This agent can correctly follow traffic signs, speed limitations, + traffic lights, while also taking into account nearby vehicles. Lane changing + decisions can be taken by analyzing the surrounding environment, + such as overtaking or tailgating avoidance. Adding to these are possible + behaviors, the agent can also keep safety distance from a car in front of it + by tracking the instantaneous time to collision and keeping it in a certain range. + Finally, different sets of behaviors are encoded in the agent, from cautious + to a more aggressive ones. + """ + + def __init__(self, vehicle, ignore_traffic_light=False, behavior='normal'): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param ignore_traffic_light: boolean to ignore any traffic light + :param behavior: type of agent to apply + """ + + super(BehaviorAgent, self).__init__(vehicle) + self.vehicle = vehicle + self.ignore_traffic_light = ignore_traffic_light + self._local_planner = LocalPlanner(self) + self._grp = None + self.look_ahead_steps = 0 + + # Vehicle information + self.speed = 0 + self.speed_limit = 0 + self.direction = None + self.incoming_direction = None + self.incoming_waypoint = None + self.start_waypoint = None + self.end_waypoint = None + self.is_at_traffic_light = 0 + self.light_state = "Green" + self.light_id_to_ignore = -1 + self.min_speed = 5 + self.behavior = None + self._sampling_resolution = 4.5 + + # Parameters for agent behavior + if behavior == 'cautious': + self.behavior = Cautious() + + elif behavior == 'normal': + self.behavior = Normal() + + elif behavior == 'aggressive': + self.behavior = Aggressive() + + def update_information(self, world): + """ + This method updates the information regarding the ego + vehicle based on the surrounding world. + + :param world: carla.world object + """ + self.speed = get_speed(self.vehicle) + self.speed_limit = world.player.get_speed_limit() + # self.speed_limit = 100 # KK set speed to 100 km/h + self._local_planner.set_speed(self.speed_limit) + self.direction = self._local_planner.target_road_option + if self.direction is None: + self.direction = RoadOption.LANEFOLLOW + + self.look_ahead_steps = int(self.speed_limit / 10) + + self.incoming_waypoint, self.incoming_direction = self._local_planner.get_incoming_waypoint_and_direction( + steps=self.look_ahead_steps) + if self.incoming_direction is None: + self.incoming_direction = RoadOption.LANEFOLLOW + + self.is_at_traffic_light = world.player.is_at_traffic_light() + if self.ignore_traffic_light: + self.light_state = "Green" + else: + if self.is_at_traffic_light: + traffic_light = world.player.get_traffic_light() + self.light_state = str(traffic_light.state) + if self.light_id_to_ignore != -1 and traffic_light.id == self.light_id_to_ignore: + # Ignore this traffic light for this iteration + self.light_state = "Green" + # Get distance to the nearest obstacle + distance_to_nearest_obstacle = self._local_planner.get_distance_to_nearest_obstacle() + + # Check if the obstacle is closer than the minimum distance + if distance_to_nearest_obstacle is not None and distance_to_nearest_obstacle < 3.0: + # Apply braking if the distance to the nearest obstacle is below the threshold + self._local_planner.brake = 1.0 + else: + self._local_planner.brake = 0.0 + #else: + # This method also includes stop signs and intersections. + #self.light_state = str(self.vehicle.get_traffic_light_state()) + + def set_destination(self, start_location, end_location, clean=False): + """ + This method creates a list of waypoints from agent's position to destination location + based on the route returned by the global router. + + :param start_location: initial position + :param end_location: final position + :param clean: boolean to clean the waypoint queue + """ + if clean: + self._local_planner.waypoints_queue.clear() + self.start_waypoint = self._map.get_waypoint(start_location) + self.end_waypoint = self._map.get_waypoint(end_location) + + route_trace = self._trace_route(self.start_waypoint, self.end_waypoint) + + self._local_planner.set_global_plan(route_trace) + + def reroute(self, spawn_points): + """ + This method implements re-routing for vehicles approaching its destination. + It finds a new target and computes another path to reach it. + + :param spawn_points: list of possible destinations for the agent + """ + + print("Target almost reached, setting new destination...") + random.shuffle(spawn_points) + new_start = self._local_planner.waypoints_queue[-1][0].transform.location + destination = spawn_points[0].location if spawn_points[0].location != new_start else spawn_points[1].location + print("New destination: " + str(destination)) + + self.set_destination(new_start, destination) + + def _trace_route(self, start_waypoint, end_waypoint): + """ + This method sets up a global router and returns the + optimal route from start_waypoint to end_waypoint. + + :param start_waypoint: initial position + :param end_waypoint: final position + """ + # Setting up global router + if self._grp is None: + wld = self.vehicle.get_world() + dao = GlobalRoutePlannerDAO( + wld.get_map(), sampling_resolution=self._sampling_resolution) + grp = GlobalRoutePlanner(dao) + grp.setup() + self._grp = grp + + # Obtain route plan + route = self._grp.trace_route( + start_waypoint.transform.location, + end_waypoint.transform.location) + + return route + + def run_step(self, debug=False): + """ + Execute one step of navigation. + + :param debug: boolean for debugging + :return control: carla.VehicleControl + """ + control = carla.VehicleControl() + + ego_vehicle_loc = self.vehicle.get_location() + ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) + + if self.is_at_traffic_light and self.light_state != "Green": + # Stop at red traffic light + control = carla.VehicleControl() + control.throttle = 0.0 + control.brake = 1.0 + control.steer = 0.0 + else: + # Get distance to the nearest obstacle + distance_to_nearest_obstacle = self._local_planner.get_distance_to_nearest_obstacle() + + # Check if the obstacle is closer than the minimum distance + if distance_to_nearest_obstacle is not None and distance_to_nearest_obstacle < 3.0: + # Apply braking if the distance to the nearest obstacle is below the threshold + control.throttle = 0.0 + control.brake = 1.0 + control.steer = 0.0 + else: + # Continue following the planned route + control = self._local_planner.run_step( + target_speed=min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist), debug=debug) + self.vehicle.apply_control(control) + + return control + + +# ============================================================================== +# -- Game Loop --------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + """ Main loop for agent""" + + pygame.init() + pygame.font.init() + world = None + tot_target_reached = 0 + num_min_waypoints = 21 + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(70.0) + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + + hud = HUD(args.width, args.height) + + #selected_world = client.load_world("Town01") # KK Changes world to Town04 + selected_world = client.get_world() + + world = World(client.get_world(), hud, args) + controller = KeyboardControl(world) + + agent = BehaviorAgent(world.player, behavior=args.behavior) + + spawn_points = world.map.get_spawn_points() + random.shuffle(spawn_points) + + if spawn_points[0].location != agent.vehicle.get_location(): + destination = spawn_points[0].location + else: + destination = spawn_points[1].location + + agent.set_destination(agent.vehicle.get_location(), destination, clean=True) + + clock = pygame.time.Clock() + + while True: + clock.tick_busy_loop(60) + if controller.parse_events(): + return + + # As soon as the server is ready continue! + if not world.world.wait_for_tick(10.0): + continue + + if args.agent == "Roaming" or args.agent == "Basic": + if controller.parse_events(): + return + + #agent.update_information(world) + # as soon as the server is ready continue! + world.world.wait_for_tick(10.0) + + world.tick(clock) + world.render(display) + pygame.display.flip() + control = agent.run_step() + control.manual_gear_shift = False + world.player.apply_control(control) + else: + agent.update_information(world) + + world.tick(clock) + world.render(display) + pygame.display.flip() + + + # Set new destination when target has been reached + if len(agent.get_local_planner().waypoints_queue) < num_min_waypoints and args.loop: + agent.reroute(spawn_points) + tot_target_reached += 1 + world.hud.notification("The target has been reached " + + str(tot_target_reached) + " times.", seconds=4.0) + + elif len(agent.get_local_planner().waypoints_queue) == 0 and not args.loop: + print("Target reached, mission accomplished...") + break + + speed_limit = world.player.get_speed_limit() + agent.get_local_planner().set_speed(speed_limit) + + control = agent.run_step() + world.player.apply_control(control) + + finally: + if world is not None: + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------- +# ============================================================================== + + +def main(): + """Main method""" + + argparser = argparse.ArgumentParser( + description='CARLA Automatic Control Client') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='Print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '--res', + metavar='WIDTHxHEIGHT', + default='1280x720', + help='Window resolution (default: 1280x720)') + argparser.add_argument( + '--filter', + metavar='PATTERN', + default='vehicle.*', + help='Actor filter (default: "vehicle.*")') + argparser.add_argument( + '--gamma', + default=2.2, + type=float, + help='Gamma correction of the camera (default: 2.2)') + argparser.add_argument( + '-l', '--loop', + action='store_true', + dest='loop', + help='Sets a new random destination upon reaching the previous one (default: False)') + argparser.add_argument( + '-b', '--behavior', type=str, + choices=["cautious", "normal", "aggressive"], + help='Choose one of the possible agent behaviors (default: normal) ', + default='normal') + argparser.add_argument("-a", "--agent", type=str, + choices=["Behavior", "Roaming", "Basic"], + help="select which agent to run", + default="Behavior") + argparser.add_argument( + '-s', '--seed', + help='Set seed for repeating executions (default: None)', + default=None, + type=int) + + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + try: + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + main() From e75612afca665fb3131bdff97431083706de709d Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Thu, 18 Dec 2025 09:01:02 +0800 Subject: [PATCH 11/13] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86=E5=9B=9B?= =?UTF-8?q?=E7=A7=8D=E6=91=84=E5=83=8F=E5=A4=B4=E7=94=A8=E6=9D=A5=E8=BF=BD?= =?UTF-8?q?=E8=B8=AA=E5=B0=8F=E8=BD=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../object_detection.py | 142 +++++++++++++++--- 1 file changed, 120 insertions(+), 22 deletions(-) diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py index 285fd97222..8133ac8dbb 100644 --- a/src/Object_Tracking_Planning/object_detection.py +++ b/src/Object_Tracking_Planning/object_detection.py @@ -2,6 +2,14 @@ from __future__ import print_function +import sys +import os +# 获取当前脚本所在的目录,并找到agents文件夹的路径 +current_dir = os.path.dirname(os.path.abspath(__file__)) +agents_path = os.path.join(current_dir, 'agents') +# 将该路径添加到模块搜索路径中 +if os.path.exists(agents_path): + sys.path.insert(0, agents_path) import argparse import collections import datetime @@ -23,6 +31,9 @@ from pygame.locals import KMOD_CTRL from pygame.locals import K_ESCAPE from pygame.locals import K_q + from pygame.locals import K_l + from pygame.locals import K_v + from pygame.locals import K_c except ImportError: raise RuntimeError('cannot import pygame, make sure pygame package is installed') @@ -162,7 +173,6 @@ def restart(self, args): self.camera_manager.set_sensor(cam_index, notify=False) actor_type = get_actor_display_name(self.player) self.hud.notification(actor_type) - def next_weather(self, reverse=False): """Get next weather setting""" self._weather_index += -1 if reverse else 1 @@ -205,16 +215,29 @@ def destroy(self): class KeyboardControl(object): - def __init__(self, world): + def __init__(self, world, start_locked=True): world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + # 可选:启动时自动锁定 + if start_locked and world.camera_manager is not None: + world.camera_manager.set_lock_mode(True) + world.camera_manager.hud.notification("Camera Lock: ON", seconds=2.0) - def parse_events(self): + def parse_events(self, world): for event in pygame.event.get(): if event.type == pygame.QUIT: return True if event.type == pygame.KEYUP: if self._is_quit_shortcut(event.key): return True + elif event.key == K_l: # 按L键切换锁定 + if world.camera_manager is not None: + world.camera_manager.toggle_lock() + elif event.key == K_v: # 按V键切换锁定视角 + if world.camera_manager is not None and world.camera_manager.lock_mode: + world.camera_manager.next_lock_view() + elif event.key == K_c: # 按C键切换相机(原有功能) + if world.camera_manager is not None: + world.camera_manager.toggle_camera() @staticmethod def _is_quit_shortcut(key): @@ -417,7 +440,28 @@ class HelpText(object): def __init__(self, font, width, height): """Constructor method""" - lines = __doc__.split('\n') + lines = [ + 'Welcome to CARLA automatic control client', + '', + 'Camera Control:', + ' C : Change camera position', + ' L : Toggle camera lock on vehicle', + ' V : Change lock view (when locked)', + ' PageUp : Toggle recording', + '', + 'Vehicle Control:', + ' W : Throttle', + ' S : Brake', + ' A/D : Steer left/right', + ' Q : Toggle reverse', + ' P : Toggle autopilot', + '', + 'Miscellaneous:', + ' H : Toggle help', + ' ESC : Quit', + ' CTRL + Q : Quit', + ] + self.font = font self.dim = (680, len(lines) * 22 + 12) self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) @@ -583,6 +627,24 @@ def __init__(self, parent_actor, hud, gamma_correction): ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 'Camera Semantic Segmentation (CityScapes Palette)'], ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] + + # 添加锁定模式相关属性 + self.lock_mode = True # 默认启用锁定模式 + self.lock_target = parent_actor # 锁定目标 + self.lock_transform_index = 0 # 锁定模式的相机位置索引 + + # 定义锁定模式的相机变换(可以自定义不同角度) + self._lock_transforms = [ + (carla.Transform( + carla.Location(x=-6.0, z=3.5), carla.Rotation(pitch=-10.0)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=2.0, z=1.5), carla.Rotation(pitch=0.0)), attachment.Rigid), + (carla.Transform( + carla.Location(x=5.0, y=2.0, z=2.0), carla.Rotation(pitch=-5.0)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=-10.0, z=5.0), carla.Rotation(pitch=-15.0)), attachment.SpringArm), + ] + world = self._parent.get_world() bp_library = world.get_blueprint_library() for item in self.sensors: @@ -611,18 +673,30 @@ def set_sensor(self, index, notify=True, force_respawn=False): if self.sensor is not None: self.sensor.destroy() self.surface = None + + # 选择变换和附着类型 + if self.lock_mode: + # 锁定模式使用锁定变换 + transform, attachment_type = self._lock_transforms[self.lock_transform_index] + else: + # 非锁定模式使用原有变换 + transform, attachment_type = self._camera_transforms[self.transform_index] + self.sensor = self._parent.get_world().spawn_actor( self.sensors[index][-1], - self._camera_transforms[self.transform_index][0], + transform, attach_to=self._parent, - attachment_type=self._camera_transforms[self.transform_index][1]) + attachment_type=attachment_type) # We need to pass the lambda a weak reference to # self to avoid circular reference. weak_self = weakref.ref(self) self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) if notify: - self.hud.notification(self.sensors[index][2]) + sensor_name = self.sensors[index][2] + if self.lock_mode: + sensor_name += " (Locked)" + self.hud.notification(sensor_name) self.index = index def next_sensor(self): @@ -634,6 +708,30 @@ def toggle_recording(self): self.recording = not self.recording self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + def toggle_lock(self): + """切换锁定模式""" + self.lock_mode = not self.lock_mode + mode_text = "Camera Lock: ON" if self.lock_mode else "Camera Lock: OFF" + self.hud.notification(mode_text, seconds=2.0) + # 重新设置传感器以应用锁定模式 + if self.sensor is not None: + self.set_sensor(self.index, notify=False, force_respawn=True) + return self.lock_mode + + def set_lock_mode(self, enable=True): + """设置锁定模式""" + self.lock_mode = enable + return self.lock_mode + + def next_lock_view(self): + """切换到下一个锁定视角""" + if self.lock_mode: + self.lock_transform_index = (self.lock_transform_index + 1) % len(self._lock_transforms) + self.hud.notification(f'Lock View {self.lock_transform_index + 1}', seconds=1.0) + # 重新设置传感器以应用新的锁定视角 + if self.sensor is not None: + self.set_sensor(self.index, notify=False, force_respawn=True) + def render(self, display): """Render method""" if self.surface is not None: @@ -663,7 +761,7 @@ def _parse_image(weak_self, image): array = np.reshape(array, (image.height, image.width, 4)) array = array[:, :, :3] array = array[:, :, ::-1] - + ######################################################################################################################################### ############################################################ yolov3 #################################################################### def calculate_iou(box1, box2): @@ -701,7 +799,7 @@ def calculate_iou(box1, box2): return iou - + test_im = np.array(image.raw_data) test_im = test_im.copy() test_im = test_im.reshape((image.height, image.width, 4)) @@ -715,7 +813,7 @@ def calculate_iou(box1, box2): frameWidth = 640 frameHeight = 480 net = cv2.dnn.readNet(model_weights, model_cfg) - + # Define your loss function #loss_fn = tf.keras.losses.BinaryCrossentropy() @@ -789,7 +887,7 @@ def calculate_iou(box1, box2): # Ground truth annotations ground_truth_labels = ['person', 'bicycle', 'car', 'traffic light', 'fire hydrant', 'pottedplant', 'stop sign', 'bench'] ground_truth_boxes = [[250, 220, 160, 190], [200, 200, 80, 80], [200, 200, 80, 80], [450, 90, 20, 50], [200, 200, 80, 80], [200, 200, 80, 80], [200, 200, 80, 80], [200, 200, 80, 80]] # [x, y, w, h] format - + #def normalize_boxes(boxes, image_width, image_height): # Convert box coordinates to numpy array # boxes = np.array(boxes) @@ -855,7 +953,7 @@ def preprocess_ground_truth(ground_truth_labels, ground_truth_boxes): #num_classes = len(label_to_id) ''' - + # Perform object matching for ground truth and detected objects TP = 0 # True positives @@ -888,7 +986,7 @@ def preprocess_ground_truth(ground_truth_labels, ground_truth_boxes): # Any remaining unmatched detected objects are false positives FP = len(detected_labels) - + eps = 1e-7 # Small value to avoid division by zero # Calculate performance metrics @@ -896,8 +994,8 @@ def preprocess_ground_truth(ground_truth_labels, ground_truth_boxes): precision = TP / (TP + FP + eps) recall = TP / (TP + FN + eps) matt_corr_coeff = (TP - (FP * FN)) / math.sqrt((TP + FP + eps) * (TP + FN + eps) * (FP + eps) * (FN + eps)) - - + + # Calculate the loss #ground_truth_targets = preprocess_ground_truth(ground_truth_labels, ground_truth_boxes) # Preprocess the ground truth targets @@ -1197,7 +1295,7 @@ def __init__(self, agent): self._waypoint_buffer = deque(maxlen=self._buffer_size) self._init_controller() # initializing controller - + def get_distance_to_nearest_obstacle(self): ego_vehicle_transform = self._vehicle.get_transform() ego_vehicle_location = ego_vehicle_transform.location @@ -1554,7 +1652,7 @@ def update_information(self, world): self.light_state = "Green" # Get distance to the nearest obstacle distance_to_nearest_obstacle = self._local_planner.get_distance_to_nearest_obstacle() - + # Check if the obstacle is closer than the minimum distance if distance_to_nearest_obstacle is not None and distance_to_nearest_obstacle < 3.0: # Apply braking if the distance to the nearest obstacle is below the threshold @@ -1634,7 +1732,7 @@ def run_step(self, debug=False): ego_vehicle_loc = self.vehicle.get_location() ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) - + if self.is_at_traffic_light and self.light_state != "Green": # Stop at red traffic light control = carla.VehicleControl() @@ -1706,7 +1804,7 @@ def game_loop(args): while True: clock.tick_busy_loop(60) - if controller.parse_events(): + if controller.parse_events(world): return # As soon as the server is ready continue! @@ -1714,7 +1812,7 @@ def game_loop(args): continue if args.agent == "Roaming" or args.agent == "Basic": - if controller.parse_events(): + if controller.parse_events(world): return #agent.update_information(world) @@ -1733,7 +1831,7 @@ def game_loop(args): world.tick(clock) world.render(display) pygame.display.flip() - + # Set new destination when target has been reached if len(agent.get_local_planner().waypoints_queue) < num_min_waypoints and args.loop: @@ -1839,4 +1937,4 @@ def main(): if __name__ == '__main__': - main() + main() \ No newline at end of file From aa63bb7e69abaf8cad9b41d88c039ad24d429cbd Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Thu, 18 Dec 2025 13:40:43 +0800 Subject: [PATCH 12/13] =?UTF-8?q?=E4=BF=AE=E5=A4=8Dobject=5Fdetection.py?= =?UTF-8?q?=EF=BC=8C=E6=81=A2=E5=A4=8D=E6=AD=A3=E7=A1=AE=E7=89=88=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../object_detection.py | 3881 +++++++++-------- 1 file changed, 1941 insertions(+), 1940 deletions(-) diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py index 8133ac8dbb..59c8ce8da7 100644 --- a/src/Object_Tracking_Planning/object_detection.py +++ b/src/Object_Tracking_Planning/object_detection.py @@ -1,1940 +1,1941 @@ -"""Example of automatic vehicle control from client side.""" - -from __future__ import print_function - -import sys -import os -# 获取当前脚本所在的目录,并找到agents文件夹的路径 -current_dir = os.path.dirname(os.path.abspath(__file__)) -agents_path = os.path.join(current_dir, 'agents') -# 将该路径添加到模块搜索路径中 -if os.path.exists(agents_path): - sys.path.insert(0, agents_path) -import argparse -import collections -import datetime -import glob -import logging -import math -import os -import cv2 -import random -import re -import sys -import weakref -from enum import Enum -from collections import deque - - -try: - import pygame - from pygame.locals import KMOD_CTRL - from pygame.locals import K_ESCAPE - from pygame.locals import K_q - from pygame.locals import K_l - from pygame.locals import K_v - from pygame.locals import K_c -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError( - 'cannot import numpy, make sure numpy package is installed') - -# ============================================================================== -# -- Find CARLA module --------------------------------------------------------- -# ============================================================================== -try: - sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( - sys.version_info.major, - sys.version_info.minor, - 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) -except IndexError: - pass - -# ============================================================================== -# -- Add PythonAPI for release mode -------------------------------------------- -# ============================================================================== -try: - sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + '/carla') -except IndexError: - pass - -import carla -from carla import ColorConverter as cc -from torch.utils.tensorboard import SummaryWriter -import torch -#import tensorflow as tf - -# Create a SummaryWriter object to write the metrics to Tensorboard -writer = SummaryWriter() - -from agents.navigation.global_route_planner import GlobalRoutePlanner -from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO -from agents.navigation.types_behavior import Cautious, Aggressive, Normal - -from agents.tools.misc import get_speed, positive, draw_waypoints, distance_vehicle - - -# ============================================================================== -# -- Global functions ---------------------------------------------------------- -# ============================================================================== - - -def find_weather_presets(): - """Method to find weather presets""" - rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') - def name(x): return ' '.join(m.group(0) for m in rgx.finditer(x)) - presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] - return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] - - -def get_actor_display_name(actor, truncate=250): - """Method to get actor display name""" - name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) - return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name - - -# ============================================================================== -# -- World --------------------------------------------------------------- -# ============================================================================== - -class World(object): - """ Class representing the surrounding environment """ - - def __init__(self, carla_world, hud, args): - """Constructor method""" - self.world = carla_world - try: - self.map = self.world.get_map() - except RuntimeError as error: - print('RuntimeError: {}'.format(error)) - print(' The server could not send the OpenDRIVE (.xodr) file:') - print(' Make sure it exists, has the same name of your town, and is correct.') - sys.exit(1) - self.hud = hud - self.player = None - self.collision_sensor = None - self.lane_invasion_sensor = None - self.gnss_sensor = None - self.camera_manager = None - self._weather_presets = find_weather_presets() - self._weather_index = 0 - self._actor_filter = args.filter - self._gamma = args.gamma - self.restart(args) - self.world.on_tick(hud.on_world_tick) - self.recording_enabled = False - self.recording_start = 0 - - def restart(self, args): - """Restart the world""" - # Keep same camera config if the camera manager exists. - cam_index = self.camera_manager.index if self.camera_manager is not None else 0 - cam_pos_id = self.camera_manager.transform_index if self.camera_manager is not None else 0 - # Set the seed if requested by user - if args.seed is not None: - random.seed(args.seed) - - # KK Get a vehicle blueprint - #blueprint = self.world.get_blueprint_library().filter('vehicle.lincoln.mkz2017')[0] # KK - blueprint = self.world.get_blueprint_library().filter('model3')[0] - blueprint.set_attribute('role_name', 'hero') - if blueprint.has_attribute('color'): - color = '236,102,17' # KK Packt logo color - blueprint.set_attribute('color', color) - # Spawn the player. - print("Spawning the player") - if self.player is not None: - spawn_point = self.player.get_transform() - spawn_point.location.z += 2.0 - spawn_point.rotation.roll = 0.0 - spawn_point.rotation.pitch = 0.0 - self.destroy() - self.player = self.world.try_spawn_actor(blueprint, spawn_point) - - while self.player is None: - if not self.map.get_spawn_points(): - print('There are no spawn points available in your map/town.') - print('Please add some Vehicle Spawn Point to your UE4 scene.') - sys.exit(1) - spawn_points = self.map.get_spawn_points() - spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() - #spawn_point = spawn_points[0] if spawn_points else carla.Transform() # KK - self.player = self.world.try_spawn_actor(blueprint, spawn_point) - # Set up the sensors. - self.collision_sensor = CollisionSensor(self.player, self.hud) - self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) - self.gnss_sensor = GnssSensor(self.player) - self.camera_manager = CameraManager(self.player, self.hud, self._gamma) - self.camera_manager.transform_index = cam_pos_id - self.camera_manager.set_sensor(cam_index, notify=False) - actor_type = get_actor_display_name(self.player) - self.hud.notification(actor_type) - def next_weather(self, reverse=False): - """Get next weather setting""" - self._weather_index += -1 if reverse else 1 - self._weather_index %= len(self._weather_presets) - preset = self._weather_presets[self._weather_index] - self.hud.notification('Weather: %s' % preset[1]) - self.player.get_world().set_weather(preset[0]) - - def tick(self, clock): - """Method for every tick""" - self.hud.tick(self, clock) - - def render(self, display): - """Render world""" - self.camera_manager.render(display) - self.hud.render(display) - - def destroy_sensors(self): - """Destroy sensors""" - self.camera_manager.sensor.destroy() - self.camera_manager.sensor = None - self.camera_manager.index = None - - def destroy(self): - """Destroys all actors""" - actors = [ - self.camera_manager.sensor, - self.collision_sensor.sensor, - self.lane_invasion_sensor.sensor, - self.gnss_sensor.sensor, - self.player] - for actor in actors: - if actor is not None: - actor.destroy() - - -# ============================================================================== -# -- KeyboardControl ----------------------------------------------------------- -# ============================================================================== - - -class KeyboardControl(object): - def __init__(self, world, start_locked=True): - world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) - # 可选:启动时自动锁定 - if start_locked and world.camera_manager is not None: - world.camera_manager.set_lock_mode(True) - world.camera_manager.hud.notification("Camera Lock: ON", seconds=2.0) - - def parse_events(self, world): - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return True - if event.type == pygame.KEYUP: - if self._is_quit_shortcut(event.key): - return True - elif event.key == K_l: # 按L键切换锁定 - if world.camera_manager is not None: - world.camera_manager.toggle_lock() - elif event.key == K_v: # 按V键切换锁定视角 - if world.camera_manager is not None and world.camera_manager.lock_mode: - world.camera_manager.next_lock_view() - elif event.key == K_c: # 按C键切换相机(原有功能) - if world.camera_manager is not None: - world.camera_manager.toggle_camera() - - @staticmethod - def _is_quit_shortcut(key): - """Shortcut for quitting""" - return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) - -# ============================================================================== -# -- HUD ----------------------------------------------------------------------- -# ============================================================================== - - -class HUD(object): - """Class for HUD text""" - - def __init__(self, width, height): - """Constructor method""" - self.dim = (width, height) - font = pygame.font.Font(pygame.font.get_default_font(), 20) - font_name = 'courier' if os.name == 'nt' else 'mono' - fonts = [x for x in pygame.font.get_fonts() if font_name in x] - default_font = 'ubuntumono' - mono = default_font if default_font in fonts else fonts[0] - mono = pygame.font.match_font(mono) - self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) - self._notifications = FadingText(font, (width, 40), (0, height - 40)) - self.help = HelpText(pygame.font.Font(mono, 24), width, height) - self.server_fps = 0 - self.frame = 0 - self.simulation_time = 0 - self._show_info = True - self._info_text = [] - self._server_clock = pygame.time.Clock() - - def on_world_tick(self, timestamp): - """Gets informations from the world at every tick""" - self._server_clock.tick() - self.server_fps = self._server_clock.get_fps() - self.frame = timestamp.frame_count - self.simulation_time = timestamp.elapsed_seconds - - def tick(self, world, clock): - """HUD method for every tick""" - self._notifications.tick(world, clock) - if not self._show_info: - return - transform = world.player.get_transform() - vel = world.player.get_velocity() - control = world.player.get_control() - heading = 'N' if abs(transform.rotation.yaw) < 89.5 else '' - heading += 'S' if abs(transform.rotation.yaw) > 90.5 else '' - heading += 'E' if 179.5 > transform.rotation.yaw > 0.5 else '' - heading += 'W' if -0.5 > transform.rotation.yaw > -179.5 else '' - colhist = world.collision_sensor.get_collision_history() - collision = [colhist[x + self.frame - 200] for x in range(0, 200)] - max_col = max(1.0, max(collision)) - collision = [x / max_col for x in collision] - vehicles = world.world.get_actors().filter('vehicle.*') - - self._info_text = [ - 'Server: % 16.0f FPS' % self.server_fps, - 'Client: % 16.0f FPS' % clock.get_fps(), - '', - 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), - 'Map: % 20s' % world.map.name, - 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), - '', - 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)), - u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (transform.rotation.yaw, heading), - 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (transform.location.x, transform.location.y)), - 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), - 'Height: % 18.0f m' % transform.location.z, - ''] - if isinstance(control, carla.VehicleControl): - self._info_text += [ - ('Throttle:', control.throttle, 0.0, 1.0), - ('Steer:', control.steer, -1.0, 1.0), - ('Brake:', control.brake, 0.0, 1.0), - ('Reverse:', control.reverse), - ('Hand brake:', control.hand_brake), - ('Manual:', control.manual_gear_shift), - 'Gear: %s' % {-1: 'R', 0: 'N'}.get(control.gear, control.gear)] - elif isinstance(control, carla.WalkerControl): - self._info_text += [ - ('Speed:', control.speed, 0.0, 5.556), - ('Jump:', control.jump)] - self._info_text += [ - '', - 'Collision:', - collision, - '', - 'Number of vehicles: % 8d' % len(vehicles)] - - if len(vehicles) > 1: - self._info_text += ['Nearby vehicles:'] - - def dist(l): - return math.sqrt((l.x - transform.location.x)**2 + (l.y - transform.location.y) - ** 2 + (l.z - transform.location.z)**2) - vehicles = [(dist(x.get_location()), x) for x in vehicles if x.id != world.player.id] - - for dist, vehicle in sorted(vehicles): - if dist > 200.0: - break - vehicle_type = get_actor_display_name(vehicle, truncate=22) - self._info_text.append('% 4dm %s' % (dist, vehicle_type)) - - def toggle_info(self): - """Toggle info on or off""" - self._show_info = not self._show_info - - def notification(self, text, seconds=2.0): - """Notification text""" - self._notifications.set_text(text, seconds=seconds) - - def error(self, text): - """Error text""" - self._notifications.set_text('Error: %s' % text, (255, 0, 0)) - - def render(self, display): - """Render for HUD class""" - if self._show_info: - info_surface = pygame.Surface((220, self.dim[1])) - info_surface.set_alpha(100) - display.blit(info_surface, (0, 0)) - v_offset = 4 - bar_h_offset = 100 - bar_width = 106 - for item in self._info_text: - if v_offset + 18 > self.dim[1]: - break - if isinstance(item, list): - if len(item) > 1: - points = [(x + 8, v_offset + 8 + (1 - y) * 30) for x, y in enumerate(item)] - pygame.draw.lines(display, (255, 136, 0), False, points, 2) - item = None - v_offset += 18 - elif isinstance(item, tuple): - if isinstance(item[1], bool): - rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) - pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) - else: - rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) - pygame.draw.rect(display, (255, 255, 255), rect_border, 1) - fig = (item[1] - item[2]) / (item[3] - item[2]) - if item[2] < 0.0: - rect = pygame.Rect( - (bar_h_offset + fig * (bar_width - 6), v_offset + 8), (6, 6)) - else: - rect = pygame.Rect((bar_h_offset, v_offset + 8), (fig * bar_width, 6)) - pygame.draw.rect(display, (255, 255, 255), rect) - item = item[0] - if item: # At this point has to be a str. - surface = self._font_mono.render(item, True, (255, 255, 255)) - display.blit(surface, (8, v_offset)) - v_offset += 18 - self._notifications.render(display) - self.help.render(display) - -# ============================================================================== -# -- FadingText ---------------------------------------------------------------- -# ============================================================================== - - -class FadingText(object): - """ Class for fading text """ - - def __init__(self, font, dim, pos): - """Constructor method""" - self.font = font - self.dim = dim - self.pos = pos - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - - def set_text(self, text, color=(255, 255, 255), seconds=2.0): - """Set fading text""" - text_texture = self.font.render(text, True, color) - self.surface = pygame.Surface(self.dim) - self.seconds_left = seconds - self.surface.fill((0, 0, 0, 0)) - self.surface.blit(text_texture, (10, 11)) - - def tick(self, _, clock): - """Fading text method for every tick""" - delta_seconds = 1e-3 * clock.get_time() - self.seconds_left = max(0.0, self.seconds_left - delta_seconds) - self.surface.set_alpha(500.0 * self.seconds_left) - - def render(self, display): - """Render fading text method""" - display.blit(self.surface, self.pos) - -# ============================================================================== -# -- HelpText ------------------------------------------------------------------ -# ============================================================================== - - -class HelpText(object): - """ Helper class for text render""" - - def __init__(self, font, width, height): - """Constructor method""" - lines = [ - 'Welcome to CARLA automatic control client', - '', - 'Camera Control:', - ' C : Change camera position', - ' L : Toggle camera lock on vehicle', - ' V : Change lock view (when locked)', - ' PageUp : Toggle recording', - '', - 'Vehicle Control:', - ' W : Throttle', - ' S : Brake', - ' A/D : Steer left/right', - ' Q : Toggle reverse', - ' P : Toggle autopilot', - '', - 'Miscellaneous:', - ' H : Toggle help', - ' ESC : Quit', - ' CTRL + Q : Quit', - ] - - self.font = font - self.dim = (680, len(lines) * 22 + 12) - self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - self.surface.fill((0, 0, 0, 0)) - for i, line in enumerate(lines): - text_texture = self.font.render(line, True, (255, 255, 255)) - self.surface.blit(text_texture, (22, i * 22)) - self._render = False - self.surface.set_alpha(220) - - def toggle(self): - """Toggle on or off the render help""" - self._render = not self._render - - def render(self, display): - """Render help text method""" - if self._render: - display.blit(self.surface, self.pos) - -# ============================================================================== -# -- CollisionSensor ----------------------------------------------------------- -# ============================================================================== - - -class CollisionSensor(object): - """ Class for collision sensors""" - - def __init__(self, parent_actor, hud): - """Constructor method""" - self.sensor = None - self.history = [] - self._parent = parent_actor - self.hud = hud - world = self._parent.get_world() - blueprint = world.get_blueprint_library().find('sensor.other.collision') - self.sensor = world.spawn_actor(blueprint, carla.Transform(), attach_to=self._parent) - # We need to pass the lambda a weak reference to - # self to avoid circular reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) - - def get_collision_history(self): - """Gets the history of collisions""" - history = collections.defaultdict(int) - for frame, intensity in self.history: - history[frame] += intensity - return history - - @staticmethod - def _on_collision(weak_self, event): - """On collision method""" - self = weak_self() - if not self: - return - actor_type = get_actor_display_name(event.other_actor) - self.hud.notification('Collision with %r' % actor_type) - impulse = event.normal_impulse - intensity = math.sqrt(impulse.x ** 2 + impulse.y ** 2 + impulse.z ** 2) - self.history.append((event.frame, intensity)) - if len(self.history) > 4000: - self.history.pop(0) - -# ============================================================================== -# -- LaneInvasionSensor -------------------------------------------------------- -# ============================================================================== - - -class LaneInvasionSensor(object): - """Class for lane invasion sensors""" - - def __init__(self, parent_actor, hud): - """Constructor method""" - self.sensor = None - self._parent = parent_actor - self.hud = hud - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.lane_invasion') - self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) - - @staticmethod - def _on_invasion(weak_self, event): - """On invasion method""" - self = weak_self() - if not self: - return - lane_types = set(x.type for x in event.crossed_lane_markings) - text = ['%r' % str(x).split()[-1] for x in lane_types] - self.hud.notification('Crossed line %s' % ' and '.join(text)) - -# ============================================================================== -# -- GnssSensor -------------------------------------------------------- -# ============================================================================== - - -class GnssSensor(object): - """ Class for GNSS sensors""" - - def __init__(self, parent_actor): - """Constructor method""" - self.sensor = None - self._parent = parent_actor - self.lat = 0.0 - self.lon = 0.0 - world = self._parent.get_world() - blueprint = world.get_blueprint_library().find('sensor.other.gnss') - self.sensor = world.spawn_actor(blueprint, carla.Transform(carla.Location(x=1.0, z=2.8)), - attach_to=self._parent) - # We need to pass the lambda a weak reference to - # self to avoid circular reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) - - @staticmethod - def _on_gnss_event(weak_self, event): - """GNSS method""" - self = weak_self() - if not self: - return - self.lat = event.latitude - self.lon = event.longitude - -# ============================================================================== -# -- CameraManager ------------------------------------------------------------- -# ============================================================================== - - -class CameraManager(object): - """ Class for camera management""" - - def __init__(self, parent_actor, hud, gamma_correction): - """Constructor method""" - self.sensor = None - self.surface = None - self._parent = parent_actor - self.hud = hud - self.recording = False - bound_y = 0.5 + self._parent.bounding_box.extent.y - attachment = carla.AttachmentType - self._camera_transforms = [ - (carla.Transform( - carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8.0)), attachment.SpringArm), - (carla.Transform( - carla.Location(x=1.6, z=1.7)), attachment.Rigid), - (carla.Transform( - carla.Location(x=5.5, y=1.5, z=1.5)), attachment.SpringArm), - (carla.Transform( - carla.Location(x=-8.0, z=6.0), carla.Rotation(pitch=6.0)), attachment.SpringArm), - (carla.Transform( - carla.Location(x=-1, y=-bound_y, z=0.5)), attachment.Rigid)] - self.transform_index = 1 - self.sensors = [ - ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], - ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], - ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], - ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], - ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], - ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, - 'Camera Semantic Segmentation (CityScapes Palette)'], - ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] - - # 添加锁定模式相关属性 - self.lock_mode = True # 默认启用锁定模式 - self.lock_target = parent_actor # 锁定目标 - self.lock_transform_index = 0 # 锁定模式的相机位置索引 - - # 定义锁定模式的相机变换(可以自定义不同角度) - self._lock_transforms = [ - (carla.Transform( - carla.Location(x=-6.0, z=3.5), carla.Rotation(pitch=-10.0)), attachment.SpringArm), - (carla.Transform( - carla.Location(x=2.0, z=1.5), carla.Rotation(pitch=0.0)), attachment.Rigid), - (carla.Transform( - carla.Location(x=5.0, y=2.0, z=2.0), carla.Rotation(pitch=-5.0)), attachment.SpringArm), - (carla.Transform( - carla.Location(x=-10.0, z=5.0), carla.Rotation(pitch=-15.0)), attachment.SpringArm), - ] - - world = self._parent.get_world() - bp_library = world.get_blueprint_library() - for item in self.sensors: - blp = bp_library.find(item[0]) - if item[0].startswith('sensor.camera'): - blp.set_attribute('image_size_x', str(hud.dim[0])) - blp.set_attribute('image_size_y', str(hud.dim[1])) - if blp.has_attribute('gamma'): - blp.set_attribute('gamma', str(gamma_correction)) - elif item[0].startswith('sensor.lidar'): - blp.set_attribute('range', '50') - item.append(blp) - self.index = None - - def toggle_camera(self): - """Activate a camera""" - self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) - self.set_sensor(self.index, notify=False, force_respawn=True) - - def set_sensor(self, index, notify=True, force_respawn=False): - """Set a sensor""" - index = index % len(self.sensors) - needs_respawn = True if self.index is None else ( - force_respawn or (self.sensors[index][0] != self.sensors[self.index][0])) - if needs_respawn: - if self.sensor is not None: - self.sensor.destroy() - self.surface = None - - # 选择变换和附着类型 - if self.lock_mode: - # 锁定模式使用锁定变换 - transform, attachment_type = self._lock_transforms[self.lock_transform_index] - else: - # 非锁定模式使用原有变换 - transform, attachment_type = self._camera_transforms[self.transform_index] - - self.sensor = self._parent.get_world().spawn_actor( - self.sensors[index][-1], - transform, - attach_to=self._parent, - attachment_type=attachment_type) - - # We need to pass the lambda a weak reference to - # self to avoid circular reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) - if notify: - sensor_name = self.sensors[index][2] - if self.lock_mode: - sensor_name += " (Locked)" - self.hud.notification(sensor_name) - self.index = index - - def next_sensor(self): - """Get the next sensor""" - self.set_sensor(self.index + 1) - - def toggle_recording(self): - """Toggle recording on or off""" - self.recording = not self.recording - self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) - - def toggle_lock(self): - """切换锁定模式""" - self.lock_mode = not self.lock_mode - mode_text = "Camera Lock: ON" if self.lock_mode else "Camera Lock: OFF" - self.hud.notification(mode_text, seconds=2.0) - # 重新设置传感器以应用锁定模式 - if self.sensor is not None: - self.set_sensor(self.index, notify=False, force_respawn=True) - return self.lock_mode - - def set_lock_mode(self, enable=True): - """设置锁定模式""" - self.lock_mode = enable - return self.lock_mode - - def next_lock_view(self): - """切换到下一个锁定视角""" - if self.lock_mode: - self.lock_transform_index = (self.lock_transform_index + 1) % len(self._lock_transforms) - self.hud.notification(f'Lock View {self.lock_transform_index + 1}', seconds=1.0) - # 重新设置传感器以应用新的锁定视角 - if self.sensor is not None: - self.set_sensor(self.index, notify=False, force_respawn=True) - - def render(self, display): - """Render method""" - if self.surface is not None: - display.blit(self.surface, (0, 0)) - - @staticmethod - def _parse_image(weak_self, image): - self = weak_self() - if not self: - return - if self.sensors[self.index][0].startswith('sensor.lidar'): - points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) - points = np.reshape(points, (int(points.shape[0] / 3), 3)) - lidar_data = np.array(points[:, :2]) - lidar_data *= min(self.hud.dim) / 100.0 - lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) - lidar_data = np.fabs(lidar_data) # pylint: disable=assignment-from-no-return - lidar_data = lidar_data.astype(np.int32) - lidar_data = np.reshape(lidar_data, (-1, 2)) - lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) - lidar_img = np.zeros(lidar_img_size) - lidar_img[tuple(lidar_data.T)] = (255, 255, 255) - self.surface = pygame.surfarray.make_surface(lidar_img) - else: - image.convert(self.sensors[self.index][1]) - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - array = array[:, :, :3] - array = array[:, :, ::-1] - - ######################################################################################################################################### -############################################################ yolov3 #################################################################### - def calculate_iou(box1, box2): - """ - Calculate the Intersection over Union (IoU) between two bounding boxes. - - Args: - box1 (list): List containing the coordinates of the first bounding box [x, y, w, h]. - box2 (list): List containing the coordinates of the second bounding box [x, y, w, h]. - - Returns: - float: The IoU value. - - """ - # Extract box coordinates - x1, y1, w1, h1 = box1 - x2, y2, w2, h2 = box2 - - # Calculate box coordinates - x_left = max(x1, x2) - y_top = max(y1, y2) - x_right = min(x1 + w1, x2 + w2) - y_bottom = min(y1 + h1, y2 + h2) - - # Calculate intersection area - intersection_area = max(0, x_right - x_left) * max(0, y_bottom - y_top) - - # Calculate union area - box1_area = w1 * h1 - box2_area = w2 * h2 - union_area = box1_area + box2_area - intersection_area - - # Calculate IoU - iou = intersection_area / union_area - - return iou - - - test_im = np.array(image.raw_data) - test_im = test_im.copy() - test_im = test_im.reshape((image.height, image.width, 4)) - test_im = test_im[:, :, :3] - - # Load YOLO - model_weights = 'yolov3.cfg' - model_cfg = 'yolov3.weights' - #frameWidth = 1200 - #frameHeight = 720 - frameWidth = 640 - frameHeight = 480 - net = cv2.dnn.readNet(model_weights, model_cfg) - - # Define your loss function - #loss_fn = tf.keras.losses.BinaryCrossentropy() - - classes = [] - with open("coco.names", "r") as f: - classes = [line.strip() for line in f.readlines()] - - layers_names = net.getLayerNames() - #output_layers = [layers_names[i[0] - 1] for i in net.getUnconnectedOutLayers()] - output_layers = [layers_names[i - 1] for i in net.getUnconnectedOutLayers()] - colors = np.random.uniform(0, 255, size=(len(classes), 3)) - - # Loading image - img = cv2.resize(test_im, (frameWidth, frameHeight), None) - height, width, channels = img.shape - img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) - - # Detect objects - blob = cv2.dnn.blobFromImage(img, 1/255, (416, 416), (0, 0, 0), True, crop=False) - net.setInput(blob) - outs = net.forward(output_layers) - - # Perform object detection - class_ids = [] - confidences = [] - boxes = [] - - for out in outs: - for detection in out: - scores = detection[5:] - class_id = np.argmax(scores) - confidence = scores[class_id] - - if confidence > 0.5: - # Object detected - center_x = int(detection[0] * width) - center_y = int(detection[1] * height) - w = int(detection[2] * width) - h = int(detection[3] * height) - - # Rectangle coordinates - x = int(center_x - w / 2) - y = int(center_y - h / 2) - - boxes.append([x, y, w, h]) - confidences.append(float(confidence)) - class_ids.append(class_id) - - indexes = cv2.dnn.NMSBoxes(boxes, confidences, 0.5, 0.4) - font = cv2.FONT_HERSHEY_SIMPLEX - colors = np.random.uniform(0, 255, size=(len(boxes), 3)) - - detected_labels = [] - detected_boxes = [] - predicted_confidences = [] - predicted_class_probs = [] - - for i in range(len(boxes)): - if i in indexes: - x, y, w, h = boxes[i] - label = str(classes[class_ids[i]]) - confidence = str(round(confidences[i], 2)) - color = colors[i] - cv2.rectangle(img, (x, y), (x + w, y + h), color, 2) - cv2.putText(img, label + " " + confidence, (x + 60, y + 60), font, 0.8, (255, 255, 200), 1) - detected_labels.append(label) - detected_boxes.append([x, y, w, h]) - predicted_confidences.append(float(confidence)) - predicted_class_probs.append(label) - - # Ground truth annotations - ground_truth_labels = ['person', 'bicycle', 'car', 'traffic light', 'fire hydrant', 'pottedplant', 'stop sign', 'bench'] - ground_truth_boxes = [[250, 220, 160, 190], [200, 200, 80, 80], [200, 200, 80, 80], [450, 90, 20, 50], [200, 200, 80, 80], [200, 200, 80, 80], [200, 200, 80, 80], [200, 200, 80, 80]] # [x, y, w, h] format - - #def normalize_boxes(boxes, image_width, image_height): - # Convert box coordinates to numpy array - # boxes = np.array(boxes) - - # Extract box coordinates - #x, y, w, h = np.split(boxes, 4, axis=1) - - # Normalize box coordinates - #normalized_x = x / image_width - #normalized_y = y / image_height - #normalized_w = w / image_width - #normalized_h = h / image_height - - # Concatenate normalized coordinates - # normalized_boxes = np.concatenate((normalized_x, normalized_y, normalized_w, normalized_h), axis=1) - - #return normalized_boxes - ''' - def preprocess_ground_truth(ground_truth_labels, ground_truth_boxes): - # Define a dictionary to map class labels to class IDs - label_to_id = { - 'person': 0, - 'bicycle': 1, - 'car': 2, - 'traffic light': 3, - 'fire hydrant': 4, - 'pottedplant': 5, - 'stop sign': 6, - 'bench': 7 - } - ''' - - # Convert class labels to class IDs# - ''' - class_ids = [label_to_id[label] for label in ground_truth_labels] - - # Convert class IDs to one-hot encoding - num_classes = len(label_to_id) - one_hot_labels = tf.one_hot(class_ids, num_classes) - - # Normalize bounding box coordinates - normalized_boxes = normalize_boxes(ground_truth_boxes, width, height) # Implement 'normalize_boxes' function to scale box coordinates between 0 and 1 - - print('Normalized Boxes Shape:', normalized_boxes.shape) - print('One-Hot Labels Shape:', one_hot_labels.shape) - print('Normalized Boxes Data Type:', normalized_boxes.dtype) - print('One-Hot Labels Data Type:', one_hot_labels.dtype) - - if normalized_boxes.shape != one_hot_labels.shape: - print('Error: Shapes of normalized_boxes and one_hot_labels do not match.') - return None - - if normalized_boxes.dtype != one_hot_labels.dtype: - print('Error: Data types of normalized_boxes and one_hot_labels do not match.') - return None - - # Combine labels and boxes into a single tensor - ground_truth_targets = tf.concat([normalized_boxes, one_hot_labels], axis=-1) - - return ground_truth_targets, normalized_boxes, one_hot_labels, label_to_id - # Call the function and store the returned values - #ground_truth_targets, normalized_boxes, one_hot_labels, label_to_id = preprocess_ground_truth(ground_truth_labels, ground_truth_boxes) - - #num_classes = len(label_to_id) - ''' - - - # Perform object matching for ground truth and detected objects - TP = 0 # True positives - FP = 0 # False positives - FN = 0 # False negatives - threshold = 0.5 - print("Detected Labels: ", detected_labels) - print("Detected boxes: ", detected_boxes) - - for i, (gt_label, gt_box) in enumerate(zip(ground_truth_labels, ground_truth_boxes)): - max_iou = 0 - max_iou_index = -1 - - for j, (det_label, det_box) in enumerate(zip(detected_labels, detected_boxes)): - iou = calculate_iou(gt_box, det_box) - if iou > max_iou: - max_iou = iou - max_iou_index = j - - if max_iou >= threshold: - # Matched object found (True positive) - TP += 1 - # Remove the matched object from detected objects - if max_iou_index != -1: - detected_labels.pop(max_iou_index) - detected_boxes.pop(max_iou_index) - else: - # No match found (False negative) - FN += 1 - - # Any remaining unmatched detected objects are false positives - FP = len(detected_labels) - - eps = 1e-7 # Small value to avoid division by zero - - # Calculate performance metrics - accuracy = TP / (TP + FP + FN + eps) - precision = TP / (TP + FP + eps) - recall = TP / (TP + FN + eps) - matt_corr_coeff = (TP - (FP * FN)) / math.sqrt((TP + FP + eps) * (TP + FN + eps) * (FP + eps) * (FN + eps)) - - - # Calculate the loss - #ground_truth_targets = preprocess_ground_truth(ground_truth_labels, ground_truth_boxes) # Preprocess the ground truth targets - - # localization_loss = loss_fn(detected_boxes, ground_truth_targets) # Calculate the localization loss - # confidence_loss = loss_fn(predicted_confidences, ground_truth_targets) # Calculate the confidence loss - # class_prob_loss = loss_fn(predicted_class_probs, ground_truth_targets) # Calculate the class probability loss - - # Log the metrics to Tensorboard - log_dir = "./logs3" # Specify the log directory her - writer = SummaryWriter(log_dir=log_dir) - writer.add_scalar('Accuracy', accuracy, image.frame) - writer.add_scalar('Precision', precision, image.frame) - writer.add_scalar('Recall', recall, image.frame) - writer.add_scalar('IOU', max_iou, image.frame) - #writer.add_scalar('Matthews Correlation Coefficient', matt_corr_coeff) - #writer.add_scalar('Localization Loss', localization_loss, image.frame) - #writer.add_scalar('Confidence Loss', confidence_loss, image.frame) - #writer.add_scalar('Class Probability Loss', class_prob_loss, image.frame) - - # Show the annotated image - #cv2.imshow("Annotated Image", img) - #cv2.waitKey(0) - #cv2.destroyAllWindows() - - self.surface = pygame.surfarray.make_surface(img.swapaxes(0, 1)) - if self.recording: - image.save_to_disk('output/%08d' % image.frame) - - -# ============================================================================== -# -- KK PID Controllers --------------------------------------------------------- -# ============================================================================== - -class VehiclePIDController(): - """ - VehiclePIDController is the combination of two PID controllers - (lateral and longitudinal) to perform the - low level control a vehicle from client side - """ - - - def __init__(self, vehicle, args_lateral, args_longitudinal, max_throttle=0.75, max_brake=0.3, max_steering=0.8): - """ - Constructor method. - - :param vehicle: actor to apply to local planner logic onto - :param args_lateral: dictionary of arguments to set the lateral PID controller - using the following semantics: - K_P -- Proportional term - K_D -- Differential term - K_I -- Integral term - :param args_longitudinal: dictionary of arguments to set the longitudinal - PID controller using the following semantics: - K_P -- Proportional term - K_D -- Differential term - K_I -- Integral term - """ - - self.max_brake = max_brake - self.max_throt = max_throttle - self.max_steer = max_steering - - self._vehicle = vehicle - self._world = self._vehicle.get_world() - self.past_steering = self._vehicle.get_control().steer - self._lon_controller = PIDLongitudinalController(self._vehicle, **args_longitudinal) - self._lat_controller = PIDLateralController(self._vehicle, **args_lateral) - - def run_step(self, target_speed, waypoint): - """ - Execute one step of control invoking both lateral and longitudinal - PID controllers to reach a target waypoint - at a given target_speed. - - :param target_speed: desired vehicle speed - :param waypoint: target location encoded as a waypoint - :return: distance (in meters) to the waypoint - """ - - acceleration = self._lon_controller.run_step(target_speed) - current_steering = self._lat_controller.run_step(waypoint) - control = carla.VehicleControl() - if acceleration >= 0.0: - control.throttle = min(acceleration, self.max_throt) - control.brake = 0.0 - else: - control.throttle = 0.0 - control.brake = min(abs(acceleration), self.max_brake) - - # Steering regulation: changes cannot happen abruptly, can't steer too much. - - if current_steering > self.past_steering + 0.1: - current_steering = self.past_steering + 0.1 - elif current_steering < self.past_steering - 0.1: - current_steering = self.past_steering - 0.1 - - if current_steering >= 0: - steering = min(self.max_steer, current_steering) - else: - steering = max(-self.max_steer, current_steering) - - control.steer = steering - control.hand_brake = False - control.manual_gear_shift = False - self.past_steering = steering - - return control - - -class PIDLongitudinalController(): - """ - PIDLongitudinalController implements longitudinal control using a PID. - """ - - - def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=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 = vehicle - self._k_p = K_P - self._k_d = K_D - self._k_i = K_I - self._dt = dt - self._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) / 2 - target_speed = target_speed / 2 - 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_speed - self._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._dt - else: - _de = 0.0 - _ie = 0.0 - - return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) - - -class PIDLateralController(): - """ - PIDLateralController implements lateral control using a PID. - """ - - def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=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 = vehicle - self._k_p = K_P - self._k_d = K_D - self._k_i = K_I - self._dt = dt - self._e_buffer = deque(maxlen=10) - - def run_step(self, waypoint): - """ - Execute one step of lateral control to steer - the 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] - """ - v_begin = vehicle_transform.location - v_end = v_begin + carla.Location(x=math.cos(math.radians(vehicle_transform.rotation.yaw)), - y=math.sin(math.radians(vehicle_transform.rotation.yaw))) - - v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0]) - w_vec = np.array([waypoint.transform.location.x - - v_begin.x, waypoint.transform.location.y - - v_begin.y, 0.0]) - _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / - (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0)) - - _cross = np.cross(v_vec, w_vec) - - if _cross[2] < 0: - _dot *= -1.0 - - self._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._dt - else: - _de = 0.0 - _ie = 0.0 - - return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) - - -# ============================================================================== -# -- KK Local Planner --------------------------------------------------------- -# ============================================================================== - -class RoadOption(Enum): - """ - RoadOption represents the possible topological configurations - when moving from a segment of lane to other. - """ - VOID = -1 - LEFT = 1 - RIGHT = 2 - STRAIGHT = 3 - LANEFOLLOW = 4 - CHANGELANELEFT = 5 - CHANGELANERIGHT = 6 - - -class LocalPlanner(object): - """ - LocalPlanner implements the basic behavior of following a trajectory - of waypoints that is generated on-the-fly. - The low-level motion of the vehicle is computed by using two PID controllers, - one is used for the lateral control - and the other for the longitudinal control (cruise speed). - - When multiple paths are available (intersections) - this local planner makes a random choice. - """ - - # Minimum distance to target waypoint as a percentage - # (e.g. within 80% of total distance) - - # FPS used for dt - FPS = 20 - - def __init__(self, agent): - """ - :param agent: agent that regulates the vehicle - :param vehicle: actor to apply to local planner logic onto - """ - self._vehicle = agent.vehicle - self._map = agent.vehicle.get_world().get_map() - - self._target_speed = None - self.sampling_radius = None - self._min_distance = None - self._current_waypoint = None - self.target_road_option = None - self._next_waypoints = None - self.target_waypoint = None - self._vehicle_controller = None - self._global_plan = None - self._pid_controller = None - self.waypoints_queue = deque(maxlen=20000) # queue with tuples of (waypoint, RoadOption) - self._buffer_size = 5 - self._waypoint_buffer = deque(maxlen=self._buffer_size) - - self._init_controller() # initializing controller - - def get_distance_to_nearest_obstacle(self): - ego_vehicle_transform = self._vehicle.get_transform() - ego_vehicle_location = ego_vehicle_transform.location - - # Get the bounding box of the ego vehicle - ego_vehicle_bbox = self._vehicle.bounding_box - - # Calculate the minimum distance to other vehicles - distance_to_nearest_obstacle = None - - for actor in self._vehicle.get_world().get_actors().filter('vehicle.*'): - if actor.id != self._vehicle.id: - actor_transform = actor.get_transform() - actor_location = actor_transform.location - - # Calculate the distance between the ego vehicle and the other vehicle - distance = ego_vehicle_location.distance(actor_location) - - # Calculate the minimum distance to the other vehicle's bounding box - distance_to_obstacle = distance - ego_vehicle_bbox.extent.x - actor.bounding_box.extent.x - - if distance_to_nearest_obstacle is None or distance_to_obstacle < distance_to_nearest_obstacle: - distance_to_nearest_obstacle = distance_to_obstacle - - - return distance_to_nearest_obstacle - - def reset_vehicle(self): - """Reset the ego-vehicle""" - self._vehicle = None - print("Resetting ego-vehicle!") - - def _init_controller(self): - """ - Controller initialization. - - dt -- time difference between physics control in seconds. - This is can be fixed from server side - using the arguments -benchmark -fps=F, since dt = 1/F - - target_speed -- desired cruise speed in km/h - - min_distance -- minimum distance to remove waypoint from queue - - lateral_dict -- dictionary of arguments to setup the lateral PID controller - {'K_P':, 'K_D':, 'K_I':, 'dt'} - - longitudinal_dict -- dictionary of arguments to setup the longitudinal PID controller - {'K_P':, 'K_D':, 'K_I':, 'dt'} - """ - # Default parameters - self.args_lat_hw_dict = { - 'K_P': 0.75, - 'K_D': 0.02, - 'K_I': 0.4, - 'dt': 1.0 / self.FPS} - self.args_lat_city_dict = { - 'K_P': 0.58, - 'K_D': 0.02, - 'K_I': 0.5, - 'dt': 1.0 / self.FPS} - self.args_long_hw_dict = { - 'K_P': 0.37, - 'K_D': 0.024, - 'K_I': 0.032, - 'dt': 1.0 / self.FPS} - self.args_long_city_dict = { - 'K_P': 0.15, - 'K_D': 0.05, - 'K_I': 0.07, - 'dt': 1.0 / self.FPS} - - self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) - - self._global_plan = False - - self._target_speed = self._vehicle.get_speed_limit() - - self._min_distance = 3 - - def set_speed(self, speed): - """ - Request new target speed. - - :param speed: new target speed in km/h - """ - - self._target_speed = speed - - def set_global_plan(self, current_plan): - """ - Sets new global plan. - - :param current_plan: list of waypoints in the actual plan - """ - for elem in current_plan: - self.waypoints_queue.append(elem) - self._global_plan = True - - def get_incoming_waypoint_and_direction(self, steps=3): - """ - Returns direction and waypoint at a distance ahead defined by the user. - - :param steps: number of steps to get the incoming waypoint. - """ - if len(self.waypoints_queue) > steps: - return self.waypoints_queue[steps] - - else: - try: - wpt, direction = self.waypoints_queue[-1] - return wpt, direction - except IndexError as i: - print(i) - return None, RoadOption.VOID - return None, RoadOption.VOID - - def run_step(self, target_speed=None, debug=False): - """ - Execute one step of local planning which involves - running the longitudinal and lateral PID controllers to - follow the waypoints trajectory. - - :param target_speed: desired speed - :param debug: boolean flag to activate waypoints debugging - :return: control - """ - - if target_speed is not None: - self._target_speed = target_speed - else: - self._target_speed = self._vehicle.get_speed_limit() - - if len(self.waypoints_queue) == 0: - control = carla.VehicleControl() - control.steer = 0.0 - control.throttle = 0.0 - control.brake = 1.0 - control.hand_brake = False - control.manual_gear_shift = False - return control - - # Buffering the waypoints - if not self._waypoint_buffer: - for i in range(self._buffer_size): - if self.waypoints_queue: - self._waypoint_buffer.append( - self.waypoints_queue.popleft()) - else: - break - - # Current vehicle waypoint - self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) - - # Target waypoint - self.target_waypoint, self.target_road_option = self._waypoint_buffer[0] - - if target_speed > 50: - args_lat = self.args_lat_hw_dict - args_long = self.args_long_hw_dict - else: - args_lat = self.args_lat_city_dict - args_long = self.args_long_city_dict - - self._pid_controller = VehiclePIDController(self._vehicle, - args_lateral=args_lat, - args_longitudinal=args_long) - - control = self._pid_controller.run_step(self._target_speed, self.target_waypoint) - - # Purge the queue of obsolete waypoints - vehicle_transform = self._vehicle.get_transform() - max_index = -1 - - for i, (waypoint, _) in enumerate(self._waypoint_buffer): - if distance_vehicle( - waypoint, vehicle_transform) < self._min_distance: - max_index = i - if max_index >= 0: - for i in range(max_index + 1): - self._waypoint_buffer.popleft() - - if debug: - draw_waypoints(self._vehicle.get_world(), - [self.target_waypoint], 1.0) - return control - - -# ============================================================================== -# -- KK Agent Class --------------------------------------------------------- -# ============================================================================== - -class AgentState(Enum): - """ - AGENT_STATE represents the possible states of a roaming agent - """ - NAVIGATING = 1 - BLOCKED_BY_VEHICLE = 2 - BLOCKED_RED_LIGHT = 3 - - -class Agent(object): - """Base class to define agents in CARLA""" - - def __init__(self, vehicle): - """ - Constructor method. - - :param vehicle: actor to apply to local planner logic onto - """ - self._vehicle = vehicle - self._proximity_tlight_threshold = 5.0 # meters - self._proximity_vehicle_threshold = 10.0 # meters - self._local_planner = None - self._world = self._vehicle.get_world() - try: - self._map = self._world.get_map() - except RuntimeError as error: - print('RuntimeError: {}'.format(error)) - print(' The server could not send the OpenDRIVE (.xodr) file:') - print(' Make sure it exists, has the same name of your town, and is correct.') - sys.exit(1) - self._last_traffic_light = None - - def get_local_planner(self): - """Get method for protected member local planner""" - return self._local_planner - - @staticmethod - def run_step(debug=False): - """ - Execute one step of navigation. - - :param debug: boolean flag for debugging - :return: control - """ - control = carla.VehicleControl() - - if debug: - control.steer = 0.0 - control.throttle = 0.0 - control.brake = 0.0 - control.hand_brake = False - control.manual_gear_shift = False - - return control - - @staticmethod - def emergency_stop(): - """ - Send an emergency stop command to the vehicle - - :return: control for braking - """ - control = carla.VehicleControl() - control.steer = 0.0 - control.throttle = 0.0 - control.brake = 1.0 - control.hand_brake = False - - return control - - -# ============================================================================== -# -- KK Behavior Agent Class --------------------------------------------------------- -# ============================================================================== - -class BehaviorAgent(Agent): - """ - BehaviorAgent implements an agent that navigates scenes to reach a given - target destination, by computing the shortest possible path to it. - This agent can correctly follow traffic signs, speed limitations, - traffic lights, while also taking into account nearby vehicles. Lane changing - decisions can be taken by analyzing the surrounding environment, - such as overtaking or tailgating avoidance. Adding to these are possible - behaviors, the agent can also keep safety distance from a car in front of it - by tracking the instantaneous time to collision and keeping it in a certain range. - Finally, different sets of behaviors are encoded in the agent, from cautious - to a more aggressive ones. - """ - - def __init__(self, vehicle, ignore_traffic_light=False, behavior='normal'): - """ - Constructor method. - - :param vehicle: actor to apply to local planner logic onto - :param ignore_traffic_light: boolean to ignore any traffic light - :param behavior: type of agent to apply - """ - - super(BehaviorAgent, self).__init__(vehicle) - self.vehicle = vehicle - self.ignore_traffic_light = ignore_traffic_light - self._local_planner = LocalPlanner(self) - self._grp = None - self.look_ahead_steps = 0 - - # Vehicle information - self.speed = 0 - self.speed_limit = 0 - self.direction = None - self.incoming_direction = None - self.incoming_waypoint = None - self.start_waypoint = None - self.end_waypoint = None - self.is_at_traffic_light = 0 - self.light_state = "Green" - self.light_id_to_ignore = -1 - self.min_speed = 5 - self.behavior = None - self._sampling_resolution = 4.5 - - # Parameters for agent behavior - if behavior == 'cautious': - self.behavior = Cautious() - - elif behavior == 'normal': - self.behavior = Normal() - - elif behavior == 'aggressive': - self.behavior = Aggressive() - - def update_information(self, world): - """ - This method updates the information regarding the ego - vehicle based on the surrounding world. - - :param world: carla.world object - """ - self.speed = get_speed(self.vehicle) - self.speed_limit = world.player.get_speed_limit() - # self.speed_limit = 100 # KK set speed to 100 km/h - self._local_planner.set_speed(self.speed_limit) - self.direction = self._local_planner.target_road_option - if self.direction is None: - self.direction = RoadOption.LANEFOLLOW - - self.look_ahead_steps = int(self.speed_limit / 10) - - self.incoming_waypoint, self.incoming_direction = self._local_planner.get_incoming_waypoint_and_direction( - steps=self.look_ahead_steps) - if self.incoming_direction is None: - self.incoming_direction = RoadOption.LANEFOLLOW - - self.is_at_traffic_light = world.player.is_at_traffic_light() - if self.ignore_traffic_light: - self.light_state = "Green" - else: - if self.is_at_traffic_light: - traffic_light = world.player.get_traffic_light() - self.light_state = str(traffic_light.state) - if self.light_id_to_ignore != -1 and traffic_light.id == self.light_id_to_ignore: - # Ignore this traffic light for this iteration - self.light_state = "Green" - # Get distance to the nearest obstacle - distance_to_nearest_obstacle = self._local_planner.get_distance_to_nearest_obstacle() - - # Check if the obstacle is closer than the minimum distance - if distance_to_nearest_obstacle is not None and distance_to_nearest_obstacle < 3.0: - # Apply braking if the distance to the nearest obstacle is below the threshold - self._local_planner.brake = 1.0 - else: - self._local_planner.brake = 0.0 - #else: - # This method also includes stop signs and intersections. - #self.light_state = str(self.vehicle.get_traffic_light_state()) - - def set_destination(self, start_location, end_location, clean=False): - """ - This method creates a list of waypoints from agent's position to destination location - based on the route returned by the global router. - - :param start_location: initial position - :param end_location: final position - :param clean: boolean to clean the waypoint queue - """ - if clean: - self._local_planner.waypoints_queue.clear() - self.start_waypoint = self._map.get_waypoint(start_location) - self.end_waypoint = self._map.get_waypoint(end_location) - - route_trace = self._trace_route(self.start_waypoint, self.end_waypoint) - - self._local_planner.set_global_plan(route_trace) - - def reroute(self, spawn_points): - """ - This method implements re-routing for vehicles approaching its destination. - It finds a new target and computes another path to reach it. - - :param spawn_points: list of possible destinations for the agent - """ - - print("Target almost reached, setting new destination...") - random.shuffle(spawn_points) - new_start = self._local_planner.waypoints_queue[-1][0].transform.location - destination = spawn_points[0].location if spawn_points[0].location != new_start else spawn_points[1].location - print("New destination: " + str(destination)) - - self.set_destination(new_start, destination) - - def _trace_route(self, start_waypoint, end_waypoint): - """ - This method sets up a global router and returns the - optimal route from start_waypoint to end_waypoint. - - :param start_waypoint: initial position - :param end_waypoint: final position - """ - # Setting up global router - if self._grp is None: - wld = self.vehicle.get_world() - dao = GlobalRoutePlannerDAO( - wld.get_map(), sampling_resolution=self._sampling_resolution) - grp = GlobalRoutePlanner(dao) - grp.setup() - self._grp = grp - - # Obtain route plan - route = self._grp.trace_route( - start_waypoint.transform.location, - end_waypoint.transform.location) - - return route - - def run_step(self, debug=False): - """ - Execute one step of navigation. - - :param debug: boolean for debugging - :return control: carla.VehicleControl - """ - control = carla.VehicleControl() - - ego_vehicle_loc = self.vehicle.get_location() - ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) - - if self.is_at_traffic_light and self.light_state != "Green": - # Stop at red traffic light - control = carla.VehicleControl() - control.throttle = 0.0 - control.brake = 1.0 - control.steer = 0.0 - else: - # Get distance to the nearest obstacle - distance_to_nearest_obstacle = self._local_planner.get_distance_to_nearest_obstacle() - - # Check if the obstacle is closer than the minimum distance - if distance_to_nearest_obstacle is not None and distance_to_nearest_obstacle < 3.0: - # Apply braking if the distance to the nearest obstacle is below the threshold - control.throttle = 0.0 - control.brake = 1.0 - control.steer = 0.0 - else: - # Continue following the planned route - control = self._local_planner.run_step( - target_speed=min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist), debug=debug) - self.vehicle.apply_control(control) - - return control - - -# ============================================================================== -# -- Game Loop --------------------------------------------------------- -# ============================================================================== - - -def game_loop(args): - """ Main loop for agent""" - - pygame.init() - pygame.font.init() - world = None - tot_target_reached = 0 - num_min_waypoints = 21 - - try: - client = carla.Client(args.host, args.port) - client.set_timeout(70.0) - - display = pygame.display.set_mode( - (args.width, args.height), - pygame.HWSURFACE | pygame.DOUBLEBUF) - - hud = HUD(args.width, args.height) - - #selected_world = client.load_world("Town01") # KK Changes world to Town04 - selected_world = client.get_world() - - world = World(client.get_world(), hud, args) - controller = KeyboardControl(world) - - agent = BehaviorAgent(world.player, behavior=args.behavior) - - spawn_points = world.map.get_spawn_points() - random.shuffle(spawn_points) - - if spawn_points[0].location != agent.vehicle.get_location(): - destination = spawn_points[0].location - else: - destination = spawn_points[1].location - - agent.set_destination(agent.vehicle.get_location(), destination, clean=True) - - clock = pygame.time.Clock() - - while True: - clock.tick_busy_loop(60) - if controller.parse_events(world): - return - - # As soon as the server is ready continue! - if not world.world.wait_for_tick(10.0): - continue - - if args.agent == "Roaming" or args.agent == "Basic": - if controller.parse_events(world): - return - - #agent.update_information(world) - # as soon as the server is ready continue! - world.world.wait_for_tick(10.0) - - world.tick(clock) - world.render(display) - pygame.display.flip() - control = agent.run_step() - control.manual_gear_shift = False - world.player.apply_control(control) - else: - agent.update_information(world) - - world.tick(clock) - world.render(display) - pygame.display.flip() - - - # Set new destination when target has been reached - if len(agent.get_local_planner().waypoints_queue) < num_min_waypoints and args.loop: - agent.reroute(spawn_points) - tot_target_reached += 1 - world.hud.notification("The target has been reached " + - str(tot_target_reached) + " times.", seconds=4.0) - - elif len(agent.get_local_planner().waypoints_queue) == 0 and not args.loop: - print("Target reached, mission accomplished...") - break - - speed_limit = world.player.get_speed_limit() - agent.get_local_planner().set_speed(speed_limit) - - control = agent.run_step() - world.player.apply_control(control) - - finally: - if world is not None: - world.destroy() - - pygame.quit() - - -# ============================================================================== -# -- main() -------------------------------------------------------------- -# ============================================================================== - - -def main(): - """Main method""" - - argparser = argparse.ArgumentParser( - description='CARLA Automatic Control Client') - argparser.add_argument( - '-v', '--verbose', - action='store_true', - dest='debug', - help='Print debug information') - argparser.add_argument( - '--host', - metavar='H', - default='127.0.0.1', - help='IP of the host server (default: 127.0.0.1)') - argparser.add_argument( - '-p', '--port', - metavar='P', - default=2000, - type=int, - help='TCP port to listen to (default: 2000)') - argparser.add_argument( - '--res', - metavar='WIDTHxHEIGHT', - default='1280x720', - help='Window resolution (default: 1280x720)') - argparser.add_argument( - '--filter', - metavar='PATTERN', - default='vehicle.*', - help='Actor filter (default: "vehicle.*")') - argparser.add_argument( - '--gamma', - default=2.2, - type=float, - help='Gamma correction of the camera (default: 2.2)') - argparser.add_argument( - '-l', '--loop', - action='store_true', - dest='loop', - help='Sets a new random destination upon reaching the previous one (default: False)') - argparser.add_argument( - '-b', '--behavior', type=str, - choices=["cautious", "normal", "aggressive"], - help='Choose one of the possible agent behaviors (default: normal) ', - default='normal') - argparser.add_argument("-a", "--agent", type=str, - choices=["Behavior", "Roaming", "Basic"], - help="select which agent to run", - default="Behavior") - argparser.add_argument( - '-s', '--seed', - help='Set seed for repeating executions (default: None)', - default=None, - type=int) - - args = argparser.parse_args() - - args.width, args.height = [int(x) for x in args.res.split('x')] - - log_level = logging.DEBUG if args.debug else logging.INFO - logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) - - logging.info('listening to server %s:%s', args.host, args.port) - - print(__doc__) - - try: - game_loop(args) - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') - - -if __name__ == '__main__': - main() \ No newline at end of file +"""Example of automatic vehicle control from client side.""" + +from __future__ import print_function + +import sys +import os +# 获取当前脚本所在的目录,并找到agents文件夹的路径 +current_dir = os.path.dirname(os.path.abspath(__file__)) +agents_path = os.path.join(current_dir, 'agents') +# 将该路径添加到模块搜索路径中 +if os.path.exists(agents_path): + sys.path.insert(0, agents_path) +import argparse +import collections +import datetime +import glob +import logging +import math +import os +import cv2 +import random +import re +import sys +import weakref +from enum import Enum +from collections import deque + + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import K_ESCAPE + from pygame.locals import K_q + from pygame.locals import K_l + from pygame.locals import K_v + from pygame.locals import K_c +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError( + 'cannot import numpy, make sure numpy package is installed') + +# ============================================================================== +# -- Find CARLA module --------------------------------------------------------- +# ============================================================================== +try: + sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( + sys.version_info.major, + sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) +except IndexError: + pass + +# ============================================================================== +# -- Add PythonAPI for release mode -------------------------------------------- +# ============================================================================== +try: + sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + '/carla') +except IndexError: + pass + +import carla +from carla import ColorConverter as cc +from torch.utils.tensorboard import SummaryWriter +import torch +#import tensorflow as tf + +# Create a SummaryWriter object to write the metrics to Tensorboard +writer = SummaryWriter() + +from agents.navigation.global_route_planner import GlobalRoutePlanner +from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO +from agents.navigation.types_behavior import Cautious, Aggressive, Normal + +from agents.tools.misc import get_speed, positive, draw_waypoints, distance_vehicle + + +# ============================================================================== +# -- Global functions ---------------------------------------------------------- +# ============================================================================== + + +def find_weather_presets(): + """Method to find weather presets""" + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') + def name(x): return ' '.join(m.group(0) for m in rgx.finditer(x)) + presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + """Method to get actor display name""" + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name + + +# ============================================================================== +# -- World --------------------------------------------------------------- +# ============================================================================== + +class World(object): + """ Class representing the surrounding environment """ + + def __init__(self, carla_world, hud, args): + """Constructor method""" + self.world = carla_world + try: + self.map = self.world.get_map() + except RuntimeError as error: + print('RuntimeError: {}'.format(error)) + print(' The server could not send the OpenDRIVE (.xodr) file:') + print(' Make sure it exists, has the same name of your town, and is correct.') + sys.exit(1) + self.hud = hud + self.player = None + self.collision_sensor = None + self.lane_invasion_sensor = None + self.gnss_sensor = None + self.camera_manager = None + self._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = args.filter + self._gamma = args.gamma + self.restart(args) + self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False + self.recording_start = 0 + + def restart(self, args): + """Restart the world""" + # Keep same camera config if the camera manager exists. + cam_index = self.camera_manager.index if self.camera_manager is not None else 0 + cam_pos_id = self.camera_manager.transform_index if self.camera_manager is not None else 0 + # Set the seed if requested by user + if args.seed is not None: + random.seed(args.seed) + + # KK Get a vehicle blueprint + #blueprint = self.world.get_blueprint_library().filter('vehicle.lincoln.mkz2017')[0] # KK + blueprint = self.world.get_blueprint_library().filter('model3')[0] + blueprint.set_attribute('role_name', 'hero') + if blueprint.has_attribute('color'): + color = '236,102,17' # KK Packt logo color + blueprint.set_attribute('color', color) + # Spawn the player. + print("Spawning the player") + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + + while self.player is None: + if not self.map.get_spawn_points(): + print('There are no spawn points available in your map/town.') + print('Please add some Vehicle Spawn Point to your UE4 scene.') + sys.exit(1) + spawn_points = self.map.get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + #spawn_point = spawn_points[0] if spawn_points else carla.Transform() # KK + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + # Set up the sensors. + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud, self._gamma) + self.camera_manager.transform_index = cam_pos_id + self.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.player) + self.hud.notification(actor_type) + def next_weather(self, reverse=False): + """Get next weather setting""" + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.player.get_world().set_weather(preset[0]) + + def tick(self, clock): + """Method for every tick""" + self.hud.tick(self, clock) + + def render(self, display): + """Render world""" + self.camera_manager.render(display) + self.hud.render(display) + + def destroy_sensors(self): + """Destroy sensors""" + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None + + def destroy(self): + """Destroys all actors""" + actors = [ + self.camera_manager.sensor, + self.collision_sensor.sensor, + self.lane_invasion_sensor.sensor, + self.gnss_sensor.sensor, + self.player] + for actor in actors: + if actor is not None: + actor.destroy() + + +# ============================================================================== +# -- KeyboardControl ----------------------------------------------------------- +# ============================================================================== + + +class KeyboardControl(object): + def __init__(self, world, start_locked=True): + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + # 可选:启动时自动锁定 + if start_locked and world.camera_manager is not None: + world.camera_manager.set_lock_mode(True) + world.camera_manager.hud.notification("Camera Lock: ON", seconds=2.0) + + def parse_events(self, world): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + if event.type == pygame.KEYUP: + if self._is_quit_shortcut(event.key): + return True + elif event.key == K_l: # 按L键切换锁定 + if world.camera_manager is not None: + world.camera_manager.toggle_lock() + elif event.key == K_v: # 按V键切换锁定视角 + if world.camera_manager is not None and world.camera_manager.lock_mode: + world.camera_manager.next_lock_view() + elif event.key == K_c: # 按C键切换相机(原有功能) + if world.camera_manager is not None: + world.camera_manager.toggle_camera() + + @staticmethod + def _is_quit_shortcut(key): + """Shortcut for quitting""" + return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) + +# ============================================================================== +# -- HUD ----------------------------------------------------------------------- +# ============================================================================== + + +class HUD(object): + """Class for HUD text""" + + def __init__(self, width, height): + """Constructor method""" + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 20) + font_name = 'courier' if os.name == 'nt' else 'mono' + fonts = [x for x in pygame.font.get_fonts() if font_name in x] + default_font = 'ubuntumono' + mono = default_font if default_font in fonts else fonts[0] + mono = pygame.font.match_font(mono) + self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + self.help = HelpText(pygame.font.Font(mono, 24), width, height) + self.server_fps = 0 + self.frame = 0 + self.simulation_time = 0 + self._show_info = True + self._info_text = [] + self._server_clock = pygame.time.Clock() + + def on_world_tick(self, timestamp): + """Gets informations from the world at every tick""" + self._server_clock.tick() + self.server_fps = self._server_clock.get_fps() + self.frame = timestamp.frame_count + self.simulation_time = timestamp.elapsed_seconds + + def tick(self, world, clock): + """HUD method for every tick""" + self._notifications.tick(world, clock) + if not self._show_info: + return + transform = world.player.get_transform() + vel = world.player.get_velocity() + control = world.player.get_control() + heading = 'N' if abs(transform.rotation.yaw) < 89.5 else '' + heading += 'S' if abs(transform.rotation.yaw) > 90.5 else '' + heading += 'E' if 179.5 > transform.rotation.yaw > 0.5 else '' + heading += 'W' if -0.5 > transform.rotation.yaw > -179.5 else '' + colhist = world.collision_sensor.get_collision_history() + collision = [colhist[x + self.frame - 200] for x in range(0, 200)] + max_col = max(1.0, max(collision)) + collision = [x / max_col for x in collision] + vehicles = world.world.get_actors().filter('vehicle.*') + + self._info_text = [ + 'Server: % 16.0f FPS' % self.server_fps, + 'Client: % 16.0f FPS' % clock.get_fps(), + '', + 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), + 'Map: % 20s' % world.map.name, + 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), + '', + 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)), + u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (transform.rotation.yaw, heading), + 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (transform.location.x, transform.location.y)), + 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), + 'Height: % 18.0f m' % transform.location.z, + ''] + if isinstance(control, carla.VehicleControl): + self._info_text += [ + ('Throttle:', control.throttle, 0.0, 1.0), + ('Steer:', control.steer, -1.0, 1.0), + ('Brake:', control.brake, 0.0, 1.0), + ('Reverse:', control.reverse), + ('Hand brake:', control.hand_brake), + ('Manual:', control.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(control.gear, control.gear)] + elif isinstance(control, carla.WalkerControl): + self._info_text += [ + ('Speed:', control.speed, 0.0, 5.556), + ('Jump:', control.jump)] + self._info_text += [ + '', + 'Collision:', + collision, + '', + 'Number of vehicles: % 8d' % len(vehicles)] + + if len(vehicles) > 1: + self._info_text += ['Nearby vehicles:'] + + def dist(l): + return math.sqrt((l.x - transform.location.x)**2 + (l.y - transform.location.y) + ** 2 + (l.z - transform.location.z)**2) + vehicles = [(dist(x.get_location()), x) for x in vehicles if x.id != world.player.id] + + for dist, vehicle in sorted(vehicles): + if dist > 200.0: + break + vehicle_type = get_actor_display_name(vehicle, truncate=22) + self._info_text.append('% 4dm %s' % (dist, vehicle_type)) + + def toggle_info(self): + """Toggle info on or off""" + self._show_info = not self._show_info + + def notification(self, text, seconds=2.0): + """Notification text""" + self._notifications.set_text(text, seconds=seconds) + + def error(self, text): + """Error text""" + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display): + """Render for HUD class""" + if self._show_info: + info_surface = pygame.Surface((220, self.dim[1])) + info_surface.set_alpha(100) + display.blit(info_surface, (0, 0)) + v_offset = 4 + bar_h_offset = 100 + bar_width = 106 + for item in self._info_text: + if v_offset + 18 > self.dim[1]: + break + if isinstance(item, list): + if len(item) > 1: + points = [(x + 8, v_offset + 8 + (1 - y) * 30) for x, y in enumerate(item)] + pygame.draw.lines(display, (255, 136, 0), False, points, 2) + item = None + v_offset += 18 + elif isinstance(item, tuple): + if isinstance(item[1], bool): + rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) + pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) + else: + rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect_border, 1) + fig = (item[1] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect( + (bar_h_offset + fig * (bar_width - 6), v_offset + 8), (6, 6)) + else: + rect = pygame.Rect((bar_h_offset, v_offset + 8), (fig * bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect) + item = item[0] + if item: # At this point has to be a str. + surface = self._font_mono.render(item, True, (255, 255, 255)) + display.blit(surface, (8, v_offset)) + v_offset += 18 + self._notifications.render(display) + self.help.render(display) + +# ============================================================================== +# -- FadingText ---------------------------------------------------------------- +# ============================================================================== + + +class FadingText(object): + """ Class for fading text """ + + def __init__(self, font, dim, pos): + """Constructor method""" + self.font = font + self.dim = dim + self.pos = pos + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + + def set_text(self, text, color=(255, 255, 255), seconds=2.0): + """Set fading text""" + text_texture = self.font.render(text, True, color) + self.surface = pygame.Surface(self.dim) + self.seconds_left = seconds + self.surface.fill((0, 0, 0, 0)) + self.surface.blit(text_texture, (10, 11)) + + def tick(self, _, clock): + """Fading text method for every tick""" + delta_seconds = 1e-3 * clock.get_time() + self.seconds_left = max(0.0, self.seconds_left - delta_seconds) + self.surface.set_alpha(500.0 * self.seconds_left) + + def render(self, display): + """Render fading text method""" + display.blit(self.surface, self.pos) + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + """ Helper class for text render""" + + def __init__(self, font, width, height): + """Constructor method""" + lines = [ + 'Welcome to CARLA automatic control client', + '', + 'Camera Control:', + ' C : Change camera position', + ' L : Toggle camera lock on vehicle', + ' V : Change lock view (when locked)', + ' PageUp : Toggle recording', + '', + 'Vehicle Control:', + ' W : Throttle', + ' S : Brake', + ' A/D : Steer left/right', + ' Q : Toggle reverse', + ' P : Toggle autopilot', + '', + 'Miscellaneous:', + ' H : Toggle help', + ' ESC : Quit', + ' CTRL + Q : Quit', + ] + + self.font = font + self.dim = (680, len(lines) * 22 + 12) + self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + self.surface.fill((0, 0, 0, 0)) + for i, line in enumerate(lines): + text_texture = self.font.render(line, True, (255, 255, 255)) + self.surface.blit(text_texture, (22, i * 22)) + self._render = False + self.surface.set_alpha(220) + + def toggle(self): + """Toggle on or off the render help""" + self._render = not self._render + + def render(self, display): + """Render help text method""" + if self._render: + display.blit(self.surface, self.pos) + +# ============================================================================== +# -- CollisionSensor ----------------------------------------------------------- +# ============================================================================== + + +class CollisionSensor(object): + """ Class for collision sensors""" + + def __init__(self, parent_actor, hud): + """Constructor method""" + self.sensor = None + self.history = [] + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + blueprint = world.get_blueprint_library().find('sensor.other.collision') + self.sensor = world.spawn_actor(blueprint, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) + + def get_collision_history(self): + """Gets the history of collisions""" + history = collections.defaultdict(int) + for frame, intensity in self.history: + history[frame] += intensity + return history + + @staticmethod + def _on_collision(weak_self, event): + """On collision method""" + self = weak_self() + if not self: + return + actor_type = get_actor_display_name(event.other_actor) + self.hud.notification('Collision with %r' % actor_type) + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x ** 2 + impulse.y ** 2 + impulse.z ** 2) + self.history.append((event.frame, intensity)) + if len(self.history) > 4000: + self.history.pop(0) + +# ============================================================================== +# -- LaneInvasionSensor -------------------------------------------------------- +# ============================================================================== + + +class LaneInvasionSensor(object): + """Class for lane invasion sensors""" + + def __init__(self, parent_actor, hud): + """Constructor method""" + self.sensor = None + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.lane_invasion') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) + + @staticmethod + def _on_invasion(weak_self, event): + """On invasion method""" + self = weak_self() + if not self: + return + lane_types = set(x.type for x in event.crossed_lane_markings) + text = ['%r' % str(x).split()[-1] for x in lane_types] + self.hud.notification('Crossed line %s' % ' and '.join(text)) + +# ============================================================================== +# -- GnssSensor -------------------------------------------------------- +# ============================================================================== + + +class GnssSensor(object): + """ Class for GNSS sensors""" + + def __init__(self, parent_actor): + """Constructor method""" + self.sensor = None + self._parent = parent_actor + self.lat = 0.0 + self.lon = 0.0 + world = self._parent.get_world() + blueprint = world.get_blueprint_library().find('sensor.other.gnss') + self.sensor = world.spawn_actor(blueprint, carla.Transform(carla.Location(x=1.0, z=2.8)), + attach_to=self._parent) + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) + + @staticmethod + def _on_gnss_event(weak_self, event): + """GNSS method""" + self = weak_self() + if not self: + return + self.lat = event.latitude + self.lon = event.longitude + +# ============================================================================== +# -- CameraManager ------------------------------------------------------------- +# ============================================================================== + + +class CameraManager(object): + """ Class for camera management""" + + def __init__(self, parent_actor, hud, gamma_correction): + """Constructor method""" + self.sensor = None + self.surface = None + self._parent = parent_actor + self.hud = hud + self.recording = False + bound_y = 0.5 + self._parent.bounding_box.extent.y + attachment = carla.AttachmentType + self._camera_transforms = [ + (carla.Transform( + carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8.0)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=1.6, z=1.7)), attachment.Rigid), + (carla.Transform( + carla.Location(x=5.5, y=1.5, z=1.5)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=-8.0, z=6.0), carla.Rotation(pitch=6.0)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=-1, y=-bound_y, z=0.5)), attachment.Rigid)] + self.transform_index = 1 + self.sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, + 'Camera Semantic Segmentation (CityScapes Palette)'], + ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] + + # 添加锁定模式相关属性 + self.lock_mode = True # 默认启用锁定模式 + self.lock_target = parent_actor # 锁定目标 + self.lock_transform_index = 0 # 锁定模式的相机位置索引 + + # 定义锁定模式的相机变换(可以自定义不同角度) + self._lock_transforms = [ + (carla.Transform( + carla.Location(x=-6.0, z=3.5), carla.Rotation(pitch=-10.0)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=2.0, z=1.5), carla.Rotation(pitch=0.0)), attachment.Rigid), + (carla.Transform( + carla.Location(x=5.0, y=2.0, z=2.0), carla.Rotation(pitch=-5.0)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=-10.0, z=5.0), carla.Rotation(pitch=-15.0)), attachment.SpringArm), + ] + + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + blp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + blp.set_attribute('image_size_x', str(hud.dim[0])) + blp.set_attribute('image_size_y', str(hud.dim[1])) + if blp.has_attribute('gamma'): + blp.set_attribute('gamma', str(gamma_correction)) + elif item[0].startswith('sensor.lidar'): + blp.set_attribute('range', '50') + item.append(blp) + self.index = None + + def toggle_camera(self): + """Activate a camera""" + self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) + self.set_sensor(self.index, notify=False, force_respawn=True) + + def set_sensor(self, index, notify=True, force_respawn=False): + """Set a sensor""" + index = index % len(self.sensors) + needs_respawn = True if self.index is None else ( + force_respawn or (self.sensors[index][0] != self.sensors[self.index][0])) + if needs_respawn: + if self.sensor is not None: + self.sensor.destroy() + self.surface = None + + # 选择变换和附着类型 + if self.lock_mode: + # 锁定模式使用锁定变换 + transform, attachment_type = self._lock_transforms[self.lock_transform_index] + else: + # 非锁定模式使用原有变换 + transform, attachment_type = self._camera_transforms[self.transform_index] + + self.sensor = self._parent.get_world().spawn_actor( + self.sensors[index][-1], + transform, + attach_to=self._parent, + attachment_type=attachment_type) + + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) + if notify: + sensor_name = self.sensors[index][2] + if self.lock_mode: + sensor_name += " (Locked)" + self.hud.notification(sensor_name) + self.index = index + + def next_sensor(self): + """Get the next sensor""" + self.set_sensor(self.index + 1) + + def toggle_recording(self): + """Toggle recording on or off""" + self.recording = not self.recording + self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + + def toggle_lock(self): + """切换锁定模式""" + self.lock_mode = not self.lock_mode + mode_text = "Camera Lock: ON" if self.lock_mode else "Camera Lock: OFF" + self.hud.notification(mode_text, seconds=2.0) + # 重新设置传感器以应用锁定模式 + if self.sensor is not None: + self.set_sensor(self.index, notify=False, force_respawn=True) + return self.lock_mode + + def set_lock_mode(self, enable=True): + """设置锁定模式""" + self.lock_mode = enable + return self.lock_mode + + def next_lock_view(self): + """切换到下一个锁定视角""" + if self.lock_mode: + self.lock_transform_index = (self.lock_transform_index + 1) % len(self._lock_transforms) + self.hud.notification(f'Lock View {self.lock_transform_index + 1}', seconds=1.0) + # 重新设置传感器以应用新的锁定视角 + if self.sensor is not None: + self.set_sensor(self.index, notify=False, force_respawn=True) + + def render(self, display): + """Render method""" + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self.sensors[self.index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 3), 3)) + lidar_data = np.array(points[:, :2]) + lidar_data *= min(self.hud.dim) / 100.0 + lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) + lidar_data = np.fabs(lidar_data) # pylint: disable=assignment-from-no-return + lidar_data = lidar_data.astype(np.int32) + lidar_data = np.reshape(lidar_data, (-1, 2)) + lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) + lidar_img = np.zeros(lidar_img_size) + lidar_img[tuple(lidar_data.T)] = (255, 255, 255) + self.surface = pygame.surfarray.make_surface(lidar_img) + else: + image.convert(self.sensors[self.index][1]) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + ######################################################################################################################################### +############################################################ yolov3 #################################################################### + def calculate_iou(box1, box2): + """ + Calculate the Intersection over Union (IoU) between two bounding boxes. + + Args: + box1 (list): List containing the coordinates of the first bounding box [x, y, w, h]. + box2 (list): List containing the coordinates of the second bounding box [x, y, w, h]. + + Returns: + float: The IoU value. + + """ + # Extract box coordinates + x1, y1, w1, h1 = box1 + x2, y2, w2, h2 = box2 + + # Calculate box coordinates + x_left = max(x1, x2) + y_top = max(y1, y2) + x_right = min(x1 + w1, x2 + w2) + y_bottom = min(y1 + h1, y2 + h2) + + # Calculate intersection area + intersection_area = max(0, x_right - x_left) * max(0, y_bottom - y_top) + + # Calculate union area + box1_area = w1 * h1 + box2_area = w2 * h2 + union_area = box1_area + box2_area - intersection_area + + # Calculate IoU + iou = intersection_area / union_area + + return iou + + + test_im = np.array(image.raw_data) + test_im = test_im.copy() + test_im = test_im.reshape((image.height, image.width, 4)) + test_im = test_im[:, :, :3] + + # Load YOLO + model_weights = 'yolov3.cfg' + model_cfg = 'yolov3.weights' + #frameWidth = 1200 + #frameHeight = 720 + frameWidth = 640 + frameHeight = 480 + net = cv2.dnn.readNet(model_weights, model_cfg) + + # Define your loss function + #loss_fn = tf.keras.losses.BinaryCrossentropy() + + classes = [] + with open("coco.names", "r") as f: + classes = [line.strip() for line in f.readlines()] + + layers_names = net.getLayerNames() + #output_layers = [layers_names[i[0] - 1] for i in net.getUnconnectedOutLayers()] + output_layers = [layers_names[i - 1] for i in net.getUnconnectedOutLayers()] + colors = np.random.uniform(0, 255, size=(len(classes), 3)) + + # Loading image + img = cv2.resize(test_im, (frameWidth, frameHeight), None) + height, width, channels = img.shape + img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + + # Detect objects + blob = cv2.dnn.blobFromImage(img, 1/255, (416, 416), (0, 0, 0), True, crop=False) + net.setInput(blob) + outs = net.forward(output_layers) + + # Perform object detection + class_ids = [] + confidences = [] + boxes = [] + + for out in outs: + for detection in out: + scores = detection[5:] + class_id = np.argmax(scores) + confidence = scores[class_id] + + if confidence > 0.5: + # Object detected + center_x = int(detection[0] * width) + center_y = int(detection[1] * height) + w = int(detection[2] * width) + h = int(detection[3] * height) + + # Rectangle coordinates + x = int(center_x - w / 2) + y = int(center_y - h / 2) + + boxes.append([x, y, w, h]) + confidences.append(float(confidence)) + class_ids.append(class_id) + + indexes = cv2.dnn.NMSBoxes(boxes, confidences, 0.5, 0.4) + font = cv2.FONT_HERSHEY_SIMPLEX + colors = np.random.uniform(0, 255, size=(len(boxes), 3)) + + detected_labels = [] + detected_boxes = [] + predicted_confidences = [] + predicted_class_probs = [] + + for i in range(len(boxes)): + if i in indexes: + x, y, w, h = boxes[i] + label = str(classes[class_ids[i]]) + confidence = str(round(confidences[i], 2)) + color = colors[i] + cv2.rectangle(img, (x, y), (x + w, y + h), color, 2) + cv2.putText(img, label + " " + confidence, (x + 60, y + 60), font, 0.8, (255, 255, 200), 1) + detected_labels.append(label) + detected_boxes.append([x, y, w, h]) + predicted_confidences.append(float(confidence)) + predicted_class_probs.append(label) + + # Ground truth annotations + ground_truth_labels = ['person', 'bicycle', 'car', 'traffic light', 'fire hydrant', 'pottedplant', 'stop sign', 'bench'] + ground_truth_boxes = [[250, 220, 160, 190], [200, 200, 80, 80], [200, 200, 80, 80], [450, 90, 20, 50], [200, 200, 80, 80], [200, 200, 80, 80], [200, 200, 80, 80], [200, 200, 80, 80]] # [x, y, w, h] format + + #def normalize_boxes(boxes, image_width, image_height): + # Convert box coordinates to numpy array + # boxes = np.array(boxes) + + # Extract box coordinates + #x, y, w, h = np.split(boxes, 4, axis=1) + + # Normalize box coordinates + #normalized_x = x / image_width + #normalized_y = y / image_height + #normalized_w = w / image_width + #normalized_h = h / image_height + + # Concatenate normalized coordinates + # normalized_boxes = np.concatenate((normalized_x, normalized_y, normalized_w, normalized_h), axis=1) + + #return normalized_boxes + ''' + def preprocess_ground_truth(ground_truth_labels, ground_truth_boxes): + # Define a dictionary to map class labels to class IDs + label_to_id = { + 'person': 0, + 'bicycle': 1, + 'car': 2, + 'traffic light': 3, + 'fire hydrant': 4, + 'pottedplant': 5, + 'stop sign': 6, + 'bench': 7 + } + ''' + + # Convert class labels to class IDs# + ''' + class_ids = [label_to_id[label] for label in ground_truth_labels] + + # Convert class IDs to one-hot encoding + num_classes = len(label_to_id) + one_hot_labels = tf.one_hot(class_ids, num_classes) + + # Normalize bounding box coordinates + normalized_boxes = normalize_boxes(ground_truth_boxes, width, height) # Implement 'normalize_boxes' function to scale box coordinates between 0 and 1 + + print('Normalized Boxes Shape:', normalized_boxes.shape) + print('One-Hot Labels Shape:', one_hot_labels.shape) + print('Normalized Boxes Data Type:', normalized_boxes.dtype) + print('One-Hot Labels Data Type:', one_hot_labels.dtype) + + if normalized_boxes.shape != one_hot_labels.shape: + print('Error: Shapes of normalized_boxes and one_hot_labels do not match.') + return None + + if normalized_boxes.dtype != one_hot_labels.dtype: + print('Error: Data types of normalized_boxes and one_hot_labels do not match.') + return None + + # Combine labels and boxes into a single tensor + ground_truth_targets = tf.concat([normalized_boxes, one_hot_labels], axis=-1) + + return ground_truth_targets, normalized_boxes, one_hot_labels, label_to_id + # Call the function and store the returned values + #ground_truth_targets, normalized_boxes, one_hot_labels, label_to_id = preprocess_ground_truth(ground_truth_labels, ground_truth_boxes) + + #num_classes = len(label_to_id) + ''' + + + # Perform object matching for ground truth and detected objects + TP = 0 # True positives + FP = 0 # False positives + FN = 0 # False negatives + threshold = 0.5 + print("Detected Labels: ", detected_labels) + print("Detected boxes: ", detected_boxes) + + for i, (gt_label, gt_box) in enumerate(zip(ground_truth_labels, ground_truth_boxes)): + max_iou = 0 + max_iou_index = -1 + + for j, (det_label, det_box) in enumerate(zip(detected_labels, detected_boxes)): + iou = calculate_iou(gt_box, det_box) + if iou > max_iou: + max_iou = iou + max_iou_index = j + + if max_iou >= threshold: + # Matched object found (True positive) + TP += 1 + # Remove the matched object from detected objects + if max_iou_index != -1: + detected_labels.pop(max_iou_index) + detected_boxes.pop(max_iou_index) + else: + # No match found (False negative) + FN += 1 + + # Any remaining unmatched detected objects are false positives + FP = len(detected_labels) + + eps = 1e-7 # Small value to avoid division by zero + + # Calculate performance metrics + accuracy = TP / (TP + FP + FN + eps) + precision = TP / (TP + FP + eps) + recall = TP / (TP + FN + eps) + matt_corr_coeff = (TP - (FP * FN)) / math.sqrt((TP + FP + eps) * (TP + FN + eps) * (FP + eps) * (FN + eps)) + + + # Calculate the loss + #ground_truth_targets = preprocess_ground_truth(ground_truth_labels, ground_truth_boxes) # Preprocess the ground truth targets + + # localization_loss = loss_fn(detected_boxes, ground_truth_targets) # Calculate the localization loss + # confidence_loss = loss_fn(predicted_confidences, ground_truth_targets) # Calculate the confidence loss + # class_prob_loss = loss_fn(predicted_class_probs, ground_truth_targets) # Calculate the class probability loss + + # Log the metrics to Tensorboard + log_dir = "./logs3" # Specify the log directory her + writer = SummaryWriter(log_dir=log_dir) + writer.add_scalar('Accuracy', accuracy, image.frame) + writer.add_scalar('Precision', precision, image.frame) + writer.add_scalar('Recall', recall, image.frame) + writer.add_scalar('IOU', max_iou, image.frame) + #writer.add_scalar('Matthews Correlation Coefficient', matt_corr_coeff) + #writer.add_scalar('Localization Loss', localization_loss, image.frame) + #writer.add_scalar('Confidence Loss', confidence_loss, image.frame) + #writer.add_scalar('Class Probability Loss', class_prob_loss, image.frame) + + # Show the annotated image + #cv2.imshow("Annotated Image", img) + #cv2.waitKey(0) + #cv2.destroyAllWindows() + + self.surface = pygame.surfarray.make_surface(img.swapaxes(0, 1)) + if self.recording: + image.save_to_disk('output/%08d' % image.frame) + + +# ============================================================================== +# -- KK PID Controllers --------------------------------------------------------- +# ============================================================================== + +class VehiclePIDController(): + """ + VehiclePIDController is the combination of two PID controllers + (lateral and longitudinal) to perform the + low level control a vehicle from client side + """ + + + def __init__(self, vehicle, args_lateral, args_longitudinal, max_throttle=0.75, max_brake=0.3, max_steering=0.8): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param args_lateral: dictionary of arguments to set the lateral PID controller + using the following semantics: + K_P -- Proportional term + K_D -- Differential term + K_I -- Integral term + :param args_longitudinal: dictionary of arguments to set the longitudinal + PID controller using the following semantics: + K_P -- Proportional term + K_D -- Differential term + K_I -- Integral term + """ + + self.max_brake = max_brake + self.max_throt = max_throttle + self.max_steer = max_steering + + self._vehicle = vehicle + self._world = self._vehicle.get_world() + self.past_steering = self._vehicle.get_control().steer + self._lon_controller = PIDLongitudinalController(self._vehicle, **args_longitudinal) + self._lat_controller = PIDLateralController(self._vehicle, **args_lateral) + + def run_step(self, target_speed, waypoint): + """ + Execute one step of control invoking both lateral and longitudinal + PID controllers to reach a target waypoint + at a given target_speed. + + :param target_speed: desired vehicle speed + :param waypoint: target location encoded as a waypoint + :return: distance (in meters) to the waypoint + """ + + acceleration = self._lon_controller.run_step(target_speed) + current_steering = self._lat_controller.run_step(waypoint) + control = carla.VehicleControl() + if acceleration >= 0.0: + control.throttle = min(acceleration, self.max_throt) + control.brake = 0.0 + else: + control.throttle = 0.0 + control.brake = min(abs(acceleration), self.max_brake) + + # Steering regulation: changes cannot happen abruptly, can't steer too much. + + if current_steering > self.past_steering + 0.1: + current_steering = self.past_steering + 0.1 + elif current_steering < self.past_steering - 0.1: + current_steering = self.past_steering - 0.1 + + if current_steering >= 0: + steering = min(self.max_steer, current_steering) + else: + steering = max(-self.max_steer, current_steering) + + control.steer = steering + control.hand_brake = False + control.manual_gear_shift = False + self.past_steering = steering + + return control + + +class PIDLongitudinalController(): + """ + PIDLongitudinalController implements longitudinal control using a PID. + """ + + + def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=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 = vehicle + self._k_p = K_P + self._k_d = K_D + self._k_i = K_I + self._dt = dt + self._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) / 2 + target_speed = target_speed / 2 + 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_speed + self._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._dt + else: + _de = 0.0 + _ie = 0.0 + + return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) + + +class PIDLateralController(): + """ + PIDLateralController implements lateral control using a PID. + """ + + def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=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 = vehicle + self._k_p = K_P + self._k_d = K_D + self._k_i = K_I + self._dt = dt + self._e_buffer = deque(maxlen=10) + + def run_step(self, waypoint): + """ + Execute one step of lateral control to steer + the 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] + """ + v_begin = vehicle_transform.location + v_end = v_begin + carla.Location(x=math.cos(math.radians(vehicle_transform.rotation.yaw)), + y=math.sin(math.radians(vehicle_transform.rotation.yaw))) + + v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0]) + w_vec = np.array([waypoint.transform.location.x - + v_begin.x, waypoint.transform.location.y - + v_begin.y, 0.0]) + _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / + (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0)) + + _cross = np.cross(v_vec, w_vec) + + if _cross[2] < 0: + _dot *= -1.0 + + self._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._dt + else: + _de = 0.0 + _ie = 0.0 + + return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) + + +# ============================================================================== +# -- KK Local Planner --------------------------------------------------------- +# ============================================================================== + +class RoadOption(Enum): + """ + RoadOption represents the possible topological configurations + when moving from a segment of lane to other. + """ + VOID = -1 + LEFT = 1 + RIGHT = 2 + STRAIGHT = 3 + LANEFOLLOW = 4 + CHANGELANELEFT = 5 + CHANGELANERIGHT = 6 + + +class LocalPlanner(object): + """ + LocalPlanner implements the basic behavior of following a trajectory + of waypoints that is generated on-the-fly. + The low-level motion of the vehicle is computed by using two PID controllers, + one is used for the lateral control + and the other for the longitudinal control (cruise speed). + + When multiple paths are available (intersections) + this local planner makes a random choice. + """ + + # Minimum distance to target waypoint as a percentage + # (e.g. within 80% of total distance) + + # FPS used for dt + FPS = 20 + + def __init__(self, agent): + """ + :param agent: agent that regulates the vehicle + :param vehicle: actor to apply to local planner logic onto + """ + self._vehicle = agent.vehicle + self._map = agent.vehicle.get_world().get_map() + + self._target_speed = None + self.sampling_radius = None + self._min_distance = None + self._current_waypoint = None + self.target_road_option = None + self._next_waypoints = None + self.target_waypoint = None + self._vehicle_controller = None + self._global_plan = None + self._pid_controller = None + self.waypoints_queue = deque(maxlen=20000) # queue with tuples of (waypoint, RoadOption) + self._buffer_size = 5 + self._waypoint_buffer = deque(maxlen=self._buffer_size) + + self._init_controller() # initializing controller + + def get_distance_to_nearest_obstacle(self): + ego_vehicle_transform = self._vehicle.get_transform() + ego_vehicle_location = ego_vehicle_transform.location + + # Get the bounding box of the ego vehicle + ego_vehicle_bbox = self._vehicle.bounding_box + + # Calculate the minimum distance to other vehicles + distance_to_nearest_obstacle = None + + for actor in self._vehicle.get_world().get_actors().filter('vehicle.*'): + if actor.id != self._vehicle.id: + actor_transform = actor.get_transform() + actor_location = actor_transform.location + + # Calculate the distance between the ego vehicle and the other vehicle + distance = ego_vehicle_location.distance(actor_location) + + # Calculate the minimum distance to the other vehicle's bounding box + distance_to_obstacle = distance - ego_vehicle_bbox.extent.x - actor.bounding_box.extent.x + + if distance_to_nearest_obstacle is None or distance_to_obstacle < distance_to_nearest_obstacle: + distance_to_nearest_obstacle = distance_to_obstacle + + + return distance_to_nearest_obstacle + + def reset_vehicle(self): + """Reset the ego-vehicle""" + self._vehicle = None + print("Resetting ego-vehicle!") + + def _init_controller(self): + """ + Controller initialization. + + dt -- time difference between physics control in seconds. + This is can be fixed from server side + using the arguments -benchmark -fps=F, since dt = 1/F + + target_speed -- desired cruise speed in km/h + + min_distance -- minimum distance to remove waypoint from queue + + lateral_dict -- dictionary of arguments to setup the lateral PID controller + {'K_P':, 'K_D':, 'K_I':, 'dt'} + + longitudinal_dict -- dictionary of arguments to setup the longitudinal PID controller + {'K_P':, 'K_D':, 'K_I':, 'dt'} + """ + # Default parameters + self.args_lat_hw_dict = { + 'K_P': 0.75, + 'K_D': 0.02, + 'K_I': 0.4, + 'dt': 1.0 / self.FPS} + self.args_lat_city_dict = { + 'K_P': 0.58, + 'K_D': 0.02, + 'K_I': 0.5, + 'dt': 1.0 / self.FPS} + self.args_long_hw_dict = { + 'K_P': 0.37, + 'K_D': 0.024, + 'K_I': 0.032, + 'dt': 1.0 / self.FPS} + self.args_long_city_dict = { + 'K_P': 0.15, + 'K_D': 0.05, + 'K_I': 0.07, + 'dt': 1.0 / self.FPS} + + self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) + + self._global_plan = False + + self._target_speed = self._vehicle.get_speed_limit() + + self._min_distance = 3 + + def set_speed(self, speed): + """ + Request new target speed. + + :param speed: new target speed in km/h + """ + + self._target_speed = speed + + def set_global_plan(self, current_plan): + """ + Sets new global plan. + + :param current_plan: list of waypoints in the actual plan + """ + for elem in current_plan: + self.waypoints_queue.append(elem) + self._global_plan = True + + def get_incoming_waypoint_and_direction(self, steps=3): + """ + Returns direction and waypoint at a distance ahead defined by the user. + + :param steps: number of steps to get the incoming waypoint. + """ + if len(self.waypoints_queue) > steps: + return self.waypoints_queue[steps] + + else: + try: + wpt, direction = self.waypoints_queue[-1] + return wpt, direction + except IndexError as i: + print(i) + return None, RoadOption.VOID + return None, RoadOption.VOID + + def run_step(self, target_speed=None, debug=False): + """ + Execute one step of local planning which involves + running the longitudinal and lateral PID controllers to + follow the waypoints trajectory. + + :param target_speed: desired speed + :param debug: boolean flag to activate waypoints debugging + :return: control + """ + + if target_speed is not None: + self._target_speed = target_speed + else: + self._target_speed = self._vehicle.get_speed_limit() + + if len(self.waypoints_queue) == 0: + control = carla.VehicleControl() + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 1.0 + control.hand_brake = False + control.manual_gear_shift = False + return control + + # Buffering the waypoints + if not self._waypoint_buffer: + for i in range(self._buffer_size): + if self.waypoints_queue: + self._waypoint_buffer.append( + self.waypoints_queue.popleft()) + else: + break + + # Current vehicle waypoint + self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) + + # Target waypoint + self.target_waypoint, self.target_road_option = self._waypoint_buffer[0] + + if target_speed > 50: + args_lat = self.args_lat_hw_dict + args_long = self.args_long_hw_dict + else: + args_lat = self.args_lat_city_dict + args_long = self.args_long_city_dict + + self._pid_controller = VehiclePIDController(self._vehicle, + args_lateral=args_lat, + args_longitudinal=args_long) + + control = self._pid_controller.run_step(self._target_speed, self.target_waypoint) + + # Purge the queue of obsolete waypoints + vehicle_transform = self._vehicle.get_transform() + max_index = -1 + + for i, (waypoint, _) in enumerate(self._waypoint_buffer): + if distance_vehicle( + waypoint, vehicle_transform) < self._min_distance: + max_index = i + if max_index >= 0: + for i in range(max_index + 1): + self._waypoint_buffer.popleft() + + if debug: + draw_waypoints(self._vehicle.get_world(), + [self.target_waypoint], 1.0) + return control + + +# ============================================================================== +# -- KK Agent Class --------------------------------------------------------- +# ============================================================================== + +class AgentState(Enum): + """ + AGENT_STATE represents the possible states of a roaming agent + """ + NAVIGATING = 1 + BLOCKED_BY_VEHICLE = 2 + BLOCKED_RED_LIGHT = 3 + + +class Agent(object): + """Base class to define agents in CARLA""" + + def __init__(self, vehicle): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + """ + self._vehicle = vehicle + self._proximity_tlight_threshold = 5.0 # meters + self._proximity_vehicle_threshold = 10.0 # meters + self._local_planner = None + self._world = self._vehicle.get_world() + try: + self._map = self._world.get_map() + except RuntimeError as error: + print('RuntimeError: {}'.format(error)) + print(' The server could not send the OpenDRIVE (.xodr) file:') + print(' Make sure it exists, has the same name of your town, and is correct.') + sys.exit(1) + self._last_traffic_light = None + + def get_local_planner(self): + """Get method for protected member local planner""" + return self._local_planner + + @staticmethod + def run_step(debug=False): + """ + Execute one step of navigation. + + :param debug: boolean flag for debugging + :return: control + """ + control = carla.VehicleControl() + + if debug: + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 0.0 + control.hand_brake = False + control.manual_gear_shift = False + + return control + + @staticmethod + def emergency_stop(): + """ + Send an emergency stop command to the vehicle + + :return: control for braking + """ + control = carla.VehicleControl() + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 1.0 + control.hand_brake = False + + return control + + +# ============================================================================== +# -- KK Behavior Agent Class --------------------------------------------------------- +# ============================================================================== + +class BehaviorAgent(Agent): + """ + BehaviorAgent implements an agent that navigates scenes to reach a given + target destination, by computing the shortest possible path to it. + This agent can correctly follow traffic signs, speed limitations, + traffic lights, while also taking into account nearby vehicles. Lane changing + decisions can be taken by analyzing the surrounding environment, + such as overtaking or tailgating avoidance. Adding to these are possible + behaviors, the agent can also keep safety distance from a car in front of it + by tracking the instantaneous time to collision and keeping it in a certain range. + Finally, different sets of behaviors are encoded in the agent, from cautious + to a more aggressive ones. + """ + + def __init__(self, vehicle, ignore_traffic_light=False, behavior='normal'): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param ignore_traffic_light: boolean to ignore any traffic light + :param behavior: type of agent to apply + """ + + super(BehaviorAgent, self).__init__(vehicle) + self.vehicle = vehicle + self.ignore_traffic_light = ignore_traffic_light + self._local_planner = LocalPlanner(self) + self._grp = None + self.look_ahead_steps = 0 + + # Vehicle information + self.speed = 0 + self.speed_limit = 0 + self.direction = None + self.incoming_direction = None + self.incoming_waypoint = None + self.start_waypoint = None + self.end_waypoint = None + self.is_at_traffic_light = 0 + self.light_state = "Green" + self.light_id_to_ignore = -1 + self.min_speed = 5 + self.behavior = None + self._sampling_resolution = 4.5 + + # Parameters for agent behavior + if behavior == 'cautious': + self.behavior = Cautious() + + elif behavior == 'normal': + self.behavior = Normal() + + elif behavior == 'aggressive': + self.behavior = Aggressive() + + def update_information(self, world): + """ + This method updates the information regarding the ego + vehicle based on the surrounding world. + + :param world: carla.world object + """ + self.speed = get_speed(self.vehicle) + self.speed_limit = world.player.get_speed_limit() + # self.speed_limit = 100 # KK set speed to 100 km/h + self._local_planner.set_speed(self.speed_limit) + self.direction = self._local_planner.target_road_option + if self.direction is None: + self.direction = RoadOption.LANEFOLLOW + + self.look_ahead_steps = int(self.speed_limit / 10) + + self.incoming_waypoint, self.incoming_direction = self._local_planner.get_incoming_waypoint_and_direction( + steps=self.look_ahead_steps) + if self.incoming_direction is None: + self.incoming_direction = RoadOption.LANEFOLLOW + + self.is_at_traffic_light = world.player.is_at_traffic_light() + if self.ignore_traffic_light: + self.light_state = "Green" + else: + if self.is_at_traffic_light: + traffic_light = world.player.get_traffic_light() + self.light_state = str(traffic_light.state) + if self.light_id_to_ignore != -1 and traffic_light.id == self.light_id_to_ignore: + # Ignore this traffic light for this iteration + self.light_state = "Green" + # Get distance to the nearest obstacle + distance_to_nearest_obstacle = self._local_planner.get_distance_to_nearest_obstacle() + + # Check if the obstacle is closer than the minimum distance + if distance_to_nearest_obstacle is not None and distance_to_nearest_obstacle < 3.0: + # Apply braking if the distance to the nearest obstacle is below the threshold + self._local_planner.brake = 1.0 + else: + self._local_planner.brake = 0.0 + #else: + # This method also includes stop signs and intersections. + #self.light_state = str(self.vehicle.get_traffic_light_state()) + + def set_destination(self, start_location, end_location, clean=False): + """ + This method creates a list of waypoints from agent's position to destination location + based on the route returned by the global router. + + :param start_location: initial position + :param end_location: final position + :param clean: boolean to clean the waypoint queue + """ + if clean: + self._local_planner.waypoints_queue.clear() + self.start_waypoint = self._map.get_waypoint(start_location) + self.end_waypoint = self._map.get_waypoint(end_location) + + route_trace = self._trace_route(self.start_waypoint, self.end_waypoint) + + self._local_planner.set_global_plan(route_trace) + + def reroute(self, spawn_points): + """ + This method implements re-routing for vehicles approaching its destination. + It finds a new target and computes another path to reach it. + + :param spawn_points: list of possible destinations for the agent + """ + + print("Target almost reached, setting new destination...") + random.shuffle(spawn_points) + new_start = self._local_planner.waypoints_queue[-1][0].transform.location + destination = spawn_points[0].location if spawn_points[0].location != new_start else spawn_points[1].location + print("New destination: " + str(destination)) + + self.set_destination(new_start, destination) + + def _trace_route(self, start_waypoint, end_waypoint): + """ + This method sets up a global router and returns the + optimal route from start_waypoint to end_waypoint. + + :param start_waypoint: initial position + :param end_waypoint: final position + """ + # Setting up global router + if self._grp is None: + wld = self.vehicle.get_world() + dao = GlobalRoutePlannerDAO( + wld.get_map(), sampling_resolution=self._sampling_resolution) + grp = GlobalRoutePlanner(dao) + grp.setup() + self._grp = grp + + # Obtain route plan + route = self._grp.trace_route( + start_waypoint.transform.location, + end_waypoint.transform.location) + + return route + + def run_step(self, debug=False): + """ + Execute one step of navigation. + + :param debug: boolean for debugging + :return control: carla.VehicleControl + """ + control = carla.VehicleControl() + + ego_vehicle_loc = self.vehicle.get_location() + ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) + + if self.is_at_traffic_light and self.light_state != "Green": + # Stop at red traffic light + control = carla.VehicleControl() + control.throttle = 0.0 + control.brake = 1.0 + control.steer = 0.0 + else: + # Get distance to the nearest obstacle + distance_to_nearest_obstacle = self._local_planner.get_distance_to_nearest_obstacle() + + # Check if the obstacle is closer than the minimum distance + if distance_to_nearest_obstacle is not None and distance_to_nearest_obstacle < 3.0: + # Apply braking if the distance to the nearest obstacle is below the threshold + control.throttle = 0.0 + control.brake = 1.0 + control.steer = 0.0 + else: + # Continue following the planned route + control = self._local_planner.run_step( + target_speed=min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist), debug=debug) + self.vehicle.apply_control(control) + + return control + + +# ============================================================================== +# -- Game Loop --------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + """ Main loop for agent""" + + pygame.init() + pygame.font.init() + world = None + tot_target_reached = 0 + num_min_waypoints = 21 + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(70.0) + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + + hud = HUD(args.width, args.height) + + #selected_world = client.load_world("Town01") # KK Changes world to Town04 + selected_world = client.get_world() + + world = World(client.get_world(), hud, args) + controller = KeyboardControl(world) + + agent = BehaviorAgent(world.player, behavior=args.behavior) + + spawn_points = world.map.get_spawn_points() + random.shuffle(spawn_points) + + if spawn_points[0].location != agent.vehicle.get_location(): + destination = spawn_points[0].location + else: + destination = spawn_points[1].location + + agent.set_destination(agent.vehicle.get_location(), destination, clean=True) + + clock = pygame.time.Clock() + + while True: + clock.tick_busy_loop(60) + if controller.parse_events(world): + return + + # As soon as the server is ready continue! + if not world.world.wait_for_tick(10.0): + continue + + if args.agent == "Roaming" or args.agent == "Basic": + if controller.parse_events(world): + return + + #agent.update_information(world) + # as soon as the server is ready continue! + world.world.wait_for_tick(10.0) + + world.tick(clock) + world.render(display) + pygame.display.flip() + control = agent.run_step() + control.manual_gear_shift = False + world.player.apply_control(control) + else: + agent.update_information(world) + + world.tick(clock) + world.render(display) + pygame.display.flip() + + + # Set new destination when target has been reached + if len(agent.get_local_planner().waypoints_queue) < num_min_waypoints and args.loop: + agent.reroute(spawn_points) + tot_target_reached += 1 + world.hud.notification("The target has been reached " + + str(tot_target_reached) + " times.", seconds=4.0) + + elif len(agent.get_local_planner().waypoints_queue) == 0 and not args.loop: + print("Target reached, mission accomplished...") + break + + speed_limit = world.player.get_speed_limit() + agent.get_local_planner().set_speed(speed_limit) + + control = agent.run_step() + world.player.apply_control(control) + + finally: + if world is not None: + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------- +# ============================================================================== + + +def main(): + """Main method""" + + argparser = argparse.ArgumentParser( + description='CARLA Automatic Control Client') + argparser.add_argument( + '-v', '--verbose', + action='store_true', + dest='debug', + help='Print debug information') + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '--res', + metavar='WIDTHxHEIGHT', + default='1280x720', + help='Window resolution (default: 1280x720)') + argparser.add_argument( + '--filter', + metavar='PATTERN', + default='vehicle.*', + help='Actor filter (default: "vehicle.*")') + argparser.add_argument( + '--gamma', + default=2.2, + type=float, + help='Gamma correction of the camera (default: 2.2)') + argparser.add_argument( + '-l', '--loop', + action='store_true', + dest='loop', + help='Sets a new random destination upon reaching the previous one (default: False)') + argparser.add_argument( + '-b', '--behavior', type=str, + choices=["cautious", "normal", "aggressive"], + help='Choose one of the possible agent behaviors (default: normal) ', + default='normal') + argparser.add_argument("-a", "--agent", type=str, + choices=["Behavior", "Roaming", "Basic"], + help="select which agent to run", + default="Behavior") + argparser.add_argument( + '-s', '--seed', + help='Set seed for repeating executions (default: None)', + default=None, + type=int) + + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split('x')] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + + logging.info('listening to server %s:%s', args.host, args.port) + + print(__doc__) + + try: + game_loop(args) + + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + + +if __name__ == '__main__': + main()# TEST MODIFICATION - 12/18/2025 13:26:06 + \ No newline at end of file From 1d2435b84c729259d619979f106b4d1f2e475f58 Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Thu, 18 Dec 2025 15:02:39 +0800 Subject: [PATCH 13/13] =?UTF-8?q?=E5=9C=A8=E4=BB=A3=E7=A0=81=E5=B0=BE?= =?UTF-8?q?=E9=83=A8=E8=A7=A3=E5=86=B3=E4=BA=86=E9=87=8D=E5=A4=8D=E8=B0=83?= =?UTF-8?q?=E7=94=A8=E7=9A=84=E9=97=AE=E9=A2=98=EF=BC=8C=E5=87=8F=E5=B0=91?= =?UTF-8?q?=E4=BA=86=E8=BF=90=E8=A1=8C=E6=97=B6=E9=97=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../object_detection.py | 3853 ++++++++--------- 1 file changed, 1912 insertions(+), 1941 deletions(-) diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py index 59c8ce8da7..2a0df72485 100644 --- a/src/Object_Tracking_Planning/object_detection.py +++ b/src/Object_Tracking_Planning/object_detection.py @@ -1,1941 +1,1912 @@ -"""Example of automatic vehicle control from client side.""" - -from __future__ import print_function - -import sys -import os -# 获取当前脚本所在的目录,并找到agents文件夹的路径 -current_dir = os.path.dirname(os.path.abspath(__file__)) -agents_path = os.path.join(current_dir, 'agents') -# 将该路径添加到模块搜索路径中 -if os.path.exists(agents_path): - sys.path.insert(0, agents_path) -import argparse -import collections -import datetime -import glob -import logging -import math -import os -import cv2 -import random -import re -import sys -import weakref -from enum import Enum -from collections import deque - - -try: - import pygame - from pygame.locals import KMOD_CTRL - from pygame.locals import K_ESCAPE - from pygame.locals import K_q - from pygame.locals import K_l - from pygame.locals import K_v - from pygame.locals import K_c -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError( - 'cannot import numpy, make sure numpy package is installed') - -# ============================================================================== -# -- Find CARLA module --------------------------------------------------------- -# ============================================================================== -try: - sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( - sys.version_info.major, - sys.version_info.minor, - 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) -except IndexError: - pass - -# ============================================================================== -# -- Add PythonAPI for release mode -------------------------------------------- -# ============================================================================== -try: - sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + '/carla') -except IndexError: - pass - -import carla -from carla import ColorConverter as cc -from torch.utils.tensorboard import SummaryWriter -import torch -#import tensorflow as tf - -# Create a SummaryWriter object to write the metrics to Tensorboard -writer = SummaryWriter() - -from agents.navigation.global_route_planner import GlobalRoutePlanner -from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO -from agents.navigation.types_behavior import Cautious, Aggressive, Normal - -from agents.tools.misc import get_speed, positive, draw_waypoints, distance_vehicle - - -# ============================================================================== -# -- Global functions ---------------------------------------------------------- -# ============================================================================== - - -def find_weather_presets(): - """Method to find weather presets""" - rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') - def name(x): return ' '.join(m.group(0) for m in rgx.finditer(x)) - presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] - return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] - - -def get_actor_display_name(actor, truncate=250): - """Method to get actor display name""" - name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) - return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name - - -# ============================================================================== -# -- World --------------------------------------------------------------- -# ============================================================================== - -class World(object): - """ Class representing the surrounding environment """ - - def __init__(self, carla_world, hud, args): - """Constructor method""" - self.world = carla_world - try: - self.map = self.world.get_map() - except RuntimeError as error: - print('RuntimeError: {}'.format(error)) - print(' The server could not send the OpenDRIVE (.xodr) file:') - print(' Make sure it exists, has the same name of your town, and is correct.') - sys.exit(1) - self.hud = hud - self.player = None - self.collision_sensor = None - self.lane_invasion_sensor = None - self.gnss_sensor = None - self.camera_manager = None - self._weather_presets = find_weather_presets() - self._weather_index = 0 - self._actor_filter = args.filter - self._gamma = args.gamma - self.restart(args) - self.world.on_tick(hud.on_world_tick) - self.recording_enabled = False - self.recording_start = 0 - - def restart(self, args): - """Restart the world""" - # Keep same camera config if the camera manager exists. - cam_index = self.camera_manager.index if self.camera_manager is not None else 0 - cam_pos_id = self.camera_manager.transform_index if self.camera_manager is not None else 0 - # Set the seed if requested by user - if args.seed is not None: - random.seed(args.seed) - - # KK Get a vehicle blueprint - #blueprint = self.world.get_blueprint_library().filter('vehicle.lincoln.mkz2017')[0] # KK - blueprint = self.world.get_blueprint_library().filter('model3')[0] - blueprint.set_attribute('role_name', 'hero') - if blueprint.has_attribute('color'): - color = '236,102,17' # KK Packt logo color - blueprint.set_attribute('color', color) - # Spawn the player. - print("Spawning the player") - if self.player is not None: - spawn_point = self.player.get_transform() - spawn_point.location.z += 2.0 - spawn_point.rotation.roll = 0.0 - spawn_point.rotation.pitch = 0.0 - self.destroy() - self.player = self.world.try_spawn_actor(blueprint, spawn_point) - - while self.player is None: - if not self.map.get_spawn_points(): - print('There are no spawn points available in your map/town.') - print('Please add some Vehicle Spawn Point to your UE4 scene.') - sys.exit(1) - spawn_points = self.map.get_spawn_points() - spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() - #spawn_point = spawn_points[0] if spawn_points else carla.Transform() # KK - self.player = self.world.try_spawn_actor(blueprint, spawn_point) - # Set up the sensors. - self.collision_sensor = CollisionSensor(self.player, self.hud) - self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) - self.gnss_sensor = GnssSensor(self.player) - self.camera_manager = CameraManager(self.player, self.hud, self._gamma) - self.camera_manager.transform_index = cam_pos_id - self.camera_manager.set_sensor(cam_index, notify=False) - actor_type = get_actor_display_name(self.player) - self.hud.notification(actor_type) - def next_weather(self, reverse=False): - """Get next weather setting""" - self._weather_index += -1 if reverse else 1 - self._weather_index %= len(self._weather_presets) - preset = self._weather_presets[self._weather_index] - self.hud.notification('Weather: %s' % preset[1]) - self.player.get_world().set_weather(preset[0]) - - def tick(self, clock): - """Method for every tick""" - self.hud.tick(self, clock) - - def render(self, display): - """Render world""" - self.camera_manager.render(display) - self.hud.render(display) - - def destroy_sensors(self): - """Destroy sensors""" - self.camera_manager.sensor.destroy() - self.camera_manager.sensor = None - self.camera_manager.index = None - - def destroy(self): - """Destroys all actors""" - actors = [ - self.camera_manager.sensor, - self.collision_sensor.sensor, - self.lane_invasion_sensor.sensor, - self.gnss_sensor.sensor, - self.player] - for actor in actors: - if actor is not None: - actor.destroy() - - -# ============================================================================== -# -- KeyboardControl ----------------------------------------------------------- -# ============================================================================== - - -class KeyboardControl(object): - def __init__(self, world, start_locked=True): - world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) - # 可选:启动时自动锁定 - if start_locked and world.camera_manager is not None: - world.camera_manager.set_lock_mode(True) - world.camera_manager.hud.notification("Camera Lock: ON", seconds=2.0) - - def parse_events(self, world): - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return True - if event.type == pygame.KEYUP: - if self._is_quit_shortcut(event.key): - return True - elif event.key == K_l: # 按L键切换锁定 - if world.camera_manager is not None: - world.camera_manager.toggle_lock() - elif event.key == K_v: # 按V键切换锁定视角 - if world.camera_manager is not None and world.camera_manager.lock_mode: - world.camera_manager.next_lock_view() - elif event.key == K_c: # 按C键切换相机(原有功能) - if world.camera_manager is not None: - world.camera_manager.toggle_camera() - - @staticmethod - def _is_quit_shortcut(key): - """Shortcut for quitting""" - return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) - -# ============================================================================== -# -- HUD ----------------------------------------------------------------------- -# ============================================================================== - - -class HUD(object): - """Class for HUD text""" - - def __init__(self, width, height): - """Constructor method""" - self.dim = (width, height) - font = pygame.font.Font(pygame.font.get_default_font(), 20) - font_name = 'courier' if os.name == 'nt' else 'mono' - fonts = [x for x in pygame.font.get_fonts() if font_name in x] - default_font = 'ubuntumono' - mono = default_font if default_font in fonts else fonts[0] - mono = pygame.font.match_font(mono) - self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) - self._notifications = FadingText(font, (width, 40), (0, height - 40)) - self.help = HelpText(pygame.font.Font(mono, 24), width, height) - self.server_fps = 0 - self.frame = 0 - self.simulation_time = 0 - self._show_info = True - self._info_text = [] - self._server_clock = pygame.time.Clock() - - def on_world_tick(self, timestamp): - """Gets informations from the world at every tick""" - self._server_clock.tick() - self.server_fps = self._server_clock.get_fps() - self.frame = timestamp.frame_count - self.simulation_time = timestamp.elapsed_seconds - - def tick(self, world, clock): - """HUD method for every tick""" - self._notifications.tick(world, clock) - if not self._show_info: - return - transform = world.player.get_transform() - vel = world.player.get_velocity() - control = world.player.get_control() - heading = 'N' if abs(transform.rotation.yaw) < 89.5 else '' - heading += 'S' if abs(transform.rotation.yaw) > 90.5 else '' - heading += 'E' if 179.5 > transform.rotation.yaw > 0.5 else '' - heading += 'W' if -0.5 > transform.rotation.yaw > -179.5 else '' - colhist = world.collision_sensor.get_collision_history() - collision = [colhist[x + self.frame - 200] for x in range(0, 200)] - max_col = max(1.0, max(collision)) - collision = [x / max_col for x in collision] - vehicles = world.world.get_actors().filter('vehicle.*') - - self._info_text = [ - 'Server: % 16.0f FPS' % self.server_fps, - 'Client: % 16.0f FPS' % clock.get_fps(), - '', - 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), - 'Map: % 20s' % world.map.name, - 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), - '', - 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)), - u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (transform.rotation.yaw, heading), - 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (transform.location.x, transform.location.y)), - 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), - 'Height: % 18.0f m' % transform.location.z, - ''] - if isinstance(control, carla.VehicleControl): - self._info_text += [ - ('Throttle:', control.throttle, 0.0, 1.0), - ('Steer:', control.steer, -1.0, 1.0), - ('Brake:', control.brake, 0.0, 1.0), - ('Reverse:', control.reverse), - ('Hand brake:', control.hand_brake), - ('Manual:', control.manual_gear_shift), - 'Gear: %s' % {-1: 'R', 0: 'N'}.get(control.gear, control.gear)] - elif isinstance(control, carla.WalkerControl): - self._info_text += [ - ('Speed:', control.speed, 0.0, 5.556), - ('Jump:', control.jump)] - self._info_text += [ - '', - 'Collision:', - collision, - '', - 'Number of vehicles: % 8d' % len(vehicles)] - - if len(vehicles) > 1: - self._info_text += ['Nearby vehicles:'] - - def dist(l): - return math.sqrt((l.x - transform.location.x)**2 + (l.y - transform.location.y) - ** 2 + (l.z - transform.location.z)**2) - vehicles = [(dist(x.get_location()), x) for x in vehicles if x.id != world.player.id] - - for dist, vehicle in sorted(vehicles): - if dist > 200.0: - break - vehicle_type = get_actor_display_name(vehicle, truncate=22) - self._info_text.append('% 4dm %s' % (dist, vehicle_type)) - - def toggle_info(self): - """Toggle info on or off""" - self._show_info = not self._show_info - - def notification(self, text, seconds=2.0): - """Notification text""" - self._notifications.set_text(text, seconds=seconds) - - def error(self, text): - """Error text""" - self._notifications.set_text('Error: %s' % text, (255, 0, 0)) - - def render(self, display): - """Render for HUD class""" - if self._show_info: - info_surface = pygame.Surface((220, self.dim[1])) - info_surface.set_alpha(100) - display.blit(info_surface, (0, 0)) - v_offset = 4 - bar_h_offset = 100 - bar_width = 106 - for item in self._info_text: - if v_offset + 18 > self.dim[1]: - break - if isinstance(item, list): - if len(item) > 1: - points = [(x + 8, v_offset + 8 + (1 - y) * 30) for x, y in enumerate(item)] - pygame.draw.lines(display, (255, 136, 0), False, points, 2) - item = None - v_offset += 18 - elif isinstance(item, tuple): - if isinstance(item[1], bool): - rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) - pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) - else: - rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) - pygame.draw.rect(display, (255, 255, 255), rect_border, 1) - fig = (item[1] - item[2]) / (item[3] - item[2]) - if item[2] < 0.0: - rect = pygame.Rect( - (bar_h_offset + fig * (bar_width - 6), v_offset + 8), (6, 6)) - else: - rect = pygame.Rect((bar_h_offset, v_offset + 8), (fig * bar_width, 6)) - pygame.draw.rect(display, (255, 255, 255), rect) - item = item[0] - if item: # At this point has to be a str. - surface = self._font_mono.render(item, True, (255, 255, 255)) - display.blit(surface, (8, v_offset)) - v_offset += 18 - self._notifications.render(display) - self.help.render(display) - -# ============================================================================== -# -- FadingText ---------------------------------------------------------------- -# ============================================================================== - - -class FadingText(object): - """ Class for fading text """ - - def __init__(self, font, dim, pos): - """Constructor method""" - self.font = font - self.dim = dim - self.pos = pos - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - - def set_text(self, text, color=(255, 255, 255), seconds=2.0): - """Set fading text""" - text_texture = self.font.render(text, True, color) - self.surface = pygame.Surface(self.dim) - self.seconds_left = seconds - self.surface.fill((0, 0, 0, 0)) - self.surface.blit(text_texture, (10, 11)) - - def tick(self, _, clock): - """Fading text method for every tick""" - delta_seconds = 1e-3 * clock.get_time() - self.seconds_left = max(0.0, self.seconds_left - delta_seconds) - self.surface.set_alpha(500.0 * self.seconds_left) - - def render(self, display): - """Render fading text method""" - display.blit(self.surface, self.pos) - -# ============================================================================== -# -- HelpText ------------------------------------------------------------------ -# ============================================================================== - - -class HelpText(object): - """ Helper class for text render""" - - def __init__(self, font, width, height): - """Constructor method""" - lines = [ - 'Welcome to CARLA automatic control client', - '', - 'Camera Control:', - ' C : Change camera position', - ' L : Toggle camera lock on vehicle', - ' V : Change lock view (when locked)', - ' PageUp : Toggle recording', - '', - 'Vehicle Control:', - ' W : Throttle', - ' S : Brake', - ' A/D : Steer left/right', - ' Q : Toggle reverse', - ' P : Toggle autopilot', - '', - 'Miscellaneous:', - ' H : Toggle help', - ' ESC : Quit', - ' CTRL + Q : Quit', - ] - - self.font = font - self.dim = (680, len(lines) * 22 + 12) - self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - self.surface.fill((0, 0, 0, 0)) - for i, line in enumerate(lines): - text_texture = self.font.render(line, True, (255, 255, 255)) - self.surface.blit(text_texture, (22, i * 22)) - self._render = False - self.surface.set_alpha(220) - - def toggle(self): - """Toggle on or off the render help""" - self._render = not self._render - - def render(self, display): - """Render help text method""" - if self._render: - display.blit(self.surface, self.pos) - -# ============================================================================== -# -- CollisionSensor ----------------------------------------------------------- -# ============================================================================== - - -class CollisionSensor(object): - """ Class for collision sensors""" - - def __init__(self, parent_actor, hud): - """Constructor method""" - self.sensor = None - self.history = [] - self._parent = parent_actor - self.hud = hud - world = self._parent.get_world() - blueprint = world.get_blueprint_library().find('sensor.other.collision') - self.sensor = world.spawn_actor(blueprint, carla.Transform(), attach_to=self._parent) - # We need to pass the lambda a weak reference to - # self to avoid circular reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) - - def get_collision_history(self): - """Gets the history of collisions""" - history = collections.defaultdict(int) - for frame, intensity in self.history: - history[frame] += intensity - return history - - @staticmethod - def _on_collision(weak_self, event): - """On collision method""" - self = weak_self() - if not self: - return - actor_type = get_actor_display_name(event.other_actor) - self.hud.notification('Collision with %r' % actor_type) - impulse = event.normal_impulse - intensity = math.sqrt(impulse.x ** 2 + impulse.y ** 2 + impulse.z ** 2) - self.history.append((event.frame, intensity)) - if len(self.history) > 4000: - self.history.pop(0) - -# ============================================================================== -# -- LaneInvasionSensor -------------------------------------------------------- -# ============================================================================== - - -class LaneInvasionSensor(object): - """Class for lane invasion sensors""" - - def __init__(self, parent_actor, hud): - """Constructor method""" - self.sensor = None - self._parent = parent_actor - self.hud = hud - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.lane_invasion') - self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) - - @staticmethod - def _on_invasion(weak_self, event): - """On invasion method""" - self = weak_self() - if not self: - return - lane_types = set(x.type for x in event.crossed_lane_markings) - text = ['%r' % str(x).split()[-1] for x in lane_types] - self.hud.notification('Crossed line %s' % ' and '.join(text)) - -# ============================================================================== -# -- GnssSensor -------------------------------------------------------- -# ============================================================================== - - -class GnssSensor(object): - """ Class for GNSS sensors""" - - def __init__(self, parent_actor): - """Constructor method""" - self.sensor = None - self._parent = parent_actor - self.lat = 0.0 - self.lon = 0.0 - world = self._parent.get_world() - blueprint = world.get_blueprint_library().find('sensor.other.gnss') - self.sensor = world.spawn_actor(blueprint, carla.Transform(carla.Location(x=1.0, z=2.8)), - attach_to=self._parent) - # We need to pass the lambda a weak reference to - # self to avoid circular reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) - - @staticmethod - def _on_gnss_event(weak_self, event): - """GNSS method""" - self = weak_self() - if not self: - return - self.lat = event.latitude - self.lon = event.longitude - -# ============================================================================== -# -- CameraManager ------------------------------------------------------------- -# ============================================================================== - - -class CameraManager(object): - """ Class for camera management""" - - def __init__(self, parent_actor, hud, gamma_correction): - """Constructor method""" - self.sensor = None - self.surface = None - self._parent = parent_actor - self.hud = hud - self.recording = False - bound_y = 0.5 + self._parent.bounding_box.extent.y - attachment = carla.AttachmentType - self._camera_transforms = [ - (carla.Transform( - carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8.0)), attachment.SpringArm), - (carla.Transform( - carla.Location(x=1.6, z=1.7)), attachment.Rigid), - (carla.Transform( - carla.Location(x=5.5, y=1.5, z=1.5)), attachment.SpringArm), - (carla.Transform( - carla.Location(x=-8.0, z=6.0), carla.Rotation(pitch=6.0)), attachment.SpringArm), - (carla.Transform( - carla.Location(x=-1, y=-bound_y, z=0.5)), attachment.Rigid)] - self.transform_index = 1 - self.sensors = [ - ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], - ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], - ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], - ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], - ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], - ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, - 'Camera Semantic Segmentation (CityScapes Palette)'], - ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] - - # 添加锁定模式相关属性 - self.lock_mode = True # 默认启用锁定模式 - self.lock_target = parent_actor # 锁定目标 - self.lock_transform_index = 0 # 锁定模式的相机位置索引 - - # 定义锁定模式的相机变换(可以自定义不同角度) - self._lock_transforms = [ - (carla.Transform( - carla.Location(x=-6.0, z=3.5), carla.Rotation(pitch=-10.0)), attachment.SpringArm), - (carla.Transform( - carla.Location(x=2.0, z=1.5), carla.Rotation(pitch=0.0)), attachment.Rigid), - (carla.Transform( - carla.Location(x=5.0, y=2.0, z=2.0), carla.Rotation(pitch=-5.0)), attachment.SpringArm), - (carla.Transform( - carla.Location(x=-10.0, z=5.0), carla.Rotation(pitch=-15.0)), attachment.SpringArm), - ] - - world = self._parent.get_world() - bp_library = world.get_blueprint_library() - for item in self.sensors: - blp = bp_library.find(item[0]) - if item[0].startswith('sensor.camera'): - blp.set_attribute('image_size_x', str(hud.dim[0])) - blp.set_attribute('image_size_y', str(hud.dim[1])) - if blp.has_attribute('gamma'): - blp.set_attribute('gamma', str(gamma_correction)) - elif item[0].startswith('sensor.lidar'): - blp.set_attribute('range', '50') - item.append(blp) - self.index = None - - def toggle_camera(self): - """Activate a camera""" - self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) - self.set_sensor(self.index, notify=False, force_respawn=True) - - def set_sensor(self, index, notify=True, force_respawn=False): - """Set a sensor""" - index = index % len(self.sensors) - needs_respawn = True if self.index is None else ( - force_respawn or (self.sensors[index][0] != self.sensors[self.index][0])) - if needs_respawn: - if self.sensor is not None: - self.sensor.destroy() - self.surface = None - - # 选择变换和附着类型 - if self.lock_mode: - # 锁定模式使用锁定变换 - transform, attachment_type = self._lock_transforms[self.lock_transform_index] - else: - # 非锁定模式使用原有变换 - transform, attachment_type = self._camera_transforms[self.transform_index] - - self.sensor = self._parent.get_world().spawn_actor( - self.sensors[index][-1], - transform, - attach_to=self._parent, - attachment_type=attachment_type) - - # We need to pass the lambda a weak reference to - # self to avoid circular reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) - if notify: - sensor_name = self.sensors[index][2] - if self.lock_mode: - sensor_name += " (Locked)" - self.hud.notification(sensor_name) - self.index = index - - def next_sensor(self): - """Get the next sensor""" - self.set_sensor(self.index + 1) - - def toggle_recording(self): - """Toggle recording on or off""" - self.recording = not self.recording - self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) - - def toggle_lock(self): - """切换锁定模式""" - self.lock_mode = not self.lock_mode - mode_text = "Camera Lock: ON" if self.lock_mode else "Camera Lock: OFF" - self.hud.notification(mode_text, seconds=2.0) - # 重新设置传感器以应用锁定模式 - if self.sensor is not None: - self.set_sensor(self.index, notify=False, force_respawn=True) - return self.lock_mode - - def set_lock_mode(self, enable=True): - """设置锁定模式""" - self.lock_mode = enable - return self.lock_mode - - def next_lock_view(self): - """切换到下一个锁定视角""" - if self.lock_mode: - self.lock_transform_index = (self.lock_transform_index + 1) % len(self._lock_transforms) - self.hud.notification(f'Lock View {self.lock_transform_index + 1}', seconds=1.0) - # 重新设置传感器以应用新的锁定视角 - if self.sensor is not None: - self.set_sensor(self.index, notify=False, force_respawn=True) - - def render(self, display): - """Render method""" - if self.surface is not None: - display.blit(self.surface, (0, 0)) - - @staticmethod - def _parse_image(weak_self, image): - self = weak_self() - if not self: - return - if self.sensors[self.index][0].startswith('sensor.lidar'): - points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) - points = np.reshape(points, (int(points.shape[0] / 3), 3)) - lidar_data = np.array(points[:, :2]) - lidar_data *= min(self.hud.dim) / 100.0 - lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) - lidar_data = np.fabs(lidar_data) # pylint: disable=assignment-from-no-return - lidar_data = lidar_data.astype(np.int32) - lidar_data = np.reshape(lidar_data, (-1, 2)) - lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) - lidar_img = np.zeros(lidar_img_size) - lidar_img[tuple(lidar_data.T)] = (255, 255, 255) - self.surface = pygame.surfarray.make_surface(lidar_img) - else: - image.convert(self.sensors[self.index][1]) - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - array = array[:, :, :3] - array = array[:, :, ::-1] - - ######################################################################################################################################### -############################################################ yolov3 #################################################################### - def calculate_iou(box1, box2): - """ - Calculate the Intersection over Union (IoU) between two bounding boxes. - - Args: - box1 (list): List containing the coordinates of the first bounding box [x, y, w, h]. - box2 (list): List containing the coordinates of the second bounding box [x, y, w, h]. - - Returns: - float: The IoU value. - - """ - # Extract box coordinates - x1, y1, w1, h1 = box1 - x2, y2, w2, h2 = box2 - - # Calculate box coordinates - x_left = max(x1, x2) - y_top = max(y1, y2) - x_right = min(x1 + w1, x2 + w2) - y_bottom = min(y1 + h1, y2 + h2) - - # Calculate intersection area - intersection_area = max(0, x_right - x_left) * max(0, y_bottom - y_top) - - # Calculate union area - box1_area = w1 * h1 - box2_area = w2 * h2 - union_area = box1_area + box2_area - intersection_area - - # Calculate IoU - iou = intersection_area / union_area - - return iou - - - test_im = np.array(image.raw_data) - test_im = test_im.copy() - test_im = test_im.reshape((image.height, image.width, 4)) - test_im = test_im[:, :, :3] - - # Load YOLO - model_weights = 'yolov3.cfg' - model_cfg = 'yolov3.weights' - #frameWidth = 1200 - #frameHeight = 720 - frameWidth = 640 - frameHeight = 480 - net = cv2.dnn.readNet(model_weights, model_cfg) - - # Define your loss function - #loss_fn = tf.keras.losses.BinaryCrossentropy() - - classes = [] - with open("coco.names", "r") as f: - classes = [line.strip() for line in f.readlines()] - - layers_names = net.getLayerNames() - #output_layers = [layers_names[i[0] - 1] for i in net.getUnconnectedOutLayers()] - output_layers = [layers_names[i - 1] for i in net.getUnconnectedOutLayers()] - colors = np.random.uniform(0, 255, size=(len(classes), 3)) - - # Loading image - img = cv2.resize(test_im, (frameWidth, frameHeight), None) - height, width, channels = img.shape - img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) - - # Detect objects - blob = cv2.dnn.blobFromImage(img, 1/255, (416, 416), (0, 0, 0), True, crop=False) - net.setInput(blob) - outs = net.forward(output_layers) - - # Perform object detection - class_ids = [] - confidences = [] - boxes = [] - - for out in outs: - for detection in out: - scores = detection[5:] - class_id = np.argmax(scores) - confidence = scores[class_id] - - if confidence > 0.5: - # Object detected - center_x = int(detection[0] * width) - center_y = int(detection[1] * height) - w = int(detection[2] * width) - h = int(detection[3] * height) - - # Rectangle coordinates - x = int(center_x - w / 2) - y = int(center_y - h / 2) - - boxes.append([x, y, w, h]) - confidences.append(float(confidence)) - class_ids.append(class_id) - - indexes = cv2.dnn.NMSBoxes(boxes, confidences, 0.5, 0.4) - font = cv2.FONT_HERSHEY_SIMPLEX - colors = np.random.uniform(0, 255, size=(len(boxes), 3)) - - detected_labels = [] - detected_boxes = [] - predicted_confidences = [] - predicted_class_probs = [] - - for i in range(len(boxes)): - if i in indexes: - x, y, w, h = boxes[i] - label = str(classes[class_ids[i]]) - confidence = str(round(confidences[i], 2)) - color = colors[i] - cv2.rectangle(img, (x, y), (x + w, y + h), color, 2) - cv2.putText(img, label + " " + confidence, (x + 60, y + 60), font, 0.8, (255, 255, 200), 1) - detected_labels.append(label) - detected_boxes.append([x, y, w, h]) - predicted_confidences.append(float(confidence)) - predicted_class_probs.append(label) - - # Ground truth annotations - ground_truth_labels = ['person', 'bicycle', 'car', 'traffic light', 'fire hydrant', 'pottedplant', 'stop sign', 'bench'] - ground_truth_boxes = [[250, 220, 160, 190], [200, 200, 80, 80], [200, 200, 80, 80], [450, 90, 20, 50], [200, 200, 80, 80], [200, 200, 80, 80], [200, 200, 80, 80], [200, 200, 80, 80]] # [x, y, w, h] format - - #def normalize_boxes(boxes, image_width, image_height): - # Convert box coordinates to numpy array - # boxes = np.array(boxes) - - # Extract box coordinates - #x, y, w, h = np.split(boxes, 4, axis=1) - - # Normalize box coordinates - #normalized_x = x / image_width - #normalized_y = y / image_height - #normalized_w = w / image_width - #normalized_h = h / image_height - - # Concatenate normalized coordinates - # normalized_boxes = np.concatenate((normalized_x, normalized_y, normalized_w, normalized_h), axis=1) - - #return normalized_boxes - ''' - def preprocess_ground_truth(ground_truth_labels, ground_truth_boxes): - # Define a dictionary to map class labels to class IDs - label_to_id = { - 'person': 0, - 'bicycle': 1, - 'car': 2, - 'traffic light': 3, - 'fire hydrant': 4, - 'pottedplant': 5, - 'stop sign': 6, - 'bench': 7 - } - ''' - - # Convert class labels to class IDs# - ''' - class_ids = [label_to_id[label] for label in ground_truth_labels] - - # Convert class IDs to one-hot encoding - num_classes = len(label_to_id) - one_hot_labels = tf.one_hot(class_ids, num_classes) - - # Normalize bounding box coordinates - normalized_boxes = normalize_boxes(ground_truth_boxes, width, height) # Implement 'normalize_boxes' function to scale box coordinates between 0 and 1 - - print('Normalized Boxes Shape:', normalized_boxes.shape) - print('One-Hot Labels Shape:', one_hot_labels.shape) - print('Normalized Boxes Data Type:', normalized_boxes.dtype) - print('One-Hot Labels Data Type:', one_hot_labels.dtype) - - if normalized_boxes.shape != one_hot_labels.shape: - print('Error: Shapes of normalized_boxes and one_hot_labels do not match.') - return None - - if normalized_boxes.dtype != one_hot_labels.dtype: - print('Error: Data types of normalized_boxes and one_hot_labels do not match.') - return None - - # Combine labels and boxes into a single tensor - ground_truth_targets = tf.concat([normalized_boxes, one_hot_labels], axis=-1) - - return ground_truth_targets, normalized_boxes, one_hot_labels, label_to_id - # Call the function and store the returned values - #ground_truth_targets, normalized_boxes, one_hot_labels, label_to_id = preprocess_ground_truth(ground_truth_labels, ground_truth_boxes) - - #num_classes = len(label_to_id) - ''' - - - # Perform object matching for ground truth and detected objects - TP = 0 # True positives - FP = 0 # False positives - FN = 0 # False negatives - threshold = 0.5 - print("Detected Labels: ", detected_labels) - print("Detected boxes: ", detected_boxes) - - for i, (gt_label, gt_box) in enumerate(zip(ground_truth_labels, ground_truth_boxes)): - max_iou = 0 - max_iou_index = -1 - - for j, (det_label, det_box) in enumerate(zip(detected_labels, detected_boxes)): - iou = calculate_iou(gt_box, det_box) - if iou > max_iou: - max_iou = iou - max_iou_index = j - - if max_iou >= threshold: - # Matched object found (True positive) - TP += 1 - # Remove the matched object from detected objects - if max_iou_index != -1: - detected_labels.pop(max_iou_index) - detected_boxes.pop(max_iou_index) - else: - # No match found (False negative) - FN += 1 - - # Any remaining unmatched detected objects are false positives - FP = len(detected_labels) - - eps = 1e-7 # Small value to avoid division by zero - - # Calculate performance metrics - accuracy = TP / (TP + FP + FN + eps) - precision = TP / (TP + FP + eps) - recall = TP / (TP + FN + eps) - matt_corr_coeff = (TP - (FP * FN)) / math.sqrt((TP + FP + eps) * (TP + FN + eps) * (FP + eps) * (FN + eps)) - - - # Calculate the loss - #ground_truth_targets = preprocess_ground_truth(ground_truth_labels, ground_truth_boxes) # Preprocess the ground truth targets - - # localization_loss = loss_fn(detected_boxes, ground_truth_targets) # Calculate the localization loss - # confidence_loss = loss_fn(predicted_confidences, ground_truth_targets) # Calculate the confidence loss - # class_prob_loss = loss_fn(predicted_class_probs, ground_truth_targets) # Calculate the class probability loss - - # Log the metrics to Tensorboard - log_dir = "./logs3" # Specify the log directory her - writer = SummaryWriter(log_dir=log_dir) - writer.add_scalar('Accuracy', accuracy, image.frame) - writer.add_scalar('Precision', precision, image.frame) - writer.add_scalar('Recall', recall, image.frame) - writer.add_scalar('IOU', max_iou, image.frame) - #writer.add_scalar('Matthews Correlation Coefficient', matt_corr_coeff) - #writer.add_scalar('Localization Loss', localization_loss, image.frame) - #writer.add_scalar('Confidence Loss', confidence_loss, image.frame) - #writer.add_scalar('Class Probability Loss', class_prob_loss, image.frame) - - # Show the annotated image - #cv2.imshow("Annotated Image", img) - #cv2.waitKey(0) - #cv2.destroyAllWindows() - - self.surface = pygame.surfarray.make_surface(img.swapaxes(0, 1)) - if self.recording: - image.save_to_disk('output/%08d' % image.frame) - - -# ============================================================================== -# -- KK PID Controllers --------------------------------------------------------- -# ============================================================================== - -class VehiclePIDController(): - """ - VehiclePIDController is the combination of two PID controllers - (lateral and longitudinal) to perform the - low level control a vehicle from client side - """ - - - def __init__(self, vehicle, args_lateral, args_longitudinal, max_throttle=0.75, max_brake=0.3, max_steering=0.8): - """ - Constructor method. - - :param vehicle: actor to apply to local planner logic onto - :param args_lateral: dictionary of arguments to set the lateral PID controller - using the following semantics: - K_P -- Proportional term - K_D -- Differential term - K_I -- Integral term - :param args_longitudinal: dictionary of arguments to set the longitudinal - PID controller using the following semantics: - K_P -- Proportional term - K_D -- Differential term - K_I -- Integral term - """ - - self.max_brake = max_brake - self.max_throt = max_throttle - self.max_steer = max_steering - - self._vehicle = vehicle - self._world = self._vehicle.get_world() - self.past_steering = self._vehicle.get_control().steer - self._lon_controller = PIDLongitudinalController(self._vehicle, **args_longitudinal) - self._lat_controller = PIDLateralController(self._vehicle, **args_lateral) - - def run_step(self, target_speed, waypoint): - """ - Execute one step of control invoking both lateral and longitudinal - PID controllers to reach a target waypoint - at a given target_speed. - - :param target_speed: desired vehicle speed - :param waypoint: target location encoded as a waypoint - :return: distance (in meters) to the waypoint - """ - - acceleration = self._lon_controller.run_step(target_speed) - current_steering = self._lat_controller.run_step(waypoint) - control = carla.VehicleControl() - if acceleration >= 0.0: - control.throttle = min(acceleration, self.max_throt) - control.brake = 0.0 - else: - control.throttle = 0.0 - control.brake = min(abs(acceleration), self.max_brake) - - # Steering regulation: changes cannot happen abruptly, can't steer too much. - - if current_steering > self.past_steering + 0.1: - current_steering = self.past_steering + 0.1 - elif current_steering < self.past_steering - 0.1: - current_steering = self.past_steering - 0.1 - - if current_steering >= 0: - steering = min(self.max_steer, current_steering) - else: - steering = max(-self.max_steer, current_steering) - - control.steer = steering - control.hand_brake = False - control.manual_gear_shift = False - self.past_steering = steering - - return control - - -class PIDLongitudinalController(): - """ - PIDLongitudinalController implements longitudinal control using a PID. - """ - - - def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=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 = vehicle - self._k_p = K_P - self._k_d = K_D - self._k_i = K_I - self._dt = dt - self._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) / 2 - target_speed = target_speed / 2 - 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_speed - self._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._dt - else: - _de = 0.0 - _ie = 0.0 - - return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) - - -class PIDLateralController(): - """ - PIDLateralController implements lateral control using a PID. - """ - - def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=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 = vehicle - self._k_p = K_P - self._k_d = K_D - self._k_i = K_I - self._dt = dt - self._e_buffer = deque(maxlen=10) - - def run_step(self, waypoint): - """ - Execute one step of lateral control to steer - the 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] - """ - v_begin = vehicle_transform.location - v_end = v_begin + carla.Location(x=math.cos(math.radians(vehicle_transform.rotation.yaw)), - y=math.sin(math.radians(vehicle_transform.rotation.yaw))) - - v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0]) - w_vec = np.array([waypoint.transform.location.x - - v_begin.x, waypoint.transform.location.y - - v_begin.y, 0.0]) - _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / - (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0)) - - _cross = np.cross(v_vec, w_vec) - - if _cross[2] < 0: - _dot *= -1.0 - - self._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._dt - else: - _de = 0.0 - _ie = 0.0 - - return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) - - -# ============================================================================== -# -- KK Local Planner --------------------------------------------------------- -# ============================================================================== - -class RoadOption(Enum): - """ - RoadOption represents the possible topological configurations - when moving from a segment of lane to other. - """ - VOID = -1 - LEFT = 1 - RIGHT = 2 - STRAIGHT = 3 - LANEFOLLOW = 4 - CHANGELANELEFT = 5 - CHANGELANERIGHT = 6 - - -class LocalPlanner(object): - """ - LocalPlanner implements the basic behavior of following a trajectory - of waypoints that is generated on-the-fly. - The low-level motion of the vehicle is computed by using two PID controllers, - one is used for the lateral control - and the other for the longitudinal control (cruise speed). - - When multiple paths are available (intersections) - this local planner makes a random choice. - """ - - # Minimum distance to target waypoint as a percentage - # (e.g. within 80% of total distance) - - # FPS used for dt - FPS = 20 - - def __init__(self, agent): - """ - :param agent: agent that regulates the vehicle - :param vehicle: actor to apply to local planner logic onto - """ - self._vehicle = agent.vehicle - self._map = agent.vehicle.get_world().get_map() - - self._target_speed = None - self.sampling_radius = None - self._min_distance = None - self._current_waypoint = None - self.target_road_option = None - self._next_waypoints = None - self.target_waypoint = None - self._vehicle_controller = None - self._global_plan = None - self._pid_controller = None - self.waypoints_queue = deque(maxlen=20000) # queue with tuples of (waypoint, RoadOption) - self._buffer_size = 5 - self._waypoint_buffer = deque(maxlen=self._buffer_size) - - self._init_controller() # initializing controller - - def get_distance_to_nearest_obstacle(self): - ego_vehicle_transform = self._vehicle.get_transform() - ego_vehicle_location = ego_vehicle_transform.location - - # Get the bounding box of the ego vehicle - ego_vehicle_bbox = self._vehicle.bounding_box - - # Calculate the minimum distance to other vehicles - distance_to_nearest_obstacle = None - - for actor in self._vehicle.get_world().get_actors().filter('vehicle.*'): - if actor.id != self._vehicle.id: - actor_transform = actor.get_transform() - actor_location = actor_transform.location - - # Calculate the distance between the ego vehicle and the other vehicle - distance = ego_vehicle_location.distance(actor_location) - - # Calculate the minimum distance to the other vehicle's bounding box - distance_to_obstacle = distance - ego_vehicle_bbox.extent.x - actor.bounding_box.extent.x - - if distance_to_nearest_obstacle is None or distance_to_obstacle < distance_to_nearest_obstacle: - distance_to_nearest_obstacle = distance_to_obstacle - - - return distance_to_nearest_obstacle - - def reset_vehicle(self): - """Reset the ego-vehicle""" - self._vehicle = None - print("Resetting ego-vehicle!") - - def _init_controller(self): - """ - Controller initialization. - - dt -- time difference between physics control in seconds. - This is can be fixed from server side - using the arguments -benchmark -fps=F, since dt = 1/F - - target_speed -- desired cruise speed in km/h - - min_distance -- minimum distance to remove waypoint from queue - - lateral_dict -- dictionary of arguments to setup the lateral PID controller - {'K_P':, 'K_D':, 'K_I':, 'dt'} - - longitudinal_dict -- dictionary of arguments to setup the longitudinal PID controller - {'K_P':, 'K_D':, 'K_I':, 'dt'} - """ - # Default parameters - self.args_lat_hw_dict = { - 'K_P': 0.75, - 'K_D': 0.02, - 'K_I': 0.4, - 'dt': 1.0 / self.FPS} - self.args_lat_city_dict = { - 'K_P': 0.58, - 'K_D': 0.02, - 'K_I': 0.5, - 'dt': 1.0 / self.FPS} - self.args_long_hw_dict = { - 'K_P': 0.37, - 'K_D': 0.024, - 'K_I': 0.032, - 'dt': 1.0 / self.FPS} - self.args_long_city_dict = { - 'K_P': 0.15, - 'K_D': 0.05, - 'K_I': 0.07, - 'dt': 1.0 / self.FPS} - - self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) - - self._global_plan = False - - self._target_speed = self._vehicle.get_speed_limit() - - self._min_distance = 3 - - def set_speed(self, speed): - """ - Request new target speed. - - :param speed: new target speed in km/h - """ - - self._target_speed = speed - - def set_global_plan(self, current_plan): - """ - Sets new global plan. - - :param current_plan: list of waypoints in the actual plan - """ - for elem in current_plan: - self.waypoints_queue.append(elem) - self._global_plan = True - - def get_incoming_waypoint_and_direction(self, steps=3): - """ - Returns direction and waypoint at a distance ahead defined by the user. - - :param steps: number of steps to get the incoming waypoint. - """ - if len(self.waypoints_queue) > steps: - return self.waypoints_queue[steps] - - else: - try: - wpt, direction = self.waypoints_queue[-1] - return wpt, direction - except IndexError as i: - print(i) - return None, RoadOption.VOID - return None, RoadOption.VOID - - def run_step(self, target_speed=None, debug=False): - """ - Execute one step of local planning which involves - running the longitudinal and lateral PID controllers to - follow the waypoints trajectory. - - :param target_speed: desired speed - :param debug: boolean flag to activate waypoints debugging - :return: control - """ - - if target_speed is not None: - self._target_speed = target_speed - else: - self._target_speed = self._vehicle.get_speed_limit() - - if len(self.waypoints_queue) == 0: - control = carla.VehicleControl() - control.steer = 0.0 - control.throttle = 0.0 - control.brake = 1.0 - control.hand_brake = False - control.manual_gear_shift = False - return control - - # Buffering the waypoints - if not self._waypoint_buffer: - for i in range(self._buffer_size): - if self.waypoints_queue: - self._waypoint_buffer.append( - self.waypoints_queue.popleft()) - else: - break - - # Current vehicle waypoint - self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) - - # Target waypoint - self.target_waypoint, self.target_road_option = self._waypoint_buffer[0] - - if target_speed > 50: - args_lat = self.args_lat_hw_dict - args_long = self.args_long_hw_dict - else: - args_lat = self.args_lat_city_dict - args_long = self.args_long_city_dict - - self._pid_controller = VehiclePIDController(self._vehicle, - args_lateral=args_lat, - args_longitudinal=args_long) - - control = self._pid_controller.run_step(self._target_speed, self.target_waypoint) - - # Purge the queue of obsolete waypoints - vehicle_transform = self._vehicle.get_transform() - max_index = -1 - - for i, (waypoint, _) in enumerate(self._waypoint_buffer): - if distance_vehicle( - waypoint, vehicle_transform) < self._min_distance: - max_index = i - if max_index >= 0: - for i in range(max_index + 1): - self._waypoint_buffer.popleft() - - if debug: - draw_waypoints(self._vehicle.get_world(), - [self.target_waypoint], 1.0) - return control - - -# ============================================================================== -# -- KK Agent Class --------------------------------------------------------- -# ============================================================================== - -class AgentState(Enum): - """ - AGENT_STATE represents the possible states of a roaming agent - """ - NAVIGATING = 1 - BLOCKED_BY_VEHICLE = 2 - BLOCKED_RED_LIGHT = 3 - - -class Agent(object): - """Base class to define agents in CARLA""" - - def __init__(self, vehicle): - """ - Constructor method. - - :param vehicle: actor to apply to local planner logic onto - """ - self._vehicle = vehicle - self._proximity_tlight_threshold = 5.0 # meters - self._proximity_vehicle_threshold = 10.0 # meters - self._local_planner = None - self._world = self._vehicle.get_world() - try: - self._map = self._world.get_map() - except RuntimeError as error: - print('RuntimeError: {}'.format(error)) - print(' The server could not send the OpenDRIVE (.xodr) file:') - print(' Make sure it exists, has the same name of your town, and is correct.') - sys.exit(1) - self._last_traffic_light = None - - def get_local_planner(self): - """Get method for protected member local planner""" - return self._local_planner - - @staticmethod - def run_step(debug=False): - """ - Execute one step of navigation. - - :param debug: boolean flag for debugging - :return: control - """ - control = carla.VehicleControl() - - if debug: - control.steer = 0.0 - control.throttle = 0.0 - control.brake = 0.0 - control.hand_brake = False - control.manual_gear_shift = False - - return control - - @staticmethod - def emergency_stop(): - """ - Send an emergency stop command to the vehicle - - :return: control for braking - """ - control = carla.VehicleControl() - control.steer = 0.0 - control.throttle = 0.0 - control.brake = 1.0 - control.hand_brake = False - - return control - - -# ============================================================================== -# -- KK Behavior Agent Class --------------------------------------------------------- -# ============================================================================== - -class BehaviorAgent(Agent): - """ - BehaviorAgent implements an agent that navigates scenes to reach a given - target destination, by computing the shortest possible path to it. - This agent can correctly follow traffic signs, speed limitations, - traffic lights, while also taking into account nearby vehicles. Lane changing - decisions can be taken by analyzing the surrounding environment, - such as overtaking or tailgating avoidance. Adding to these are possible - behaviors, the agent can also keep safety distance from a car in front of it - by tracking the instantaneous time to collision and keeping it in a certain range. - Finally, different sets of behaviors are encoded in the agent, from cautious - to a more aggressive ones. - """ - - def __init__(self, vehicle, ignore_traffic_light=False, behavior='normal'): - """ - Constructor method. - - :param vehicle: actor to apply to local planner logic onto - :param ignore_traffic_light: boolean to ignore any traffic light - :param behavior: type of agent to apply - """ - - super(BehaviorAgent, self).__init__(vehicle) - self.vehicle = vehicle - self.ignore_traffic_light = ignore_traffic_light - self._local_planner = LocalPlanner(self) - self._grp = None - self.look_ahead_steps = 0 - - # Vehicle information - self.speed = 0 - self.speed_limit = 0 - self.direction = None - self.incoming_direction = None - self.incoming_waypoint = None - self.start_waypoint = None - self.end_waypoint = None - self.is_at_traffic_light = 0 - self.light_state = "Green" - self.light_id_to_ignore = -1 - self.min_speed = 5 - self.behavior = None - self._sampling_resolution = 4.5 - - # Parameters for agent behavior - if behavior == 'cautious': - self.behavior = Cautious() - - elif behavior == 'normal': - self.behavior = Normal() - - elif behavior == 'aggressive': - self.behavior = Aggressive() - - def update_information(self, world): - """ - This method updates the information regarding the ego - vehicle based on the surrounding world. - - :param world: carla.world object - """ - self.speed = get_speed(self.vehicle) - self.speed_limit = world.player.get_speed_limit() - # self.speed_limit = 100 # KK set speed to 100 km/h - self._local_planner.set_speed(self.speed_limit) - self.direction = self._local_planner.target_road_option - if self.direction is None: - self.direction = RoadOption.LANEFOLLOW - - self.look_ahead_steps = int(self.speed_limit / 10) - - self.incoming_waypoint, self.incoming_direction = self._local_planner.get_incoming_waypoint_and_direction( - steps=self.look_ahead_steps) - if self.incoming_direction is None: - self.incoming_direction = RoadOption.LANEFOLLOW - - self.is_at_traffic_light = world.player.is_at_traffic_light() - if self.ignore_traffic_light: - self.light_state = "Green" - else: - if self.is_at_traffic_light: - traffic_light = world.player.get_traffic_light() - self.light_state = str(traffic_light.state) - if self.light_id_to_ignore != -1 and traffic_light.id == self.light_id_to_ignore: - # Ignore this traffic light for this iteration - self.light_state = "Green" - # Get distance to the nearest obstacle - distance_to_nearest_obstacle = self._local_planner.get_distance_to_nearest_obstacle() - - # Check if the obstacle is closer than the minimum distance - if distance_to_nearest_obstacle is not None and distance_to_nearest_obstacle < 3.0: - # Apply braking if the distance to the nearest obstacle is below the threshold - self._local_planner.brake = 1.0 - else: - self._local_planner.brake = 0.0 - #else: - # This method also includes stop signs and intersections. - #self.light_state = str(self.vehicle.get_traffic_light_state()) - - def set_destination(self, start_location, end_location, clean=False): - """ - This method creates a list of waypoints from agent's position to destination location - based on the route returned by the global router. - - :param start_location: initial position - :param end_location: final position - :param clean: boolean to clean the waypoint queue - """ - if clean: - self._local_planner.waypoints_queue.clear() - self.start_waypoint = self._map.get_waypoint(start_location) - self.end_waypoint = self._map.get_waypoint(end_location) - - route_trace = self._trace_route(self.start_waypoint, self.end_waypoint) - - self._local_planner.set_global_plan(route_trace) - - def reroute(self, spawn_points): - """ - This method implements re-routing for vehicles approaching its destination. - It finds a new target and computes another path to reach it. - - :param spawn_points: list of possible destinations for the agent - """ - - print("Target almost reached, setting new destination...") - random.shuffle(spawn_points) - new_start = self._local_planner.waypoints_queue[-1][0].transform.location - destination = spawn_points[0].location if spawn_points[0].location != new_start else spawn_points[1].location - print("New destination: " + str(destination)) - - self.set_destination(new_start, destination) - - def _trace_route(self, start_waypoint, end_waypoint): - """ - This method sets up a global router and returns the - optimal route from start_waypoint to end_waypoint. - - :param start_waypoint: initial position - :param end_waypoint: final position - """ - # Setting up global router - if self._grp is None: - wld = self.vehicle.get_world() - dao = GlobalRoutePlannerDAO( - wld.get_map(), sampling_resolution=self._sampling_resolution) - grp = GlobalRoutePlanner(dao) - grp.setup() - self._grp = grp - - # Obtain route plan - route = self._grp.trace_route( - start_waypoint.transform.location, - end_waypoint.transform.location) - - return route - - def run_step(self, debug=False): - """ - Execute one step of navigation. - - :param debug: boolean for debugging - :return control: carla.VehicleControl - """ - control = carla.VehicleControl() - - ego_vehicle_loc = self.vehicle.get_location() - ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) - - if self.is_at_traffic_light and self.light_state != "Green": - # Stop at red traffic light - control = carla.VehicleControl() - control.throttle = 0.0 - control.brake = 1.0 - control.steer = 0.0 - else: - # Get distance to the nearest obstacle - distance_to_nearest_obstacle = self._local_planner.get_distance_to_nearest_obstacle() - - # Check if the obstacle is closer than the minimum distance - if distance_to_nearest_obstacle is not None and distance_to_nearest_obstacle < 3.0: - # Apply braking if the distance to the nearest obstacle is below the threshold - control.throttle = 0.0 - control.brake = 1.0 - control.steer = 0.0 - else: - # Continue following the planned route - control = self._local_planner.run_step( - target_speed=min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist), debug=debug) - self.vehicle.apply_control(control) - - return control - - -# ============================================================================== -# -- Game Loop --------------------------------------------------------- -# ============================================================================== - - -def game_loop(args): - """ Main loop for agent""" - - pygame.init() - pygame.font.init() - world = None - tot_target_reached = 0 - num_min_waypoints = 21 - - try: - client = carla.Client(args.host, args.port) - client.set_timeout(70.0) - - display = pygame.display.set_mode( - (args.width, args.height), - pygame.HWSURFACE | pygame.DOUBLEBUF) - - hud = HUD(args.width, args.height) - - #selected_world = client.load_world("Town01") # KK Changes world to Town04 - selected_world = client.get_world() - - world = World(client.get_world(), hud, args) - controller = KeyboardControl(world) - - agent = BehaviorAgent(world.player, behavior=args.behavior) - - spawn_points = world.map.get_spawn_points() - random.shuffle(spawn_points) - - if spawn_points[0].location != agent.vehicle.get_location(): - destination = spawn_points[0].location - else: - destination = spawn_points[1].location - - agent.set_destination(agent.vehicle.get_location(), destination, clean=True) - - clock = pygame.time.Clock() - - while True: - clock.tick_busy_loop(60) - if controller.parse_events(world): - return - - # As soon as the server is ready continue! - if not world.world.wait_for_tick(10.0): - continue - - if args.agent == "Roaming" or args.agent == "Basic": - if controller.parse_events(world): - return - - #agent.update_information(world) - # as soon as the server is ready continue! - world.world.wait_for_tick(10.0) - - world.tick(clock) - world.render(display) - pygame.display.flip() - control = agent.run_step() - control.manual_gear_shift = False - world.player.apply_control(control) - else: - agent.update_information(world) - - world.tick(clock) - world.render(display) - pygame.display.flip() - - - # Set new destination when target has been reached - if len(agent.get_local_planner().waypoints_queue) < num_min_waypoints and args.loop: - agent.reroute(spawn_points) - tot_target_reached += 1 - world.hud.notification("The target has been reached " + - str(tot_target_reached) + " times.", seconds=4.0) - - elif len(agent.get_local_planner().waypoints_queue) == 0 and not args.loop: - print("Target reached, mission accomplished...") - break - - speed_limit = world.player.get_speed_limit() - agent.get_local_planner().set_speed(speed_limit) - - control = agent.run_step() - world.player.apply_control(control) - - finally: - if world is not None: - world.destroy() - - pygame.quit() - - -# ============================================================================== -# -- main() -------------------------------------------------------------- -# ============================================================================== - - -def main(): - """Main method""" - - argparser = argparse.ArgumentParser( - description='CARLA Automatic Control Client') - argparser.add_argument( - '-v', '--verbose', - action='store_true', - dest='debug', - help='Print debug information') - argparser.add_argument( - '--host', - metavar='H', - default='127.0.0.1', - help='IP of the host server (default: 127.0.0.1)') - argparser.add_argument( - '-p', '--port', - metavar='P', - default=2000, - type=int, - help='TCP port to listen to (default: 2000)') - argparser.add_argument( - '--res', - metavar='WIDTHxHEIGHT', - default='1280x720', - help='Window resolution (default: 1280x720)') - argparser.add_argument( - '--filter', - metavar='PATTERN', - default='vehicle.*', - help='Actor filter (default: "vehicle.*")') - argparser.add_argument( - '--gamma', - default=2.2, - type=float, - help='Gamma correction of the camera (default: 2.2)') - argparser.add_argument( - '-l', '--loop', - action='store_true', - dest='loop', - help='Sets a new random destination upon reaching the previous one (default: False)') - argparser.add_argument( - '-b', '--behavior', type=str, - choices=["cautious", "normal", "aggressive"], - help='Choose one of the possible agent behaviors (default: normal) ', - default='normal') - argparser.add_argument("-a", "--agent", type=str, - choices=["Behavior", "Roaming", "Basic"], - help="select which agent to run", - default="Behavior") - argparser.add_argument( - '-s', '--seed', - help='Set seed for repeating executions (default: None)', - default=None, - type=int) - - args = argparser.parse_args() - - args.width, args.height = [int(x) for x in args.res.split('x')] - - log_level = logging.DEBUG if args.debug else logging.INFO - logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) - - logging.info('listening to server %s:%s', args.host, args.port) - - print(__doc__) - - try: - game_loop(args) - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') - - -if __name__ == '__main__': - main()# TEST MODIFICATION - 12/18/2025 13:26:06 - \ No newline at end of file +"""Example of automatic vehicle control from client side.""" + +from __future__ import print_function + +import sys +import os +# 获取当前脚本所在的目录,并找到agents文件夹的路径 +current_dir = os.path.dirname(os.path.abspath(__file__)) +agents_path = os.path.join(current_dir, 'agents') +# 将该路径添加到模块搜索路径中 +if os.path.exists(agents_path): + sys.path.insert(0, agents_path) +import argparse +import collections +import datetime +import glob +import logging +import math +import os +import cv2 +import random +import re +import sys +import weakref +from enum import Enum +from collections import deque + + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import K_ESCAPE + from pygame.locals import K_q + from pygame.locals import K_l + from pygame.locals import K_v + from pygame.locals import K_c +except ImportError: + raise RuntimeError('cannot import pygame, make sure pygame package is installed') + +try: + import numpy as np +except ImportError: + raise RuntimeError( + 'cannot import numpy, make sure numpy package is installed') + +# ============================================================================== +# -- Find CARLA module --------------------------------------------------------- +# ============================================================================== +try: + sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( + sys.version_info.major, + sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) +except IndexError: + pass + +# ============================================================================== +# -- Add PythonAPI for release mode -------------------------------------------- +# ============================================================================== +try: + sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + '/carla') +except IndexError: + pass + +import carla +from carla import ColorConverter as cc +from torch.utils.tensorboard import SummaryWriter +import torch +#import tensorflow as tf + +# Create a SummaryWriter object to write the metrics to Tensorboard +writer = SummaryWriter() + +from agents.navigation.global_route_planner import GlobalRoutePlanner +from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO +from agents.navigation.types_behavior import Cautious, Aggressive, Normal + +from agents.tools.misc import get_speed, positive, draw_waypoints, distance_vehicle + + +# ============================================================================== +# -- Global functions ---------------------------------------------------------- +# ============================================================================== + + +def find_weather_presets(): + """Method to find weather presets""" + rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') + def name(x): return ' '.join(m.group(0) for m in rgx.finditer(x)) + presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + """Method to get actor display name""" + name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) + return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name + + +# ============================================================================== +# -- World --------------------------------------------------------------- +# ============================================================================== + +class World(object): + """ Class representing the surrounding environment """ + + def __init__(self, carla_world, hud, args): + """Constructor method""" + self.world = carla_world + try: + self.map = self.world.get_map() + except RuntimeError as error: + print('RuntimeError: {}'.format(error)) + print(' The server could not send the OpenDRIVE (.xodr) file:') + print(' Make sure it exists, has the same name of your town, and is correct.') + sys.exit(1) + self.hud = hud + self.player = None + self.collision_sensor = None + self.lane_invasion_sensor = None + self.gnss_sensor = None + self.camera_manager = None + self._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = args.filter + self._gamma = args.gamma + self.restart(args) + self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False + self.recording_start = 0 + + def restart(self, args): + """Restart the world""" + # Keep same camera config if the camera manager exists. + cam_index = self.camera_manager.index if self.camera_manager is not None else 0 + cam_pos_id = self.camera_manager.transform_index if self.camera_manager is not None else 0 + # Set the seed if requested by user + if args.seed is not None: + random.seed(args.seed) + + # KK Get a vehicle blueprint + #blueprint = self.world.get_blueprint_library().filter('vehicle.lincoln.mkz2017')[0] # KK + blueprint = self.world.get_blueprint_library().filter('model3')[0] + blueprint.set_attribute('role_name', 'hero') + if blueprint.has_attribute('color'): + color = '236,102,17' # KK Packt logo color + blueprint.set_attribute('color', color) + # Spawn the player. + print("Spawning the player") + if self.player is not None: + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + self.destroy() + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + + while self.player is None: + if not self.map.get_spawn_points(): + print('There are no spawn points available in your map/town.') + print('Please add some Vehicle Spawn Point to your UE4 scene.') + sys.exit(1) + spawn_points = self.map.get_spawn_points() + spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() + #spawn_point = spawn_points[0] if spawn_points else carla.Transform() # KK + self.player = self.world.try_spawn_actor(blueprint, spawn_point) + # Set up the sensors. + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud, self._gamma) + self.camera_manager.transform_index = cam_pos_id + self.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.player) + self.hud.notification(actor_type) + def next_weather(self, reverse=False): + """Get next weather setting""" + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification('Weather: %s' % preset[1]) + self.player.get_world().set_weather(preset[0]) + + def tick(self, clock): + """Method for every tick""" + self.hud.tick(self, clock) + + def render(self, display): + """Render world""" + self.camera_manager.render(display) + self.hud.render(display) + + def destroy_sensors(self): + """Destroy sensors""" + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None + + def destroy(self): + """Destroys all actors""" + actors = [ + self.camera_manager.sensor, + self.collision_sensor.sensor, + self.lane_invasion_sensor.sensor, + self.gnss_sensor.sensor, + self.player] + for actor in actors: + if actor is not None: + actor.destroy() + + +# ============================================================================== +# -- KeyboardControl ----------------------------------------------------------- +# ============================================================================== + + +class KeyboardControl(object): + def __init__(self, world, start_locked=True): + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + # 可选:启动时自动锁定 + if start_locked and world.camera_manager is not None: + world.camera_manager.set_lock_mode(True) + world.camera_manager.hud.notification("Camera Lock: ON", seconds=2.0) + + def parse_events(self, world): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + if event.type == pygame.KEYUP: + if self._is_quit_shortcut(event.key): + return True + elif event.key == K_l: # 按L键切换锁定 + if world.camera_manager is not None: + world.camera_manager.toggle_lock() + elif event.key == K_v: # 按V键切换锁定视角 + if world.camera_manager is not None and world.camera_manager.lock_mode: + world.camera_manager.next_lock_view() + elif event.key == K_c: # 按C键切换相机(原有功能) + if world.camera_manager is not None: + world.camera_manager.toggle_camera() + + @staticmethod + def _is_quit_shortcut(key): + """Shortcut for quitting""" + return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) + +# ============================================================================== +# -- HUD ----------------------------------------------------------------------- +# ============================================================================== + + +class HUD(object): + """Class for HUD text""" + + def __init__(self, width, height): + """Constructor method""" + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 20) + font_name = 'courier' if os.name == 'nt' else 'mono' + fonts = [x for x in pygame.font.get_fonts() if font_name in x] + default_font = 'ubuntumono' + mono = default_font if default_font in fonts else fonts[0] + mono = pygame.font.match_font(mono) + self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + self.help = HelpText(pygame.font.Font(mono, 24), width, height) + self.server_fps = 0 + self.frame = 0 + self.simulation_time = 0 + self._show_info = True + self._info_text = [] + self._server_clock = pygame.time.Clock() + + def on_world_tick(self, timestamp): + """Gets informations from the world at every tick""" + self._server_clock.tick() + self.server_fps = self._server_clock.get_fps() + self.frame = timestamp.frame_count + self.simulation_time = timestamp.elapsed_seconds + + def tick(self, world, clock): + """HUD method for every tick""" + self._notifications.tick(world, clock) + if not self._show_info: + return + transform = world.player.get_transform() + vel = world.player.get_velocity() + control = world.player.get_control() + heading = 'N' if abs(transform.rotation.yaw) < 89.5 else '' + heading += 'S' if abs(transform.rotation.yaw) > 90.5 else '' + heading += 'E' if 179.5 > transform.rotation.yaw > 0.5 else '' + heading += 'W' if -0.5 > transform.rotation.yaw > -179.5 else '' + colhist = world.collision_sensor.get_collision_history() + collision = [colhist[x + self.frame - 200] for x in range(0, 200)] + max_col = max(1.0, max(collision)) + collision = [x / max_col for x in collision] + vehicles = world.world.get_actors().filter('vehicle.*') + + self._info_text = [ + 'Server: % 16.0f FPS' % self.server_fps, + 'Client: % 16.0f FPS' % clock.get_fps(), + '', + 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), + 'Map: % 20s' % world.map.name, + 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), + '', + 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)), + u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (transform.rotation.yaw, heading), + 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (transform.location.x, transform.location.y)), + 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), + 'Height: % 18.0f m' % transform.location.z, + ''] + if isinstance(control, carla.VehicleControl): + self._info_text += [ + ('Throttle:', control.throttle, 0.0, 1.0), + ('Steer:', control.steer, -1.0, 1.0), + ('Brake:', control.brake, 0.0, 1.0), + ('Reverse:', control.reverse), + ('Hand brake:', control.hand_brake), + ('Manual:', control.manual_gear_shift), + 'Gear: %s' % {-1: 'R', 0: 'N'}.get(control.gear, control.gear)] + elif isinstance(control, carla.WalkerControl): + self._info_text += [ + ('Speed:', control.speed, 0.0, 5.556), + ('Jump:', control.jump)] + self._info_text += [ + '', + 'Collision:', + collision, + '', + 'Number of vehicles: % 8d' % len(vehicles)] + + if len(vehicles) > 1: + self._info_text += ['Nearby vehicles:'] + + def dist(l): + return math.sqrt((l.x - transform.location.x)**2 + (l.y - transform.location.y) + ** 2 + (l.z - transform.location.z)**2) + vehicles = [(dist(x.get_location()), x) for x in vehicles if x.id != world.player.id] + + for dist, vehicle in sorted(vehicles): + if dist > 200.0: + break + vehicle_type = get_actor_display_name(vehicle, truncate=22) + self._info_text.append('% 4dm %s' % (dist, vehicle_type)) + + def toggle_info(self): + """Toggle info on or off""" + self._show_info = not self._show_info + + def notification(self, text, seconds=2.0): + """Notification text""" + self._notifications.set_text(text, seconds=seconds) + + def error(self, text): + """Error text""" + self._notifications.set_text('Error: %s' % text, (255, 0, 0)) + + def render(self, display): + """Render for HUD class""" + if self._show_info: + info_surface = pygame.Surface((220, self.dim[1])) + info_surface.set_alpha(100) + display.blit(info_surface, (0, 0)) + v_offset = 4 + bar_h_offset = 100 + bar_width = 106 + for item in self._info_text: + if v_offset + 18 > self.dim[1]: + break + if isinstance(item, list): + if len(item) > 1: + points = [(x + 8, v_offset + 8 + (1 - y) * 30) for x, y in enumerate(item)] + pygame.draw.lines(display, (255, 136, 0), False, points, 2) + item = None + v_offset += 18 + elif isinstance(item, tuple): + if isinstance(item[1], bool): + rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) + pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) + else: + rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect_border, 1) + fig = (item[1] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect( + (bar_h_offset + fig * (bar_width - 6), v_offset + 8), (6, 6)) + else: + rect = pygame.Rect((bar_h_offset, v_offset + 8), (fig * bar_width, 6)) + pygame.draw.rect(display, (255, 255, 255), rect) + item = item[0] + if item: # At this point has to be a str. + surface = self._font_mono.render(item, True, (255, 255, 255)) + display.blit(surface, (8, v_offset)) + v_offset += 18 + self._notifications.render(display) + self.help.render(display) + +# ============================================================================== +# -- FadingText ---------------------------------------------------------------- +# ============================================================================== + + +class FadingText(object): + """ Class for fading text """ + + def __init__(self, font, dim, pos): + """Constructor method""" + self.font = font + self.dim = dim + self.pos = pos + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + + def set_text(self, text, color=(255, 255, 255), seconds=2.0): + """Set fading text""" + text_texture = self.font.render(text, True, color) + self.surface = pygame.Surface(self.dim) + self.seconds_left = seconds + self.surface.fill((0, 0, 0, 0)) + self.surface.blit(text_texture, (10, 11)) + + def tick(self, _, clock): + """Fading text method for every tick""" + delta_seconds = 1e-3 * clock.get_time() + self.seconds_left = max(0.0, self.seconds_left - delta_seconds) + self.surface.set_alpha(500.0 * self.seconds_left) + + def render(self, display): + """Render fading text method""" + display.blit(self.surface, self.pos) + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + """ Helper class for text render""" + + def __init__(self, font, width, height): + """Constructor method""" + lines = [ + 'Welcome to CARLA automatic control client', + '', + 'Camera Control:', + ' C : Change camera position', + ' L : Toggle camera lock on vehicle', + ' V : Change lock view (when locked)', + ' PageUp : Toggle recording', + '', + 'Vehicle Control:', + ' W : Throttle', + ' S : Brake', + ' A/D : Steer left/right', + ' Q : Toggle reverse', + ' P : Toggle autopilot', + '', + 'Miscellaneous:', + ' H : Toggle help', + ' ESC : Quit', + ' CTRL + Q : Quit', + ] + + self.font = font + self.dim = (680, len(lines) * 22 + 12) + self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + self.surface.fill((0, 0, 0, 0)) + for i, line in enumerate(lines): + text_texture = self.font.render(line, True, (255, 255, 255)) + self.surface.blit(text_texture, (22, i * 22)) + self._render = False + self.surface.set_alpha(220) + + def toggle(self): + """Toggle on or off the render help""" + self._render = not self._render + + def render(self, display): + """Render help text method""" + if self._render: + display.blit(self.surface, self.pos) + +# ============================================================================== +# -- CollisionSensor ----------------------------------------------------------- +# ============================================================================== + + +class CollisionSensor(object): + """ Class for collision sensors""" + + def __init__(self, parent_actor, hud): + """Constructor method""" + self.sensor = None + self.history = [] + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + blueprint = world.get_blueprint_library().find('sensor.other.collision') + self.sensor = world.spawn_actor(blueprint, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) + + def get_collision_history(self): + """Gets the history of collisions""" + history = collections.defaultdict(int) + for frame, intensity in self.history: + history[frame] += intensity + return history + + @staticmethod + def _on_collision(weak_self, event): + """On collision method""" + self = weak_self() + if not self: + return + actor_type = get_actor_display_name(event.other_actor) + self.hud.notification('Collision with %r' % actor_type) + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x ** 2 + impulse.y ** 2 + impulse.z ** 2) + self.history.append((event.frame, intensity)) + if len(self.history) > 4000: + self.history.pop(0) + +# ============================================================================== +# -- LaneInvasionSensor -------------------------------------------------------- +# ============================================================================== + + +class LaneInvasionSensor(object): + """Class for lane invasion sensors""" + + def __init__(self, parent_actor, hud): + """Constructor method""" + self.sensor = None + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.lane_invasion') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) + + @staticmethod + def _on_invasion(weak_self, event): + """On invasion method""" + self = weak_self() + if not self: + return + lane_types = set(x.type for x in event.crossed_lane_markings) + text = ['%r' % str(x).split()[-1] for x in lane_types] + self.hud.notification('Crossed line %s' % ' and '.join(text)) + +# ============================================================================== +# -- GnssSensor -------------------------------------------------------- +# ============================================================================== + + +class GnssSensor(object): + """ Class for GNSS sensors""" + + def __init__(self, parent_actor): + """Constructor method""" + self.sensor = None + self._parent = parent_actor + self.lat = 0.0 + self.lon = 0.0 + world = self._parent.get_world() + blueprint = world.get_blueprint_library().find('sensor.other.gnss') + self.sensor = world.spawn_actor(blueprint, carla.Transform(carla.Location(x=1.0, z=2.8)), + attach_to=self._parent) + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) + + @staticmethod + def _on_gnss_event(weak_self, event): + """GNSS method""" + self = weak_self() + if not self: + return + self.lat = event.latitude + self.lon = event.longitude + +# ============================================================================== +# -- CameraManager ------------------------------------------------------------- +# ============================================================================== + + +class CameraManager(object): + """ Class for camera management""" + + def __init__(self, parent_actor, hud, gamma_correction): + """Constructor method""" + self.sensor = None + self.surface = None + self._parent = parent_actor + self.hud = hud + self.recording = False + bound_y = 0.5 + self._parent.bounding_box.extent.y + attachment = carla.AttachmentType + self._camera_transforms = [ + (carla.Transform( + carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8.0)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=1.6, z=1.7)), attachment.Rigid), + (carla.Transform( + carla.Location(x=5.5, y=1.5, z=1.5)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=-8.0, z=6.0), carla.Rotation(pitch=6.0)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=-1, y=-bound_y, z=0.5)), attachment.Rigid)] + self.transform_index = 1 + self.sensors = [ + ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], + ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], + ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], + ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], + ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], + ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, + 'Camera Semantic Segmentation (CityScapes Palette)'], + ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] + + # 添加锁定模式相关属性 + self.lock_mode = True # 默认启用锁定模式 + self.lock_target = parent_actor # 锁定目标 + self.lock_transform_index = 0 # 锁定模式的相机位置索引 + + # 定义锁定模式的相机变换(可以自定义不同角度) + self._lock_transforms = [ + (carla.Transform( + carla.Location(x=-6.0, z=3.5), carla.Rotation(pitch=-10.0)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=2.0, z=1.5), carla.Rotation(pitch=0.0)), attachment.Rigid), + (carla.Transform( + carla.Location(x=5.0, y=2.0, z=2.0), carla.Rotation(pitch=-5.0)), attachment.SpringArm), + (carla.Transform( + carla.Location(x=-10.0, z=5.0), carla.Rotation(pitch=-15.0)), attachment.SpringArm), + ] + + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + blp = bp_library.find(item[0]) + if item[0].startswith('sensor.camera'): + blp.set_attribute('image_size_x', str(hud.dim[0])) + blp.set_attribute('image_size_y', str(hud.dim[1])) + if blp.has_attribute('gamma'): + blp.set_attribute('gamma', str(gamma_correction)) + elif item[0].startswith('sensor.lidar'): + blp.set_attribute('range', '50') + item.append(blp) + self.index = None + + def toggle_camera(self): + """Activate a camera""" + self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) + self.set_sensor(self.index, notify=False, force_respawn=True) + + def set_sensor(self, index, notify=True, force_respawn=False): + """Set a sensor""" + index = index % len(self.sensors) + needs_respawn = True if self.index is None else ( + force_respawn or (self.sensors[index][0] != self.sensors[self.index][0])) + if needs_respawn: + if self.sensor is not None: + self.sensor.destroy() + self.surface = None + + # 选择变换和附着类型 + if self.lock_mode: + # 锁定模式使用锁定变换 + transform, attachment_type = self._lock_transforms[self.lock_transform_index] + else: + # 非锁定模式使用原有变换 + transform, attachment_type = self._camera_transforms[self.transform_index] + + self.sensor = self._parent.get_world().spawn_actor( + self.sensors[index][-1], + transform, + attach_to=self._parent, + attachment_type=attachment_type) + + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) + if notify: + sensor_name = self.sensors[index][2] + if self.lock_mode: + sensor_name += " (Locked)" + self.hud.notification(sensor_name) + self.index = index + + def next_sensor(self): + """Get the next sensor""" + self.set_sensor(self.index + 1) + + def toggle_recording(self): + """Toggle recording on or off""" + self.recording = not self.recording + self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) + + def toggle_lock(self): + """切换锁定模式""" + self.lock_mode = not self.lock_mode + mode_text = "Camera Lock: ON" if self.lock_mode else "Camera Lock: OFF" + self.hud.notification(mode_text, seconds=2.0) + # 重新设置传感器以应用锁定模式 + if self.sensor is not None: + self.set_sensor(self.index, notify=False, force_respawn=True) + return self.lock_mode + + def set_lock_mode(self, enable=True): + """设置锁定模式""" + self.lock_mode = enable + return self.lock_mode + + def next_lock_view(self): + """切换到下一个锁定视角""" + if self.lock_mode: + self.lock_transform_index = (self.lock_transform_index + 1) % len(self._lock_transforms) + self.hud.notification(f'Lock View {self.lock_transform_index + 1}', seconds=1.0) + # 重新设置传感器以应用新的锁定视角 + if self.sensor is not None: + self.set_sensor(self.index, notify=False, force_respawn=True) + + def render(self, display): + """Render method""" + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self.sensors[self.index][0].startswith('sensor.lidar'): + points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 3), 3)) + lidar_data = np.array(points[:, :2]) + lidar_data *= min(self.hud.dim) / 100.0 + lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) + lidar_data = np.fabs(lidar_data) # pylint: disable=assignment-from-no-return + lidar_data = lidar_data.astype(np.int32) + lidar_data = np.reshape(lidar_data, (-1, 2)) + lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) + lidar_img = np.zeros(lidar_img_size) + lidar_img[tuple(lidar_data.T)] = (255, 255, 255) + self.surface = pygame.surfarray.make_surface(lidar_img) + else: + image.convert(self.sensors[self.index][1]) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + ######################################################################################################################################### +############################################################ yolov3 #################################################################### + def calculate_iou(box1, box2): + """ + Calculate the Intersection over Union (IoU) between two bounding boxes. + + Args: + box1 (list): List containing the coordinates of the first bounding box [x, y, w, h]. + box2 (list): List containing the coordinates of the second bounding box [x, y, w, h]. + + Returns: + float: The IoU value. + + """ + # Extract box coordinates + x1, y1, w1, h1 = box1 + x2, y2, w2, h2 = box2 + + # Calculate box coordinates + x_left = max(x1, x2) + y_top = max(y1, y2) + x_right = min(x1 + w1, x2 + w2) + y_bottom = min(y1 + h1, y2 + h2) + + # Calculate intersection area + intersection_area = max(0, x_right - x_left) * max(0, y_bottom - y_top) + + # Calculate union area + box1_area = w1 * h1 + box2_area = w2 * h2 + union_area = box1_area + box2_area - intersection_area + + # Calculate IoU + iou = intersection_area / union_area + + return iou + + + test_im = np.array(image.raw_data) + test_im = test_im.copy() + test_im = test_im.reshape((image.height, image.width, 4)) + test_im = test_im[:, :, :3] + + # Load YOLO + model_weights = 'yolov3.cfg' + model_cfg = 'yolov3.weights' + #frameWidth = 1200 + #frameHeight = 720 + frameWidth = 640 + frameHeight = 480 + net = cv2.dnn.readNet(model_weights, model_cfg) + + # Define your loss function + #loss_fn = tf.keras.losses.BinaryCrossentropy() + + classes = [] + with open("coco.names", "r") as f: + classes = [line.strip() for line in f.readlines()] + + layers_names = net.getLayerNames() + #output_layers = [layers_names[i[0] - 1] for i in net.getUnconnectedOutLayers()] + output_layers = [layers_names[i - 1] for i in net.getUnconnectedOutLayers()] + colors = np.random.uniform(0, 255, size=(len(classes), 3)) + + # Loading image + img = cv2.resize(test_im, (frameWidth, frameHeight), None) + height, width, channels = img.shape + img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + + # Detect objects + blob = cv2.dnn.blobFromImage(img, 1/255, (416, 416), (0, 0, 0), True, crop=False) + net.setInput(blob) + outs = net.forward(output_layers) + + # Perform object detection + class_ids = [] + confidences = [] + boxes = [] + + for out in outs: + for detection in out: + scores = detection[5:] + class_id = np.argmax(scores) + confidence = scores[class_id] + + if confidence > 0.5: + # Object detected + center_x = int(detection[0] * width) + center_y = int(detection[1] * height) + w = int(detection[2] * width) + h = int(detection[3] * height) + + # Rectangle coordinates + x = int(center_x - w / 2) + y = int(center_y - h / 2) + + boxes.append([x, y, w, h]) + confidences.append(float(confidence)) + class_ids.append(class_id) + + indexes = cv2.dnn.NMSBoxes(boxes, confidences, 0.5, 0.4) + font = cv2.FONT_HERSHEY_SIMPLEX + colors = np.random.uniform(0, 255, size=(len(boxes), 3)) + + detected_labels = [] + detected_boxes = [] + predicted_confidences = [] + predicted_class_probs = [] + + for i in range(len(boxes)): + if i in indexes: + x, y, w, h = boxes[i] + label = str(classes[class_ids[i]]) + confidence = str(round(confidences[i], 2)) + color = colors[i] + cv2.rectangle(img, (x, y), (x + w, y + h), color, 2) + cv2.putText(img, label + " " + confidence, (x + 60, y + 60), font, 0.8, (255, 255, 200), 1) + detected_labels.append(label) + detected_boxes.append([x, y, w, h]) + predicted_confidences.append(float(confidence)) + predicted_class_probs.append(label) + + # Ground truth annotations + ground_truth_labels = ['person', 'bicycle', 'car', 'traffic light', 'fire hydrant', 'pottedplant', 'stop sign', 'bench'] + ground_truth_boxes = [[250, 220, 160, 190], [200, 200, 80, 80], [200, 200, 80, 80], [450, 90, 20, 50], [200, 200, 80, 80], [200, 200, 80, 80], [200, 200, 80, 80], [200, 200, 80, 80]] # [x, y, w, h] format + + #def normalize_boxes(boxes, image_width, image_height): + # Convert box coordinates to numpy array + # boxes = np.array(boxes) + + # Extract box coordinates + #x, y, w, h = np.split(boxes, 4, axis=1) + + # Normalize box coordinates + #normalized_x = x / image_width + #normalized_y = y / image_height + #normalized_w = w / image_width + #normalized_h = h / image_height + + # Concatenate normalized coordinates + # normalized_boxes = np.concatenate((normalized_x, normalized_y, normalized_w, normalized_h), axis=1) + + #return normalized_boxes + ''' + def preprocess_ground_truth(ground_truth_labels, ground_truth_boxes): + # Define a dictionary to map class labels to class IDs + label_to_id = { + 'person': 0, + 'bicycle': 1, + 'car': 2, + 'traffic light': 3, + 'fire hydrant': 4, + 'pottedplant': 5, + 'stop sign': 6, + 'bench': 7 + } + ''' + + # Convert class labels to class IDs# + ''' + class_ids = [label_to_id[label] for label in ground_truth_labels] + + # Convert class IDs to one-hot encoding + num_classes = len(label_to_id) + one_hot_labels = tf.one_hot(class_ids, num_classes) + + # Normalize bounding box coordinates + normalized_boxes = normalize_boxes(ground_truth_boxes, width, height) # Implement 'normalize_boxes' function to scale box coordinates between 0 and 1 + + print('Normalized Boxes Shape:', normalized_boxes.shape) + print('One-Hot Labels Shape:', one_hot_labels.shape) + print('Normalized Boxes Data Type:', normalized_boxes.dtype) + print('One-Hot Labels Data Type:', one_hot_labels.dtype) + + if normalized_boxes.shape != one_hot_labels.shape: + print('Error: Shapes of normalized_boxes and one_hot_labels do not match.') + return None + + if normalized_boxes.dtype != one_hot_labels.dtype: + print('Error: Data types of normalized_boxes and one_hot_labels do not match.') + return None + + # Combine labels and boxes into a single tensor + ground_truth_targets = tf.concat([normalized_boxes, one_hot_labels], axis=-1) + + return ground_truth_targets, normalized_boxes, one_hot_labels, label_to_id + # Call the function and store the returned values + #ground_truth_targets, normalized_boxes, one_hot_labels, label_to_id = preprocess_ground_truth(ground_truth_labels, ground_truth_boxes) + + #num_classes = len(label_to_id) + ''' + + + # Perform object matching for ground truth and detected objects + TP = 0 # True positives + FP = 0 # False positives + FN = 0 # False negatives + threshold = 0.5 + print("Detected Labels: ", detected_labels) + print("Detected boxes: ", detected_boxes) + + for i, (gt_label, gt_box) in enumerate(zip(ground_truth_labels, ground_truth_boxes)): + max_iou = 0 + max_iou_index = -1 + + for j, (det_label, det_box) in enumerate(zip(detected_labels, detected_boxes)): + iou = calculate_iou(gt_box, det_box) + if iou > max_iou: + max_iou = iou + max_iou_index = j + + if max_iou >= threshold: + # Matched object found (True positive) + TP += 1 + # Remove the matched object from detected objects + if max_iou_index != -1: + detected_labels.pop(max_iou_index) + detected_boxes.pop(max_iou_index) + else: + # No match found (False negative) + FN += 1 + + # Any remaining unmatched detected objects are false positives + FP = len(detected_labels) + + eps = 1e-7 # Small value to avoid division by zero + + # Calculate performance metrics + accuracy = TP / (TP + FP + FN + eps) + precision = TP / (TP + FP + eps) + recall = TP / (TP + FN + eps) + matt_corr_coeff = (TP - (FP * FN)) / math.sqrt((TP + FP + eps) * (TP + FN + eps) * (FP + eps) * (FN + eps)) + + + # Calculate the loss + #ground_truth_targets = preprocess_ground_truth(ground_truth_labels, ground_truth_boxes) # Preprocess the ground truth targets + + # localization_loss = loss_fn(detected_boxes, ground_truth_targets) # Calculate the localization loss + # confidence_loss = loss_fn(predicted_confidences, ground_truth_targets) # Calculate the confidence loss + # class_prob_loss = loss_fn(predicted_class_probs, ground_truth_targets) # Calculate the class probability loss + + # Log the metrics to Tensorboard + log_dir = "./logs3" # Specify the log directory her + writer = SummaryWriter(log_dir=log_dir) + writer.add_scalar('Accuracy', accuracy, image.frame) + writer.add_scalar('Precision', precision, image.frame) + writer.add_scalar('Recall', recall, image.frame) + writer.add_scalar('IOU', max_iou, image.frame) + #writer.add_scalar('Matthews Correlation Coefficient', matt_corr_coeff) + #writer.add_scalar('Localization Loss', localization_loss, image.frame) + #writer.add_scalar('Confidence Loss', confidence_loss, image.frame) + #writer.add_scalar('Class Probability Loss', class_prob_loss, image.frame) + + # Show the annotated image + #cv2.imshow("Annotated Image", img) + #cv2.waitKey(0) + #cv2.destroyAllWindows() + + self.surface = pygame.surfarray.make_surface(img.swapaxes(0, 1)) + if self.recording: + image.save_to_disk('output/%08d' % image.frame) + + +# ============================================================================== +# -- KK PID Controllers --------------------------------------------------------- +# ============================================================================== + +class VehiclePIDController(): + """ + VehiclePIDController is the combination of two PID controllers + (lateral and longitudinal) to perform the + low level control a vehicle from client side + """ + + + def __init__(self, vehicle, args_lateral, args_longitudinal, max_throttle=0.75, max_brake=0.3, max_steering=0.8): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param args_lateral: dictionary of arguments to set the lateral PID controller + using the following semantics: + K_P -- Proportional term + K_D -- Differential term + K_I -- Integral term + :param args_longitudinal: dictionary of arguments to set the longitudinal + PID controller using the following semantics: + K_P -- Proportional term + K_D -- Differential term + K_I -- Integral term + """ + + self.max_brake = max_brake + self.max_throt = max_throttle + self.max_steer = max_steering + + self._vehicle = vehicle + self._world = self._vehicle.get_world() + self.past_steering = self._vehicle.get_control().steer + self._lon_controller = PIDLongitudinalController(self._vehicle, **args_longitudinal) + self._lat_controller = PIDLateralController(self._vehicle, **args_lateral) + + def run_step(self, target_speed, waypoint): + """ + Execute one step of control invoking both lateral and longitudinal + PID controllers to reach a target waypoint + at a given target_speed. + + :param target_speed: desired vehicle speed + :param waypoint: target location encoded as a waypoint + :return: distance (in meters) to the waypoint + """ + + acceleration = self._lon_controller.run_step(target_speed) + current_steering = self._lat_controller.run_step(waypoint) + control = carla.VehicleControl() + if acceleration >= 0.0: + control.throttle = min(acceleration, self.max_throt) + control.brake = 0.0 + else: + control.throttle = 0.0 + control.brake = min(abs(acceleration), self.max_brake) + + # Steering regulation: changes cannot happen abruptly, can't steer too much. + + if current_steering > self.past_steering + 0.1: + current_steering = self.past_steering + 0.1 + elif current_steering < self.past_steering - 0.1: + current_steering = self.past_steering - 0.1 + + if current_steering >= 0: + steering = min(self.max_steer, current_steering) + else: + steering = max(-self.max_steer, current_steering) + + control.steer = steering + control.hand_brake = False + control.manual_gear_shift = False + self.past_steering = steering + + return control + + +class PIDLongitudinalController(): + """ + PIDLongitudinalController implements longitudinal control using a PID. + """ + + + def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=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 = vehicle + self._k_p = K_P + self._k_d = K_D + self._k_i = K_I + self._dt = dt + self._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) / 2 + target_speed = target_speed / 2 + 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_speed + self._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._dt + else: + _de = 0.0 + _ie = 0.0 + + return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) + + +class PIDLateralController(): + """ + PIDLateralController implements lateral control using a PID. + """ + + def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=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 = vehicle + self._k_p = K_P + self._k_d = K_D + self._k_i = K_I + self._dt = dt + self._e_buffer = deque(maxlen=10) + + def run_step(self, waypoint): + """ + Execute one step of lateral control to steer + the 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] + """ + v_begin = vehicle_transform.location + v_end = v_begin + carla.Location(x=math.cos(math.radians(vehicle_transform.rotation.yaw)), + y=math.sin(math.radians(vehicle_transform.rotation.yaw))) + + v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0]) + w_vec = np.array([waypoint.transform.location.x - + v_begin.x, waypoint.transform.location.y - + v_begin.y, 0.0]) + _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / + (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0)) + + _cross = np.cross(v_vec, w_vec) + + if _cross[2] < 0: + _dot *= -1.0 + + self._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._dt + else: + _de = 0.0 + _ie = 0.0 + + return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) + + +# ============================================================================== +# -- KK Local Planner --------------------------------------------------------- +# ============================================================================== + +class RoadOption(Enum): + """ + RoadOption represents the possible topological configurations + when moving from a segment of lane to other. + """ + VOID = -1 + LEFT = 1 + RIGHT = 2 + STRAIGHT = 3 + LANEFOLLOW = 4 + CHANGELANELEFT = 5 + CHANGELANERIGHT = 6 + + +class LocalPlanner(object): + """ + LocalPlanner implements the basic behavior of following a trajectory + of waypoints that is generated on-the-fly. + The low-level motion of the vehicle is computed by using two PID controllers, + one is used for the lateral control + and the other for the longitudinal control (cruise speed). + + When multiple paths are available (intersections) + this local planner makes a random choice. + """ + + # Minimum distance to target waypoint as a percentage + # (e.g. within 80% of total distance) + + # FPS used for dt + FPS = 20 + + def __init__(self, agent): + """ + :param agent: agent that regulates the vehicle + :param vehicle: actor to apply to local planner logic onto + """ + self._vehicle = agent.vehicle + self._map = agent.vehicle.get_world().get_map() + + self._target_speed = None + self.sampling_radius = None + self._min_distance = None + self._current_waypoint = None + self.target_road_option = None + self._next_waypoints = None + self.target_waypoint = None + self._vehicle_controller = None + self._global_plan = None + self._pid_controller = None + self.waypoints_queue = deque(maxlen=20000) # queue with tuples of (waypoint, RoadOption) + self._buffer_size = 5 + self._waypoint_buffer = deque(maxlen=self._buffer_size) + + self._init_controller() # initializing controller + + def get_distance_to_nearest_obstacle(self): + ego_vehicle_transform = self._vehicle.get_transform() + ego_vehicle_location = ego_vehicle_transform.location + + # Get the bounding box of the ego vehicle + ego_vehicle_bbox = self._vehicle.bounding_box + + # Calculate the minimum distance to other vehicles + distance_to_nearest_obstacle = None + + for actor in self._vehicle.get_world().get_actors().filter('vehicle.*'): + if actor.id != self._vehicle.id: + actor_transform = actor.get_transform() + actor_location = actor_transform.location + + # Calculate the distance between the ego vehicle and the other vehicle + distance = ego_vehicle_location.distance(actor_location) + + # Calculate the minimum distance to the other vehicle's bounding box + distance_to_obstacle = distance - ego_vehicle_bbox.extent.x - actor.bounding_box.extent.x + + if distance_to_nearest_obstacle is None or distance_to_obstacle < distance_to_nearest_obstacle: + distance_to_nearest_obstacle = distance_to_obstacle + + + return distance_to_nearest_obstacle + + def reset_vehicle(self): + """Reset the ego-vehicle""" + self._vehicle = None + print("Resetting ego-vehicle!") + + def _init_controller(self): + """ + Controller initialization. + + dt -- time difference between physics control in seconds. + This is can be fixed from server side + using the arguments -benchmark -fps=F, since dt = 1/F + + target_speed -- desired cruise speed in km/h + + min_distance -- minimum distance to remove waypoint from queue + + lateral_dict -- dictionary of arguments to setup the lateral PID controller + {'K_P':, 'K_D':, 'K_I':, 'dt'} + + longitudinal_dict -- dictionary of arguments to setup the longitudinal PID controller + {'K_P':, 'K_D':, 'K_I':, 'dt'} + """ + # Default parameters + self.args_lat_hw_dict = { + 'K_P': 0.75, + 'K_D': 0.02, + 'K_I': 0.4, + 'dt': 1.0 / self.FPS} + self.args_lat_city_dict = { + 'K_P': 0.58, + 'K_D': 0.02, + 'K_I': 0.5, + 'dt': 1.0 / self.FPS} + self.args_long_hw_dict = { + 'K_P': 0.37, + 'K_D': 0.024, + 'K_I': 0.032, + 'dt': 1.0 / self.FPS} + self.args_long_city_dict = { + 'K_P': 0.15, + 'K_D': 0.05, + 'K_I': 0.07, + 'dt': 1.0 / self.FPS} + + self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) + + self._global_plan = False + + self._target_speed = self._vehicle.get_speed_limit() + + self._min_distance = 3 + + def set_speed(self, speed): + """ + Request new target speed. + + :param speed: new target speed in km/h + """ + + self._target_speed = speed + + def set_global_plan(self, current_plan): + """ + Sets new global plan. + + :param current_plan: list of waypoints in the actual plan + """ + for elem in current_plan: + self.waypoints_queue.append(elem) + self._global_plan = True + + def get_incoming_waypoint_and_direction(self, steps=3): + """ + Returns direction and waypoint at a distance ahead defined by the user. + + :param steps: number of steps to get the incoming waypoint. + """ + if len(self.waypoints_queue) > steps: + return self.waypoints_queue[steps] + + else: + try: + wpt, direction = self.waypoints_queue[-1] + return wpt, direction + except IndexError as i: + print(i) + return None, RoadOption.VOID + return None, RoadOption.VOID + + def run_step(self, target_speed=None, debug=False): + """ + Execute one step of local planning which involves + running the longitudinal and lateral PID controllers to + follow the waypoints trajectory. + + :param target_speed: desired speed + :param debug: boolean flag to activate waypoints debugging + :return: control + """ + + if target_speed is not None: + self._target_speed = target_speed + else: + self._target_speed = self._vehicle.get_speed_limit() + + if len(self.waypoints_queue) == 0: + control = carla.VehicleControl() + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 1.0 + control.hand_brake = False + control.manual_gear_shift = False + return control + + # Buffering the waypoints + if not self._waypoint_buffer: + for i in range(self._buffer_size): + if self.waypoints_queue: + self._waypoint_buffer.append( + self.waypoints_queue.popleft()) + else: + break + + # Current vehicle waypoint + self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) + + # Target waypoint + self.target_waypoint, self.target_road_option = self._waypoint_buffer[0] + + if target_speed > 50: + args_lat = self.args_lat_hw_dict + args_long = self.args_long_hw_dict + else: + args_lat = self.args_lat_city_dict + args_long = self.args_long_city_dict + + self._pid_controller = VehiclePIDController(self._vehicle, + args_lateral=args_lat, + args_longitudinal=args_long) + + control = self._pid_controller.run_step(self._target_speed, self.target_waypoint) + + # Purge the queue of obsolete waypoints + vehicle_transform = self._vehicle.get_transform() + max_index = -1 + + for i, (waypoint, _) in enumerate(self._waypoint_buffer): + if distance_vehicle( + waypoint, vehicle_transform) < self._min_distance: + max_index = i + if max_index >= 0: + for i in range(max_index + 1): + self._waypoint_buffer.popleft() + + if debug: + draw_waypoints(self._vehicle.get_world(), + [self.target_waypoint], 1.0) + return control + + +# ============================================================================== +# -- KK Agent Class --------------------------------------------------------- +# ============================================================================== + +class AgentState(Enum): + """ + AGENT_STATE represents the possible states of a roaming agent + """ + NAVIGATING = 1 + BLOCKED_BY_VEHICLE = 2 + BLOCKED_RED_LIGHT = 3 + + +class Agent(object): + """Base class to define agents in CARLA""" + + def __init__(self, vehicle): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + """ + self._vehicle = vehicle + self._proximity_tlight_threshold = 5.0 # meters + self._proximity_vehicle_threshold = 10.0 # meters + self._local_planner = None + self._world = self._vehicle.get_world() + try: + self._map = self._world.get_map() + except RuntimeError as error: + print('RuntimeError: {}'.format(error)) + print(' The server could not send the OpenDRIVE (.xodr) file:') + print(' Make sure it exists, has the same name of your town, and is correct.') + sys.exit(1) + self._last_traffic_light = None + + def get_local_planner(self): + """Get method for protected member local planner""" + return self._local_planner + + @staticmethod + def run_step(debug=False): + """ + Execute one step of navigation. + + :param debug: boolean flag for debugging + :return: control + """ + control = carla.VehicleControl() + + if debug: + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 0.0 + control.hand_brake = False + control.manual_gear_shift = False + + return control + + @staticmethod + def emergency_stop(): + """ + Send an emergency stop command to the vehicle + + :return: control for braking + """ + control = carla.VehicleControl() + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 1.0 + control.hand_brake = False + + return control + + +# ============================================================================== +# -- KK Behavior Agent Class --------------------------------------------------------- +# ============================================================================== + +class BehaviorAgent(Agent): + """ + BehaviorAgent implements an agent that navigates scenes to reach a given + target destination, by computing the shortest possible path to it. + This agent can correctly follow traffic signs, speed limitations, + traffic lights, while also taking into account nearby vehicles. Lane changing + decisions can be taken by analyzing the surrounding environment, + such as overtaking or tailgating avoidance. Adding to these are possible + behaviors, the agent can also keep safety distance from a car in front of it + by tracking the instantaneous time to collision and keeping it in a certain range. + Finally, different sets of behaviors are encoded in the agent, from cautious + to a more aggressive ones. + """ + + def __init__(self, vehicle, ignore_traffic_light=False, behavior='normal'): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param ignore_traffic_light: boolean to ignore any traffic light + :param behavior: type of agent to apply + """ + + super(BehaviorAgent, self).__init__(vehicle) + self.vehicle = vehicle + self.ignore_traffic_light = ignore_traffic_light + self._local_planner = LocalPlanner(self) + self._grp = None + self.look_ahead_steps = 0 + + # Vehicle information + self.speed = 0 + self.speed_limit = 0 + self.direction = None + self.incoming_direction = None + self.incoming_waypoint = None + self.start_waypoint = None + self.end_waypoint = None + self.is_at_traffic_light = 0 + self.light_state = "Green" + self.light_id_to_ignore = -1 + self.min_speed = 5 + self.behavior = None + self._sampling_resolution = 4.5 + + # Parameters for agent behavior + if behavior == 'cautious': + self.behavior = Cautious() + + elif behavior == 'normal': + self.behavior = Normal() + + elif behavior == 'aggressive': + self.behavior = Aggressive() + + def update_information(self, world): + """ + This method updates the information regarding the ego + vehicle based on the surrounding world. + + :param world: carla.world object + """ + self.speed = get_speed(self.vehicle) + self.speed_limit = world.player.get_speed_limit() + # self.speed_limit = 100 # KK set speed to 100 km/h + self._local_planner.set_speed(self.speed_limit) + self.direction = self._local_planner.target_road_option + if self.direction is None: + self.direction = RoadOption.LANEFOLLOW + + self.look_ahead_steps = int(self.speed_limit / 10) + + self.incoming_waypoint, self.incoming_direction = self._local_planner.get_incoming_waypoint_and_direction( + steps=self.look_ahead_steps) + if self.incoming_direction is None: + self.incoming_direction = RoadOption.LANEFOLLOW + + self.is_at_traffic_light = world.player.is_at_traffic_light() + if self.ignore_traffic_light: + self.light_state = "Green" + else: + if self.is_at_traffic_light: + traffic_light = world.player.get_traffic_light() + self.light_state = str(traffic_light.state) + if self.light_id_to_ignore != -1 and traffic_light.id == self.light_id_to_ignore: + # Ignore this traffic light for this iteration + self.light_state = "Green" + # Get distance to the nearest obstacle + distance_to_nearest_obstacle = self._local_planner.get_distance_to_nearest_obstacle() + + # Check if the obstacle is closer than the minimum distance + if distance_to_nearest_obstacle is not None and distance_to_nearest_obstacle < 3.0: + # Apply braking if the distance to the nearest obstacle is below the threshold + self._local_planner.brake = 1.0 + else: + self._local_planner.brake = 0.0 + #else: + # This method also includes stop signs and intersections. + #self.light_state = str(self.vehicle.get_traffic_light_state()) + + def set_destination(self, start_location, end_location, clean=False): + """ + This method creates a list of waypoints from agent's position to destination location + based on the route returned by the global router. + + :param start_location: initial position + :param end_location: final position + :param clean: boolean to clean the waypoint queue + """ + if clean: + self._local_planner.waypoints_queue.clear() + self.start_waypoint = self._map.get_waypoint(start_location) + self.end_waypoint = self._map.get_waypoint(end_location) + + route_trace = self._trace_route(self.start_waypoint, self.end_waypoint) + + self._local_planner.set_global_plan(route_trace) + + def reroute(self, spawn_points): + """ + This method implements re-routing for vehicles approaching its destination. + It finds a new target and computes another path to reach it. + + :param spawn_points: list of possible destinations for the agent + """ + + print("Target almost reached, setting new destination...") + random.shuffle(spawn_points) + new_start = self._local_planner.waypoints_queue[-1][0].transform.location + destination = spawn_points[0].location if spawn_points[0].location != new_start else spawn_points[1].location + print("New destination: " + str(destination)) + + self.set_destination(new_start, destination) + + def _trace_route(self, start_waypoint, end_waypoint): + """ + This method sets up a global router and returns the + optimal route from start_waypoint to end_waypoint. + + :param start_waypoint: initial position + :param end_waypoint: final position + """ + # Setting up global router + if self._grp is None: + wld = self.vehicle.get_world() + dao = GlobalRoutePlannerDAO( + wld.get_map(), sampling_resolution=self._sampling_resolution) + grp = GlobalRoutePlanner(dao) + grp.setup() + self._grp = grp + + # Obtain route plan + route = self._grp.trace_route( + start_waypoint.transform.location, + end_waypoint.transform.location) + + return route + + def run_step(self, debug=False): + """ + Execute one step of navigation. + + :param debug: boolean for debugging + :return control: carla.VehicleControl + """ + control = carla.VehicleControl() + + ego_vehicle_loc = self.vehicle.get_location() + ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) + + if self.is_at_traffic_light and self.light_state != "Green": + # Stop at red traffic light + control = carla.VehicleControl() + control.throttle = 0.0 + control.brake = 1.0 + control.steer = 0.0 + else: + # Get distance to the nearest obstacle + distance_to_nearest_obstacle = self._local_planner.get_distance_to_nearest_obstacle() + + # Check if the obstacle is closer than the minimum distance + if distance_to_nearest_obstacle is not None and distance_to_nearest_obstacle < 3.0: + # Apply braking if the distance to the nearest obstacle is below the threshold + control.throttle = 0.0 + control.brake = 1.0 + control.steer = 0.0 + else: + # Continue following the planned route + control = self._local_planner.run_step( + target_speed=min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist), debug=debug) + self.vehicle.apply_control(control) + + return control + + +# ============================================================================== +# -- Game Loop --------------------------------------------------------- +# ============================================================================== + + def game_loop(args): + """Main loop for agent""" + pygame.init() + pygame.font.init() + world = None + tot_target_reached = 0 + num_min_waypoints = 21 + + try: + client = carla.Client(args.host, args.port) + client.set_timeout(70.0) + + display = pygame.display.set_mode( + (args.width, args.height), + pygame.HWSURFACE | pygame.DOUBLEBUF) + + hud = HUD(args.width, args.height) + world = World(client.get_world(), hud, args) + controller = KeyboardControl(world) + agent = BehaviorAgent(world.player, behavior=args.behavior) + + # 设置初始目的地 + spawn_points = world.map.get_spawn_points() + random.shuffle(spawn_points) + + start_index = 0 if spawn_points[0].location != agent.vehicle.get_location() else 1 + destination = spawn_points[start_index].location + agent.set_destination(agent.vehicle.get_location(), destination, clean=True) + + clock = pygame.time.Clock() + + while True: + clock.tick_busy_loop(60) + + # 处理控制事件 + if controller.parse_events(): + break + + # 等待服务器就绪 + if not world.world.wait_for_tick(10.0): + continue + + # 更新代理信息 + if args.agent != "Roaming" and args.agent != "Basic": + agent.update_information(world) + + # 更新和渲染世界 + world.tick(clock) + world.render(display) + pygame.display.flip() + + # 处理不同代理类型 + if args.agent == "Roaming" or args.agent == "Basic": + control = agent.run_step() + control.manual_gear_shift = False + world.player.apply_control(control) + continue # 跳过后续控制逻辑 + + # 检查是否需要重新规划路线 + waypoints_queue = agent.get_local_planner().waypoints_queue + if len(waypoints_queue) < num_min_waypoints and args.loop: + agent.reroute(spawn_points) + tot_target_reached += 1 + world.hud.notification( + f"The target has been reached {tot_target_reached} times.", + seconds=4.0 + ) + elif len(waypoints_queue) == 0 and not args.loop: + print("Target reached, mission accomplished...") + break + + # 设置速度限制并执行控制 + speed_limit = world.player.get_speed_limit() + agent.get_local_planner().set_speed(speed_limit) + control = agent.run_step() + world.player.apply_control(control) + + except Exception as e: + logging.error(f"Error in game loop: {e}") + raise + finally: + if world is not None: + world.destroy() + pygame.quit() + + def main(): + """Main method""" + argparser = argparse.ArgumentParser( + description='CARLA Automatic Control Client' + ) + + # 参数定义 + arguments = [ + ('-v', '--verbose', 'store_true', 'debug', 'Print debug information'), + ('', '--host', 'store', 'host', 'IP of the host server (default: 127.0.0.1)', '127.0.0.1'), + ('-p', '--port', 'store', 'port', 'TCP port to listen to (default: 2000)', 2000), + ('', '--res', 'store', 'res', 'Window resolution (default: 1280x720)', '1280x720'), + ('', '--filter', 'store', 'filter', 'Actor filter (default: "vehicle.*")', 'vehicle.*'), + ('', '--gamma', 'store', 'gamma', 'Gamma correction of the camera (default: 2.2)', 2.2), + ('-l', '--loop', 'store_true', 'loop', 'Sets a new random destination upon reaching the previous one'), + ('-b', '--behavior', 'store', 'behavior', + 'Choose one of the possible agent behaviors (default: normal)', 'normal', + ["cautious", "normal", "aggressive"]), + ('-a', '--agent', 'store', 'agent', 'select which agent to run', 'Behavior', + ["Behavior", "Roaming", "Basic"]), + ('-s', '--seed', 'store', 'seed', 'Set seed for repeating executions', None, int) + ] + + for arg in arguments: + flags = [f for f in arg[:2] if f] # 处理可能为空的短标志 + kwargs = { + 'dest': arg[3], + 'help': arg[4], + 'default': arg[5] if len(arg) > 5 else None + } + + if len(arg) > 6 and isinstance(arg[6], list): + kwargs['choices'] = arg[6] + if len(arg) > 7: + kwargs['type'] = arg[7] + elif arg[2] == 'store': + kwargs['type'] = str + + action = 'store_true' if arg[2] == 'store_true' else 'store' + argparser.add_argument(*flags, action=action, **kwargs) + + args = argparser.parse_args() + args.width, args.height = map(int, args.res.split('x')) + + # 设置日志 + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) + logging.info('Listening to server %s:%s', args.host, args.port) + + # 设置随机种子 + if args.seed is not None: + random.seed(args.seed) + + try: + game_loop(args) + except KeyboardInterrupt: + print('\nCancelled by user. Bye!') + except Exception as e: + logging.error(f"Unexpected error: {e}") + sys.exit(1) + + if __name__ == '__main__': + main()