From 0d5b42a9cb1e926b8e5fa2212416706a5c8d18a2 Mon Sep 17 00:00:00 2001
From: 0023csw <3485741830@qq.com>
Date: Thu, 18 Dec 2025 20:07:24 +0800
Subject: [PATCH 1/9] =?UTF-8?q?=E6=A8=A1=E5=9D=97=E5=8C=96PID=E6=8E=A7?=
=?UTF-8?q?=E5=88=B6=E4=BC=98=E5=8C=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../object_detection.py | 106 ++++++++++++------
1 file changed, 70 insertions(+), 36 deletions(-)
diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py
index 97cba38106..919f3f96c9 100644
--- a/src/Object_Tracking_Planning/object_detection.py
+++ b/src/Object_Tracking_Planning/object_detection.py
@@ -1178,7 +1178,7 @@ def _pid_control(self, target_speed, current_speed):
return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0)
-class PIDLateralController():
+class PIDLateralController:
"""
PIDLateralController implements lateral control using a PID.
"""
@@ -1187,64 +1187,98 @@ 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
+ Args:
+ vehicle: Actor to apply local planner logic onto
+ K_P: Proportional term coefficient (default: 1.0)
+ K_D: Differential term coefficient (default: 0.0)
+ K_I: Integral term coefficient (default: 0.0)
+ dt: Time differential in seconds (default: 0.03)
"""
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)
+ self._error_buffer = deque(maxlen=10)
def run_step(self, waypoint):
"""
- Execute one step of lateral control to steer
- the vehicle towards a certain waypoin.
+ Execute one step of lateral control to steer the vehicle towards a waypoint.
- :param waypoint: target waypoint
- :return: steering control in the range [-1, 1] where:
- -1 maximum steering to left
- +1 maximum steering to right
+ Args:
+ waypoint: Target waypoint
+
+ Returns:
+ float: Steering control in 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
+ Estimate steering angle based on PID equations.
- :param waypoint: target waypoint
- :param vehicle_transform: current transform of the vehicle
- :return: steering control in the range [-1, 1]
+ Args:
+ waypoint: Target waypoint
+ vehicle_transform: Current transform of the vehicle
+
+ Returns:
+ float: Steering control in range [-1, 1]
"""
+ # Calculate vehicle direction vector
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)))
+ yaw_rad = math.radians(vehicle_transform.rotation.yaw)
+ v_end = v_begin + carla.Location(
+ x=math.cos(yaw_rad),
+ y=math.sin(yaw_rad)
+ )
+ # Calculate vectors
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)
+ w_vec = np.array([
+ waypoint.transform.location.x - v_begin.x,
+ waypoint.transform.location.y - v_begin.y,
+ 0.0
+ ])
+
+ # Calculate angle difference
+ v_norm = np.linalg.norm(v_vec)
+ w_norm = np.linalg.norm(w_vec)
+ dot_product = np.dot(w_vec, v_vec) / (v_norm * w_norm)
+ dot_product_clipped = np.clip(dot_product, -1.0, 1.0)
+ angle_diff = math.acos(dot_product_clipped)
+
+ # Determine sign based on cross product
+ cross_product = np.cross(v_vec, w_vec)
+ if cross_product[2] < 0:
+ angle_diff *= -1.0
+
+ # Update error buffer and calculate PID terms
+ self._error_buffer.append(angle_diff)
+
+ # Calculate derivative and integral terms
+ derivative_error = self._calculate_derivative_error()
+ integral_error = self._calculate_integral_error()
+
+ # PID calculation with clamping
+ pid_output = (
+ self._k_p * angle_diff +
+ self._k_d * derivative_error +
+ self._k_i * integral_error
+ )
- if _cross[2] < 0:
- _dot *= -1.0
+ return np.clip(pid_output, -1.0, 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
+ def _calculate_derivative_error(self):
+ """Calculate derivative error term."""
+ if len(self._error_buffer) >= 2:
+ return (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt
+ return 0.0
- return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0)
+ def _calculate_integral_error(self):
+ """Calculate integral error term."""
+ return sum(self._error_buffer) * self._dt
# ==============================================================================
From 837902a843be986dcc615ac4ee2a23b05222a015 Mon Sep 17 00:00:00 2001
From: 0023csw <3485741830@qq.com>
Date: Fri, 19 Dec 2025 14:57:41 +0800
Subject: [PATCH 2/9] =?UTF-8?q?=E7=AE=80=E6=B4=81=E5=8C=96=E4=B8=8E?=
=?UTF-8?q?=E7=8E=AF=E5=A2=83=E7=AE=A1=E7=90=86=E4=BC=98=E5=8C=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../object_detection.py | 172 ++++++++++++------
1 file changed, 113 insertions(+), 59 deletions(-)
diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py
index 919f3f96c9..a05617ff4c 100644
--- a/src/Object_Tracking_Planning/object_detection.py
+++ b/src/Object_Tracking_Planning/object_detection.py
@@ -105,113 +105,167 @@ def get_actor_display_name(actor, truncate=250):
# ==============================================================================
class World(object):
- """ Class representing the surrounding environment """
+ """Class representing the surrounding environment"""
+
+ # 常量提取
+ DEFAULT_VEHICLE_MODEL = 'model3'
+ DEFAULT_VEHICLE_COLOR = '236,102,17'
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.args = args
+
+ self._initialize_map()
+ self._weather_presets = find_weather_presets()
+ self._weather_index = 0
+
+ # 初始化所有传感器和玩家为None
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):
+ self.restart()
+ self.world.on_tick(hud.on_world_tick)
+
+ def _initialize_map(self):
+ """初始化地图,处理可能的错误"""
+ try:
+ self.map = self.world.get_map()
+ except RuntimeError as error:
+ print(f'RuntimeError: {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)
+
+ def restart(self):
"""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]
+ # 保存相机状态
+ cam_index = self.camera_manager.index if self.camera_manager else 0
+ cam_pos_id = self.camera_manager.transform_index if self.camera_manager else 0
+
+ # 设置随机种子
+ if self.args.seed is not None:
+ random.seed(self.args.seed)
+
+ # 创建并配置车辆
+ blueprint = self._create_vehicle_blueprint()
+
+ # 生成玩家车辆
+ self._spawn_player(blueprint)
+
+ # 设置传感器
+ self._setup_sensors(cam_index, cam_pos_id)
+
+ def _create_vehicle_blueprint(self):
+ """创建并配置车辆蓝图"""
+ blueprint_lib = self.world.get_blueprint_library()
+ blueprint = blueprint_lib.filter(self.DEFAULT_VEHICLE_MODEL)[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")
+ blueprint.set_attribute('color', self.DEFAULT_VEHICLE_COLOR)
+
+ return blueprint
+
+ def _spawn_player(self, blueprint):
+ """生成玩家车辆"""
+ # 如果玩家已存在,在当前位置重新生成
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()
+ spawn_point = self._get_respawn_point()
+ self.destroy() # 销毁现有actor
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
+ # 首次生成或重新生成失败时
+ if self.player is None:
+ spawn_point = self._get_random_spawn_point()
self.player = self.world.try_spawn_actor(blueprint, spawn_point)
- # Set up the sensors.
+
+ def _get_respawn_point(self):
+ """获取重新生成点(当前位置上方)"""
+ spawn_point = self.player.get_transform()
+ spawn_point.location.z += 2.0
+ spawn_point.rotation.roll = 0.0
+ spawn_point.rotation.pitch = 0.0
+ return spawn_point
+
+ def _get_random_spawn_point(self):
+ """获取随机生成点"""
+ spawn_points = self.map.get_spawn_points()
+
+ if not 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)
+
+ return random.choice(spawn_points)
+
+ def _setup_sensors(self, cam_index, cam_pos_id):
+ """设置所有传感器"""
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 = CameraManager(self.player, self.hud, self.args.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])
+ self.hud.notification(f'Weather: {preset[1]}')
+ self.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
+ """销毁所有传感器"""
+ if self.camera_manager and self.camera_manager.sensor:
+ self.camera_manager.sensor.destroy()
+ self.camera_manager.sensor = None
+ self.camera_manager.index = None
def destroy(self):
- """Destroys all actors"""
+ """销毁所有actor"""
actors = [
- self.camera_manager.sensor,
- self.collision_sensor.sensor,
- self.lane_invasion_sensor.sensor,
- self.gnss_sensor.sensor,
- self.player]
+ self.camera_manager.sensor if self.camera_manager else None,
+ self.collision_sensor.sensor if self.collision_sensor else None,
+ self.lane_invasion_sensor.sensor if self.lane_invasion_sensor else None,
+ self.gnss_sensor.sensor if self.gnss_sensor else None,
+ self.player
+ ]
+
for actor in actors:
if actor is not None:
actor.destroy()
+ # 重置引用
+ self.player = None
+ self.collision_sensor = None
+ self.lane_invasion_sensor = None
+ self.gnss_sensor = None
+ self.camera_manager = None
+
# ==============================================================================
# -- KeyboardControl -----------------------------------------------------------
From 7dfbfeaf46b9bd20bcbff3b1efe5ae8bbc1941ed Mon Sep 17 00:00:00 2001
From: 0023csw <3485741830@qq.com>
Date: Fri, 19 Dec 2025 17:59:32 +0800
Subject: [PATCH 3/9] =?UTF-8?q?PID=E6=8E=A7=E5=88=B6=E5=99=A8=E5=86=8D?=
=?UTF-8?q?=E5=BA=A6=E6=8F=90=E5=8D=87=E6=B8=85=E6=99=B0=E5=BA=A6=E4=BC=98?=
=?UTF-8?q?=E5=8C=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../object_detection.py | 116 ++++++++++++------
1 file changed, 77 insertions(+), 39 deletions(-)
diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py
index a05617ff4c..27cc7819dc 100644
--- a/src/Object_Tracking_Planning/object_detection.py
+++ b/src/Object_Tracking_Planning/object_detection.py
@@ -1267,7 +1267,8 @@ def run_step(self, waypoint):
-1 = maximum steering to left
+1 = maximum steering to right
"""
- return self._pid_control(waypoint, self._vehicle.get_transform())
+ vehicle_transform = self._vehicle.get_transform()
+ return self._pid_control(waypoint, vehicle_transform)
def _pid_control(self, waypoint, vehicle_transform):
"""
@@ -1280,58 +1281,95 @@ def _pid_control(self, waypoint, vehicle_transform):
Returns:
float: Steering control in range [-1, 1]
"""
- # Calculate vehicle direction vector
- v_begin = vehicle_transform.location
- yaw_rad = math.radians(vehicle_transform.rotation.yaw)
- v_end = v_begin + carla.Location(
- x=math.cos(yaw_rad),
- y=math.sin(yaw_rad)
+ # 计算当前角度误差
+ angle_diff = self._calculate_angle_error(waypoint, vehicle_transform)
+
+ # 更新误差缓冲区
+ self._error_buffer.append(angle_diff)
+
+ # 计算PID输出
+ pid_output = self._calculate_pid_value(angle_diff)
+
+ # 限制输出范围
+ return np.clip(pid_output, -1.0, 1.0)
+
+ def _calculate_angle_error(self, waypoint, vehicle_transform):
+ """计算车辆当前方向与目标点方向之间的角度误差"""
+ # 获取车辆位置和方向
+ vehicle_pos = vehicle_transform.location
+ vehicle_yaw = math.radians(vehicle_transform.rotation.yaw)
+
+ # 计算车辆方向向量
+ vehicle_direction = carla.Location(
+ x=math.cos(vehicle_yaw),
+ y=math.sin(vehicle_yaw)
)
+ vehicle_vec_end = vehicle_pos + vehicle_direction
+
+ # 计算目标方向向量
+ target_pos = waypoint.transform.location
+
+ # 创建向量
+ vehicle_vec = np.array([
+ vehicle_vec_end.x - vehicle_pos.x,
+ vehicle_vec_end.y - vehicle_pos.y,
+ 0.0
+ ])
- # Calculate vectors
- 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,
+ target_vec = np.array([
+ target_pos.x - vehicle_pos.x,
+ target_pos.y - vehicle_pos.y,
0.0
])
- # Calculate angle difference
- v_norm = np.linalg.norm(v_vec)
- w_norm = np.linalg.norm(w_vec)
- dot_product = np.dot(w_vec, v_vec) / (v_norm * w_norm)
+ # 计算角度
+ return self._calculate_signed_angle(vehicle_vec, target_vec)
+
+ def _calculate_signed_angle(self, vec1, vec2):
+ """计算两个向量之间的有符号角度"""
+ # 归一化向量
+ vec1_norm = np.linalg.norm(vec1)
+ vec2_norm = np.linalg.norm(vec2)
+
+ # 避免除以零
+ if vec1_norm < 1e-6 or vec2_norm < 1e-6:
+ return 0.0
+
+ # 计算点积和夹角
+ dot_product = np.dot(vec1, vec2) / (vec1_norm * vec2_norm)
dot_product_clipped = np.clip(dot_product, -1.0, 1.0)
- angle_diff = math.acos(dot_product_clipped)
+ angle = math.acos(dot_product_clipped)
- # Determine sign based on cross product
- cross_product = np.cross(v_vec, w_vec)
+ # 根据叉积确定方向(正负)
+ cross_product = np.cross(vec1, vec2)
if cross_product[2] < 0:
- angle_diff *= -1.0
+ angle = -angle
- # Update error buffer and calculate PID terms
- self._error_buffer.append(angle_diff)
+ return angle
- # Calculate derivative and integral terms
- derivative_error = self._calculate_derivative_error()
- integral_error = self._calculate_integral_error()
+ def _calculate_pid_value(self, current_error):
+ """计算PID控制器的输出值"""
+ # 比例项
+ proportional = self._k_p * current_error
- # PID calculation with clamping
- pid_output = (
- self._k_p * angle_diff +
- self._k_d * derivative_error +
- self._k_i * integral_error
- )
+ # 微分项
+ derivative = self._k_d * self._get_derivative_error()
- return np.clip(pid_output, -1.0, 1.0)
+ # 积分项
+ integral = self._k_i * self._get_integral_error()
- def _calculate_derivative_error(self):
- """Calculate derivative error term."""
- if len(self._error_buffer) >= 2:
- return (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt
- return 0.0
+ return proportional + derivative + integral
+
+ def _get_derivative_error(self):
+ """获取微分误差项"""
+ if len(self._error_buffer) < 2:
+ return 0.0
+ return (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt
- def _calculate_integral_error(self):
- """Calculate integral error term."""
+ def _get_integral_error(self):
+ """获取积分误差项"""
+ if not self._error_buffer:
+ return 0.0
return sum(self._error_buffer) * self._dt
From ae3ff1e4c3bb66c3604b9560f2d1ad54994578e0 Mon Sep 17 00:00:00 2001
From: 0023csw <3485741830@qq.com>
Date: Fri, 19 Dec 2025 19:00:41 +0800
Subject: [PATCH 4/9] =?UTF-8?q?FadingText=E9=80=8F=E6=98=8E=E5=BA=A6?=
=?UTF-8?q?=E8=AE=A1=E7=AE=97=E4=BC=98=E5=8C=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../object_detection.py | 54 +++++++++++++++----
1 file changed, 45 insertions(+), 9 deletions(-)
diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py
index ce1e890a4b..3526889a37 100644
--- a/src/Object_Tracking_Planning/object_detection.py
+++ b/src/Object_Tracking_Planning/object_detection.py
@@ -463,7 +463,13 @@ def render(self, display):
class FadingText(object):
- """ Class for fading text """
+ """Class for fading text"""
+
+ # 常量定义
+ ALPHA_MULTIPLIER = 500.0 # 透明度计算系数
+ TEXT_OFFSET = (10, 11) # 文本偏移量
+ DEFAULT_COLOR = (255, 255, 255) # 默认文本颜色
+ DEFAULT_SECONDS = 2.0 # 默认显示时间
def __init__(self, font, dim, pos):
"""Constructor method"""
@@ -471,25 +477,56 @@ def __init__(self, font, dim, pos):
self.dim = dim
self.pos = pos
self.seconds_left = 0
+ self._init_surface()
+
+ def _init_surface(self):
+ """初始化Surface"""
self.surface = pygame.Surface(self.dim)
+ self._clear_surface()
+
+ def _clear_surface(self):
+ """清空Surface"""
+ self.surface.fill((0, 0, 0, 0))
- def set_text(self, text, color=(255, 255, 255), seconds=2.0):
+ def set_text(self, text, color=None, seconds=None):
"""Set fading text"""
+ # 使用默认值
+ if color is None:
+ color = self.DEFAULT_COLOR
+ if seconds is None:
+ seconds = self.DEFAULT_SECONDS
+
+ # 渲染文本
text_texture = self.font.render(text, True, color)
+
+ # 准备Surface
self.surface = pygame.Surface(self.dim)
self.seconds_left = seconds
- self.surface.fill((0, 0, 0, 0))
- self.surface.blit(text_texture, (10, 11))
+ self._clear_surface()
+
+ # 绘制文本
+ self.surface.blit(text_texture, self.TEXT_OFFSET)
def tick(self, _, clock):
"""Fading text method for every tick"""
- delta_seconds = 1e-3 * clock.get_time()
+ # 计算经过的时间
+ delta_seconds = clock.get_time() / 1000.0 # 转换为秒
+
+ # 更新时间
self.seconds_left = max(0.0, self.seconds_left - delta_seconds)
- self.surface.set_alpha(500.0 * self.seconds_left)
+
+ # 更新透明度
+ self._update_alpha()
+
+ def _update_alpha(self):
+ """更新Surface的透明度"""
+ alpha_value = self.seconds_left * self.ALPHA_MULTIPLIER
+ self.surface.set_alpha(alpha_value)
def render(self, display):
"""Render fading text method"""
- display.blit(self.surface, self.pos)
+ if self.seconds_left > 0:
+ display.blit(self.surface, self.pos)
# ==============================================================================
@@ -1267,8 +1304,7 @@ def run_step(self, waypoint):
-1 = maximum steering to left
+1 = maximum steering to right
"""
- vehicle_transform = self._vehicle.get_transform()
- return self._pid_control(waypoint, vehicle_transform)
+ return self._pid_control(waypoint, self._vehicle.get_transform())
def _pid_control(self, waypoint, vehicle_transform):
"""
From d49bb12066dbd8685683034cd26fdba6481b6604 Mon Sep 17 00:00:00 2001
From: 0023csw <3485741830@qq.com>
Date: Sat, 20 Dec 2025 14:53:50 +0800
Subject: [PATCH 5/9] =?UTF-8?q?=E6=A0=B9=E6=8D=AE=E5=AE=9E=E9=99=85?=
=?UTF-8?q?=E6=83=85=E5=86=B5=E5=AF=B9=E4=BA=8E=E7=8E=AF=E5=A2=83=E9=85=8D?=
=?UTF-8?q?=E7=BD=AE=E7=9A=84=E7=9B=B8=E5=BA=94=E8=B0=83=E6=95=B4=E5=9C=A8?=
=?UTF-8?q?readme=E4=B8=AD=E6=9B=B4=E6=96=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/Object_Tracking_Planning/README.md | 16 ++++++++++++++++
1 file changed, 16 insertions(+)
diff --git a/src/Object_Tracking_Planning/README.md b/src/Object_Tracking_Planning/README.md
index 72ed89f513..23ddabd6fb 100644
--- a/src/Object_Tracking_Planning/README.md
+++ b/src/Object_Tracking_Planning/README.md
@@ -1,2 +1,18 @@
这是一个关于利用yolov3和carla模拟器进行·自主车辆目标检测与轨迹规划,与模拟器相结合,以提升车辆的感知与决策能力。
+使用说明:
+使用YOLOv3算法进行物体检测
+与CARLA模拟器集成,实现真实的自动驾驶场景
+基于探测到的物体和环境约束的轨迹规划
+使用TensorBoard的性能监控与可视化
+安装
+先修条件:
+Python 3.7 或更高版本(本人使用的为py3.7.9)
+CARLA 模拟器 0.9.11(由于pycharm可能不支持0.9.11,也可自行切换成0.9.12,但同时版本也不可过于高。)
+
+项目结构:
+object_detection.py: Python主脚本,用于对象检测和轨迹规划
+requirements.txt: 必备 Python 库列表
+models/: 包含训练好的YOLOv3模型权重的目录
+config/: 包含训练过的YOLOv3模型配置的目录
+logs/: TensorBoard 日志文件用于性能监控
\ No newline at end of file
From cabea024708fd9a24d092f71067f3b46a47cfbd9 Mon Sep 17 00:00:00 2001
From: 0023csw <3485741830@qq.com>
Date: Sun, 21 Dec 2025 14:53:54 +0800
Subject: [PATCH 6/9] =?UTF-8?q?=E4=BB=A3=E7=90=86=E6=8E=A7=E5=88=B6?=
=?UTF-8?q?=E6=B5=81=E7=A8=8B=E8=BF=9B=E8=A1=8C=E4=BC=98=E5=8C=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../object_detection.py | 40 +++++++++----------
1 file changed, 18 insertions(+), 22 deletions(-)
diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py
index 3526889a37..1c4b0567cf 100644
--- a/src/Object_Tracking_Planning/object_detection.py
+++ b/src/Object_Tracking_Planning/object_detection.py
@@ -1927,6 +1927,7 @@ def run_step(self, debug=False):
def game_loop(args):
""" Main loop for agent"""
+ # 初始化部分保持不变
pygame.init()
pygame.font.init()
world = None
@@ -1942,13 +1943,9 @@ def game_loop(args):
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()
@@ -1963,57 +1960,56 @@ def game_loop(args):
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)
-
+ # 根据代理类型执行不同的逻辑
+ if args.agent in ["Roaming", "Basic"]:
+ world.world.wait_for_tick(10.0) # 等待服务器
world.tick(clock)
world.render(display)
pygame.display.flip()
+
+ # 为Roaming/Basic代理生成控制
control = agent.run_step()
control.manual_gear_shift = False
world.player.apply_control(control)
- else:
- agent.update_information(world)
+ continue # 跳过后续的控制应用
- world.tick(clock)
- world.render(display)
- pygame.display.flip()
+ # 其他代理类型的逻辑
+ 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()
From 9c24d5ab3ef59cd3c20438d7376b4f44f387aeec Mon Sep 17 00:00:00 2001
From: 0023csw <3485741830@qq.com>
Date: Sun, 21 Dec 2025 20:31:47 +0800
Subject: [PATCH 7/9] 1
---
src/Object_Tracking_Planning/README.md | 17 -----------------
1 file changed, 17 deletions(-)
diff --git a/src/Object_Tracking_Planning/README.md b/src/Object_Tracking_Planning/README.md
index 23ddabd6fb..7a87c8b2e8 100644
--- a/src/Object_Tracking_Planning/README.md
+++ b/src/Object_Tracking_Planning/README.md
@@ -1,18 +1 @@
这是一个关于利用yolov3和carla模拟器进行·自主车辆目标检测与轨迹规划,与模拟器相结合,以提升车辆的感知与决策能力。
-使用说明:
-使用YOLOv3算法进行物体检测
-与CARLA模拟器集成,实现真实的自动驾驶场景
-基于探测到的物体和环境约束的轨迹规划
-使用TensorBoard的性能监控与可视化
-
-安装
-先修条件:
-Python 3.7 或更高版本(本人使用的为py3.7.9)
-CARLA 模拟器 0.9.11(由于pycharm可能不支持0.9.11,也可自行切换成0.9.12,但同时版本也不可过于高。)
-
-项目结构:
-object_detection.py: Python主脚本,用于对象检测和轨迹规划
-requirements.txt: 必备 Python 库列表
-models/: 包含训练好的YOLOv3模型权重的目录
-config/: 包含训练过的YOLOv3模型配置的目录
-logs/: TensorBoard 日志文件用于性能监控
\ No newline at end of file
From 2c60ec9eedfed93f03e888f9fc89e3325b4e60a7 Mon Sep 17 00:00:00 2001
From: 0023csw <3485741830@qq.com>
Date: Mon, 22 Dec 2025 08:15:32 +0800
Subject: [PATCH 8/9] =?UTF-8?q?=E8=A7=84=E6=95=B4=E4=BB=A3=E7=A0=81?=
=?UTF-8?q?=E7=BB=93=E6=9E=84=E3=80=81=E7=BB=9F=E4=B8=80=E9=A3=8E=E6=A0=BC?=
=?UTF-8?q?=E3=80=81=E6=B6=88=E9=99=A4=E5=86=97=E4=BD=99?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/Object_Tracking_Planning/object_detection.py | 8 +++-----
1 file changed, 3 insertions(+), 5 deletions(-)
diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py
index 1c4b0567cf..644566f6c8 100644
--- a/src/Object_Tracking_Planning/object_detection.py
+++ b/src/Object_Tracking_Planning/object_detection.py
@@ -1643,13 +1643,13 @@ def __init__(self, vehicle):
vehicle: Actor to apply local planner logic onto
"""
self._vehicle = vehicle
- self._proximity_tlight_threshold = 5.0 # meters
- self._proximity_vehicle_threshold = 10.0 # meters
+ self._proximity_tlight_threshold = 5.0
+ self._proximity_vehicle_threshold = 10.0
self._local_planner = None
self._world = self._vehicle.get_world()
self._last_traffic_light = None
+ self._map = None
- # 初始化地图,处理可能的错误
self._init_map()
def _init_map(self):
@@ -1673,7 +1673,6 @@ def get_local_planner(self):
"""Get method for protected member local planner"""
return self._local_planner
- # 也可以同时提供属性访问方式
@property
def local_planner(self):
"""Get the local planner instance (property version)."""
@@ -1693,7 +1692,6 @@ def run_step(debug=False):
control = carla.VehicleControl()
if debug:
- # 设置调试模式下的默认控制参数
control.steer = 0.0
control.throttle = 0.0
control.brake = 0.0
From 5e29608cb84e19f459e89176767027416b87910e Mon Sep 17 00:00:00 2001
From: 0023csw <3485741830@qq.com>
Date: Mon, 22 Dec 2025 20:44:14 +0800
Subject: [PATCH 9/9] =?UTF-8?q?=E9=A1=B9=E7=9B=AE=E5=B0=81=E8=A3=85?=
=?UTF-8?q?=E6=88=90ros=E5=B9=B6=E6=8F=90=E4=BA=A4?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../carla_yolo_ros/CMakeLists.txt | 43 +
.../carla_yolo_ros/config/carla.perspective | 28 +
.../carla_yolo_ros/config/params.yaml | 3 +
.../carla_yolo_ros/launch/carla_bridge.launch | 35 +
.../carla_yolo_ros/models/coco.names | 80 +
.../carla_yolo_ros/models/yolov3.cfg | 792 ++++++
.../carla_yolo_ros/package.xml | 36 +
.../scripts/carla_ros_bridge.py | 273 +++
.../carla_yolo_ros/scripts/teleop_keyboard.py | 126 +
.../scripts/test_image_publisher.py | 108 +
.../carla_yolo_ros/src/object_detection.py | 2119 +++++++++++++++++
11 files changed, 3643 insertions(+)
create mode 100644 src/Object_Tracking_Planning/carla_yolo_ros/CMakeLists.txt
create mode 100644 src/Object_Tracking_Planning/carla_yolo_ros/config/carla.perspective
create mode 100644 src/Object_Tracking_Planning/carla_yolo_ros/config/params.yaml
create mode 100644 src/Object_Tracking_Planning/carla_yolo_ros/launch/carla_bridge.launch
create mode 100644 src/Object_Tracking_Planning/carla_yolo_ros/models/coco.names
create mode 100644 src/Object_Tracking_Planning/carla_yolo_ros/models/yolov3.cfg
create mode 100644 src/Object_Tracking_Planning/carla_yolo_ros/package.xml
create mode 100644 src/Object_Tracking_Planning/carla_yolo_ros/scripts/carla_ros_bridge.py
create mode 100644 src/Object_Tracking_Planning/carla_yolo_ros/scripts/teleop_keyboard.py
create mode 100644 src/Object_Tracking_Planning/carla_yolo_ros/scripts/test_image_publisher.py
create mode 100644 src/Object_Tracking_Planning/carla_yolo_ros/src/object_detection.py
diff --git a/src/Object_Tracking_Planning/carla_yolo_ros/CMakeLists.txt b/src/Object_Tracking_Planning/carla_yolo_ros/CMakeLists.txt
new file mode 100644
index 0000000000..2b733c4bb3
--- /dev/null
+++ b/src/Object_Tracking_Planning/carla_yolo_ros/CMakeLists.txt
@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(carla_yolo_ros)
+
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ rospy
+ std_msgs
+ sensor_msgs
+ geometry_msgs
+ nav_msgs
+ visualization_msgs
+ cv_bridge
+ image_transport
+ tf
+)
+
+catkin_package(
+ CATKIN_DEPENDS
+ roscpp
+ rospy
+ std_msgs
+ sensor_msgs
+ geometry_msgs
+ nav_msgs
+ visualization_msgs
+ cv_bridge
+ image_transport
+ tf
+)
+
+catkin_install_python(PROGRAMS
+ scripts/carla_ros_bridge.py
+ scripts/test_image_publisher.py
+ scripts/teleop_keyboard.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY
+ launch
+ config
+ models
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
\ No newline at end of file
diff --git a/src/Object_Tracking_Planning/carla_yolo_ros/config/carla.perspective b/src/Object_Tracking_Planning/carla_yolo_ros/config/carla.perspective
new file mode 100644
index 0000000000..a69391d9a2
--- /dev/null
+++ b/src/Object_Tracking_Planning/carla_yolo_ros/config/carla.perspective
@@ -0,0 +1,28 @@
+
+
+
+
+
+
+
+ help
+ Help
+
+
+
+
+
+
+
+ /carla/status
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/Object_Tracking_Planning/carla_yolo_ros/config/params.yaml b/src/Object_Tracking_Planning/carla_yolo_ros/config/params.yaml
new file mode 100644
index 0000000000..f2eb741510
--- /dev/null
+++ b/src/Object_Tracking_Planning/carla_yolo_ros/config/params.yaml
@@ -0,0 +1,3 @@
+carla_host: "127.0.0.1"
+carla_port: 2000
+yolo_confidence_threshold: 0.5
\ No newline at end of file
diff --git a/src/Object_Tracking_Planning/carla_yolo_ros/launch/carla_bridge.launch b/src/Object_Tracking_Planning/carla_yolo_ros/launch/carla_bridge.launch
new file mode 100644
index 0000000000..7b050ca279
--- /dev/null
+++ b/src/Object_Tracking_Planning/carla_yolo_ros/launch/carla_bridge.launch
@@ -0,0 +1,35 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/Object_Tracking_Planning/carla_yolo_ros/models/coco.names b/src/Object_Tracking_Planning/carla_yolo_ros/models/coco.names
new file mode 100644
index 0000000000..1f42c8eb44
--- /dev/null
+++ b/src/Object_Tracking_Planning/carla_yolo_ros/models/coco.names
@@ -0,0 +1,80 @@
+person
+bicycle
+car
+motorcycle
+airplane
+bus
+train
+truck
+boat
+traffic light
+fire hydrant
+stop sign
+parking meter
+bench
+bird
+cat
+dog
+horse
+sheep
+cow
+elephant
+bear
+zebra
+giraffe
+backpack
+umbrella
+handbag
+tie
+suitcase
+frisbee
+skis
+snowboard
+sports ball
+kite
+baseball bat
+baseball glove
+skateboard
+surfboard
+tennis racket
+bottle
+wine glass
+cup
+fork
+knife
+spoon
+bowl
+banana
+apple
+sandwich
+orange
+broccoli
+carrot
+hot dog
+pizza
+donut
+cake
+chair
+couch
+potted plant
+bed
+dining table
+toilet
+tv
+laptop
+mouse
+remote
+keyboard
+cell phone
+microwave
+oven
+toaster
+sink
+refrigerator
+book
+clock
+vase
+scissors
+teddy bear
+hair drier
+toothbrush
\ No newline at end of file
diff --git a/src/Object_Tracking_Planning/carla_yolo_ros/models/yolov3.cfg b/src/Object_Tracking_Planning/carla_yolo_ros/models/yolov3.cfg
new file mode 100644
index 0000000000..3faed378e2
--- /dev/null
+++ b/src/Object_Tracking_Planning/carla_yolo_ros/models/yolov3.cfg
@@ -0,0 +1,792 @@
+[net]
+# Testing
+#batch=1
+#subdivisions=1
+# Training
+batch=64
+subdivisions=64
+width=608
+height=608
+channels=3
+momentum=0.9
+decay=0.0005
+angle=50
+flip=1
+saturation = 1.5
+exposure = 1.5
+hue=.1
+gassian_noise=1
+mosiac=1
+
+learning_rate=0.00001
+burn_in=1000
+max_batches = 10000
+policy=steps
+steps=3600,4000
+scales=.1,.1
+
+[convolutional]
+batch_normalize=1
+filters=32
+size=3
+stride=1
+pad=1
+activation=leaky
+
+# Downsample
+
+[convolutional]
+batch_normalize=1
+filters=64
+size=3
+stride=2
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=32
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=64
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+# Downsample
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=3
+stride=2
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=64
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=64
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+# Downsample
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=2
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+# Downsample
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=2
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+# Downsample
+
+[convolutional]
+batch_normalize=1
+filters=1024
+size=3
+stride=2
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=1024
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=1024
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=1024
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=1024
+size=3
+stride=1
+pad=1
+activation=leaky
+
+[shortcut]
+from=-3
+activation=linear
+
+######################
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=1024
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=1024
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=512
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=1024
+activation=leaky
+
+[convolutional]
+size=1
+stride=1
+pad=1
+filters=18
+activation=linear
+
+
+[yolo]
+mask = 6,7,8
+anchors = 33, 23, 24, 47, 60, 26, 42, 38, 46, 56, 70, 43, 54, 87, 87, 63, 107,112
+classes=1
+num=9
+jitter=.3
+ignore_thresh = .7
+truth_thresh = 1
+random=1
+
+
+[route]
+layers = -4
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[upsample]
+stride=2
+
+[route]
+layers = -1, 61
+
+
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=512
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=512
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=256
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=512
+activation=leaky
+
+[convolutional]
+size=1
+stride=1
+pad=1
+filters=18
+activation=linear
+
+
+[yolo]
+mask = 3,4,5
+anchors = 33, 23, 24, 47, 60, 26, 42, 38, 46, 56, 70, 43, 54, 87, 87, 63, 107,112
+classes=1
+num=9
+jitter=.3
+ignore_thresh = .7
+truth_thresh = 1
+random=1
+
+
+
+[route]
+layers = -4
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[upsample]
+stride=2
+
+[route]
+layers = -1, 36
+
+
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=256
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=256
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+filters=128
+size=1
+stride=1
+pad=1
+activation=leaky
+
+[convolutional]
+batch_normalize=1
+size=3
+stride=1
+pad=1
+filters=256
+activation=leaky
+
+[convolutional]
+size=1
+stride=1
+pad=1
+filters=18
+activation=linear
+
+
+[yolo]
+mask = 0,1,2
+anchors = 33, 23, 24, 47, 60, 26, 42, 38, 46, 56, 70, 43, 54, 87, 87, 63, 107,112
+classes=1
+num=9
+jitter=.3
+ignore_thresh = .7
+truth_thresh = 1
+random=1
+
diff --git a/src/Object_Tracking_Planning/carla_yolo_ros/package.xml b/src/Object_Tracking_Planning/carla_yolo_ros/package.xml
new file mode 100644
index 0000000000..a19a154b2e
--- /dev/null
+++ b/src/Object_Tracking_Planning/carla_yolo_ros/package.xml
@@ -0,0 +1,36 @@
+
+
+ carla_yolo_ros
+ 1.0.0
+ ROS integration for CARLA autonomous vehicle with YOLOv3 object detection
+ lihuashi
+ MIT
+
+ catkin
+
+ roscpp
+ rospy
+ std_msgs
+ sensor_msgs
+ geometry_msgs
+ nav_msgs
+ visualization_msgs
+ cv_bridge
+ image_transport
+ tf
+
+ roscpp
+ rospy
+ std_msgs
+ sensor_msgs
+ geometry_msgs
+ nav_msgs
+ visualization_msgs
+ cv_bridge
+ image_transport
+ tf
+ rqt_gui
+ rqt_image_view
+ rqt_console
+ rqt_topic
+
\ No newline at end of file
diff --git a/src/Object_Tracking_Planning/carla_yolo_ros/scripts/carla_ros_bridge.py b/src/Object_Tracking_Planning/carla_yolo_ros/scripts/carla_ros_bridge.py
new file mode 100644
index 0000000000..321f445176
--- /dev/null
+++ b/src/Object_Tracking_Planning/carla_yolo_ros/scripts/carla_ros_bridge.py
@@ -0,0 +1,273 @@
+ def control_cmd_callback(self, msg):
+ """处理控制命令"""
+ rospy.loginfo(f"Received control command: linear={msg.linear.x}, angular={msg.angular.z}")
+
+ # 发布状态
+ status_msg = String()
+ status_msg.data = f"Control: throttle={msg.linear.x}, steer={msg.angular.z}"
+ self.status_pub.publish(status_msg)
+
+ # 如果连接到CARLA,应用控制
+ if self.carla_connected and hasattr(self, 'vehicle'):
+ try:
+ import carla
+
+ # 创建CARLA控制命令
+ control = carla.VehicleControl()
+ control.throttle = max(0.0, min(1.0, msg.linear.x)) # 限制在0-1之间
+ control.brake = max(0.0, min(1.0, -msg.linear.x)) if msg.linear.x < 0 else 0.0
+ control.steer = max(-1.0, min(1.0, msg.angular.z))
+ control.hand_brake = False
+ control.manual_gear_shift = False
+
+ # 应用控制
+ self.vehicle.apply_control(control)
+
+ except Exception as e:
+ rospy.logerr(f"Error applying CARLA control: {e}")
+
+ def start_simulation_callback(self, msg):
+ """处理启动仿真命令"""
+ if msg.data:
+ rospy.loginfo("Starting simulation...")
+ self.publish_status("Simulation starting")
+
+ # 如果使用CARLA模式,启动代理
+ if self.carla_connected and hasattr(self, 'agent'):
+ try:
+ # 这里可以启动自动驾驶代理
+ rospy.loginfo("CARLA agent ready")
+ except Exception as e:
+ rospy.logerr(f"Error starting CARLA agent: {e}")
+
+ def stop_simulation_callback(self, msg):
+ """处理停止仿真命令"""
+ if msg.data:
+ rospy.loginfo("Stopping simulation...")
+ self.publish_status("Simulation stopping")
+
+ # 停止车辆
+ if self.carla_connected and hasattr(self, 'vehicle'):
+ try:
+ control = carla.VehicleControl()
+ control.throttle = 0.0
+ control.brake = 1.0
+ control.steer = 0.0
+ self.vehicle.apply_control(control)
+ except Exception as e:
+ rospy.logerr(f"Error stopping vehicle: {e}")
+
+ def image_callback(self, msg):
+ """处理ROS图像消息"""
+ if self.simulation_mode == 'ros_only':
+ try:
+ # 转换为OpenCV格式
+ cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+
+ # 使用YOLO进行对象检测(简化版本)
+ detections = self.detect_objects_simple(cv_image)
+
+ # 发布检测结果
+ if detections:
+ detection_msg = String()
+ detection_msg.data = f"Detected {len(detections)} objects: {', '.join(detections)}"
+ self.detection_pub.publish(detection_msg)
+
+ # 可视化并发布图像
+ vis_image = self.draw_detections(cv_image.copy(), detections)
+ ros_image = self.bridge.cv2_to_imgmsg(vis_image, "bgr8")
+ ros_image.header = msg.header
+ self.detection_image_pub.publish(ros_image)
+
+ rospy.logdebug("Processed ROS image with object detection")
+
+ except Exception as e:
+ rospy.logerr(f"Error processing image: {e}")
+
+ def detect_objects_simple(self, image):
+ """简单的对象检测(用于演示)"""
+ import cv2
+ import numpy as np
+
+ # 这里实现一个简化的检测逻辑
+ # 在实际应用中,应该使用你的YOLOv3模型
+
+ # 检查图像中是否有特定的颜色区域
+ # 这是一个简单的演示,实际应该加载YOLO模型
+
+ detections = []
+
+ try:
+ # 尝试加载YOLO模型
+ model_dir = os.path.join(os.path.dirname(__file__), '..', 'models')
+ cfg_path = os.path.join(model_dir, 'yolov3.cfg')
+ weights_path = os.path.join(model_dir, 'yolov3.weights')
+ names_path = os.path.join(model_dir, 'coco.names')
+
+ if os.path.exists(cfg_path) and os.path.exists(weights_path):
+ # 读取类别名称
+ with open(names_path, 'r') as f:
+ classes = [line.strip() for line in f.readlines()]
+
+ # 这里可以添加实际的YOLO检测代码
+ # 为了简化,我们只返回一些模拟检测
+ detections = ['car', 'person', 'traffic light']
+ else:
+ rospy.logwarn("YOLO model files not found, using simulated detections")
+ detections = ['simulated_car', 'simulated_person']
+
+ except Exception as e:
+ rospy.logerr(f"Error in detection: {e}")
+ detections = ['error']
+
+ return detections
+
+ def draw_detections(self, image, detections):
+ """在图像上绘制检测结果"""
+ import cv2
+
+ height, width = image.shape[:2]
+
+ for i, obj in enumerate(detections):
+ # 计算位置
+ x = int(width * 0.1)
+ y = int(height * 0.1 * (i + 1))
+
+ # 绘制文本
+ cv2.putText(image, obj, (x, y),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
+
+ # 绘制边界框
+ cv2.rectangle(image, (x-5, y-25), (x+100, y+5), (0, 255, 0), 2)
+
+ # 添加水印
+ cv2.putText(image, "ROS-CARLA Bridge", (10, height-10),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
+
+ return image
+
+ def update_carla_state(self):
+ """更新CARLA状态(如果连接)"""
+ if self.carla_connected and hasattr(self, 'vehicle'):
+ try:
+ # 获取车辆变换
+ transform = self.vehicle.get_transform()
+ velocity = self.vehicle.get_velocity()
+
+ # 计算速度(m/s)
+ speed = (velocity.x**2 + velocity.y**2 + velocity.z**2)**0.5
+
+ # 发布速度
+ speed_msg = Float32()
+ speed_msg.data = speed
+ self.velocity_pub.publish(speed_msg)
+
+ # 发布里程计
+ odom = self.create_odometry_message(transform, velocity)
+ self.odometry_pub.publish(odom)
+
+ # 更新状态
+ status_msg = String()
+ status_msg.data = f"CARLA: Speed={speed:.2f}m/s, Location=({transform.location.x:.1f}, {transform.location.y:.1f})"
+ self.status_pub.publish(status_msg)
+
+ except Exception as e:
+ rospy.logerr(f"Error updating CARLA state: {e}")
+
+ def create_odometry_message(self, transform, velocity):
+ """创建ROS里程计消息"""
+ from nav_msgs.msg import Odometry
+ import tf.transformations as tf_trans
+ import numpy as np
+
+ odom = Odometry()
+ odom.header.stamp = rospy.Time.now()
+ odom.header.frame_id = "carla_map"
+ odom.child_frame_id = "carla_vehicle"
+
+ # 位置
+ odom.pose.pose.position.x = transform.location.x
+ odom.pose.pose.position.y = -transform.location.y # CARLA使用左手坐标系
+ odom.pose.pose.position.z = transform.location.z
+
+ # 方向(四元数)
+ roll = np.deg2rad(transform.rotation.roll)
+ pitch = np.deg2rad(transform.rotation.pitch)
+ yaw = np.deg2rad(-transform.rotation.yaw)
+
+ q = tf_trans.quaternion_from_euler(roll, pitch, yaw)
+ odom.pose.pose.orientation.x = q[0]
+ odom.pose.pose.orientation.y = q[1]
+ odom.pose.pose.orientation.z = q[2]
+ odom.pose.pose.orientation.w = q[3]
+
+ # 速度
+ odom.twist.twist.linear.x = velocity.x
+ odom.twist.twist.linear.y = -velocity.y
+ odom.twist.twist.linear.z = velocity.z
+
+ return odom
+
+ def publish_status(self, message):
+ """发布状态消息"""
+ msg = String()
+ msg.data = message
+ self.status_pub.publish(msg)
+
+ def run(self):
+ """主循环"""
+ rate = rospy.Rate(self.publish_rate)
+
+ rospy.loginfo(f"{self.node_name} running at {self.publish_rate}Hz")
+
+ while not rospy.is_shutdown() and self.running:
+ try:
+ # 更新CARLA状态
+ if self.carla_connected:
+ self.update_carla_state()
+
+ # 发布心跳
+ self.publish_status(f"{self.node_name} running. CARLA: {self.carla_connected}")
+
+ rate.sleep()
+
+ except rospy.ROSInterruptException:
+ break
+ except Exception as e:
+ rospy.logerr(f"Error in main loop: {e}")
+ time.sleep(1.0)
+
+ def shutdown(self):
+ """关闭节点"""
+ rospy.loginfo(f"Shutting down {self.node_name}")
+ self.running = False
+
+ # 清理CARLA资源
+ if self.carla_connected:
+ try:
+ if hasattr(self, 'vehicle'):
+ self.vehicle.destroy()
+ rospy.loginfo("CARLA vehicle destroyed")
+ except Exception as e:
+ rospy.logerr(f"Error during CARLA cleanup: {e}")
+
+def main():
+ try:
+ # 创建桥接器实例
+ bridge = CARLAROSBridge()
+
+ # 注册关闭处理
+ rospy.on_shutdown(bridge.shutdown)
+
+ # 运行主循环
+ bridge.run()
+
+ except rospy.ROSInterruptException:
+ rospy.loginfo("Node interrupted by ROS")
+ except Exception as e:
+ rospy.logerr(f"Fatal error in main: {e}")
+ finally:
+ rospy.loginfo("CARLA ROS Bridge shutdown complete")
+
+if __name__ == '__main__':
+ main()
diff --git a/src/Object_Tracking_Planning/carla_yolo_ros/scripts/teleop_keyboard.py b/src/Object_Tracking_Planning/carla_yolo_ros/scripts/teleop_keyboard.py
new file mode 100644
index 0000000000..35590a05b7
--- /dev/null
+++ b/src/Object_Tracking_Planning/carla_yolo_ros/scripts/teleop_keyboard.py
@@ -0,0 +1,126 @@
+#!/usr/bin/env python3
+"""
+键盘遥操作节点 - 用于控制CARLA车辆
+"""
+
+import rospy
+import sys
+import select
+import termios
+import tty
+from geometry_msgs.msg import Twist
+from std_msgs.msg import String
+
+class TeleopKeyboard:
+ def __init__(self):
+ rospy.init_node('teleop_keyboard', anonymous=True)
+
+ # 控制参数
+ self.linear_speed = rospy.get_param('~linear_speed', 0.5)
+ self.angular_speed = rospy.get_param('~angular_speed', 0.5)
+
+ # 发布者
+ self.cmd_pub = rospy.Publisher('/carla/control_cmd', Twist, queue_size=10)
+ self.instruction_pub = rospy.Publisher('/teleop/instructions', String, queue_size=10)
+
+ # 保存终端设置
+ self.settings = termios.tcgetattr(sys.stdin)
+
+ rospy.loginfo("Teleop Keyboard Node Initialized")
+ rospy.loginfo("Control keys:")
+ rospy.loginfo(" W: Forward")
+ rospy.loginfo(" S: Backward")
+ rospy.loginfo(" A: Left")
+ rospy.loginfo(" D: Right")
+ rospy.loginfo(" Space: Stop")
+ rospy.loginfo(" Q: Quit")
+
+ def get_key(self):
+ """获取键盘输入"""
+ tty.setraw(sys.stdin.fileno())
+ select.select([sys.stdin], [], [], 0)
+ key = sys.stdin.read(1)
+ termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
+ return key
+
+ def publish_instruction(self, message):
+ """发布指令说明"""
+ msg = String()
+ msg.data = message
+ self.instruction_pub.publish(msg)
+
+ def run(self):
+ """主循环"""
+ try:
+ self.publish_instruction("Use WASD to control, Space to stop, Q to quit")
+
+ while not rospy.is_shutdown():
+ key = self.get_key()
+
+ # 创建控制消息
+ twist = Twist()
+
+ if key == 'w' or key == 'W':
+ # 前进
+ twist.linear.x = self.linear_speed
+ twist.angular.z = 0.0
+ self.publish_instruction("Moving FORWARD")
+
+ elif key == 's' or key == 'S':
+ # 后退
+ twist.linear.x = -self.linear_speed
+ twist.angular.z = 0.0
+ self.publish_instruction("Moving BACKWARD")
+
+ elif key == 'a' or key == 'A':
+ # 左转
+ twist.linear.x = self.linear_speed * 0.5
+ twist.angular.z = self.angular_speed
+ self.publish_instruction("Turning LEFT")
+
+ elif key == 'd' or key == 'D':
+ # 右转
+ twist.linear.x = self.linear_speed * 0.5
+ twist.angular.z = -self.angular_speed
+ self.publish_instruction("Turning RIGHT")
+
+ elif key == ' ':
+ # 停止
+ twist.linear.x = 0.0
+ twist.angular.z = 0.0
+ self.publish_instruction("STOPPING")
+
+ elif key == 'q' or key == 'Q':
+ # 退出
+ rospy.loginfo("Quitting teleop")
+ break
+
+ else:
+ # 无效按键
+ continue
+
+ # 发布控制命令
+ self.cmd_pub.publish(twist)
+ rospy.loginfo(f"Command: linear={twist.linear.x:.2f}, angular={twist.angular.z:.2f}")
+
+ except Exception as e:
+ rospy.logerr(f"Error in teleop: {e}")
+
+ finally:
+ # 恢复终端设置
+ termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
+
+ # 发送停止命令
+ stop_twist = Twist()
+ stop_twist.linear.x = 0.0
+ stop_twist.angular.z = 0.0
+ self.cmd_pub.publish(stop_twist)
+
+ rospy.loginfo("Teleop Keyboard Node Shutdown")
+
+def main():
+ teleop = TeleopKeyboard()
+ teleop.run()
+
+if __name__ == '__main__':
+ main()
diff --git a/src/Object_Tracking_Planning/carla_yolo_ros/scripts/test_image_publisher.py b/src/Object_Tracking_Planning/carla_yolo_ros/scripts/test_image_publisher.py
new file mode 100644
index 0000000000..9d5b9207d9
--- /dev/null
+++ b/src/Object_Tracking_Planning/carla_yolo_ros/scripts/test_image_publisher.py
@@ -0,0 +1,108 @@
+#!/usr/bin/env python3
+"""
+测试图像发布器 - 为ROS节点提供测试图像
+"""
+
+import rospy
+import cv2
+import numpy as np
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+import time
+
+class TestImagePublisher:
+ def __init__(self):
+ rospy.init_node('test_image_publisher', anonymous=True)
+
+ # 参数
+ self.publish_rate = rospy.get_param('~publish_rate', 5) # Hz
+ self.image_width = rospy.get_param('~width', 640)
+ self.image_height = rospy.get_param('~height', 480)
+
+ # 发布者
+ self.image_pub = rospy.Publisher('/camera/image_raw', Image, queue_size=10)
+ self.status_pub = rospy.Publisher('/test/status', String, queue_size=10)
+
+ self.bridge = CvBridge()
+ self.frame_count = 0
+
+ rospy.loginfo(f"Test Image Publisher initialized: {self.image_width}x{self.image_height} @ {self.publish_rate}Hz")
+
+ def create_test_image(self):
+ """创建测试图像"""
+ # 创建黑色背景
+ image = np.zeros((self.image_height, self.image_width, 3), dtype=np.uint8)
+
+ # 添加一些模拟对象
+ colors = [(0, 255, 0), (255, 0, 0), (0, 0, 255), (255, 255, 0)]
+ objects = ['Car', 'Person', 'Bicycle', 'Traffic Light']
+
+ for i, (color, obj) in enumerate(zip(colors, objects)):
+ # 计算位置
+ x = 100 + i * 120
+ y = 100 + (i % 2) * 150
+
+ # 绘制对象
+ cv2.rectangle(image, (x, y), (x + 80, y + 60), color, 2)
+ cv2.putText(image, obj, (x, y - 10),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)
+
+ # 添加检测标记
+ cv2.circle(image, (x + 40, y + 30), 5, color, -1)
+
+ # 添加帧计数和水印
+ cv2.putText(image, f"Frame: {self.frame_count}", (10, 30),
+ cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
+ cv2.putText(image, "Test Image for YOLO Detection", (10, self.image_height - 20),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 200, 200), 1)
+
+ self.frame_count += 1
+ return image
+
+ def publish_status(self, message):
+ """发布状态"""
+ from std_msgs.msg import String
+ msg = String()
+ msg.data = message
+ self.status_pub.publish(msg)
+
+ def run(self):
+ """主循环"""
+ rate = rospy.Rate(self.publish_rate)
+
+ rospy.loginfo("Starting test image publishing...")
+
+ while not rospy.is_shutdown():
+ try:
+ # 创建测试图像
+ image = self.create_test_image()
+
+ # 转换为ROS消息
+ ros_image = self.bridge.cv2_to_imgmsg(image, "bgr8")
+ ros_image.header.stamp = rospy.Time.now()
+ ros_image.header.frame_id = "test_camera"
+
+ # 发布图像
+ self.image_pub.publish(ros_image)
+
+ # 发布状态
+ self.publish_status(f"Published frame {self.frame_count}")
+
+ rospy.logdebug(f"Published test image frame {self.frame_count}")
+
+ rate.sleep()
+
+ except rospy.ROSInterruptException:
+ break
+ except Exception as e:
+ rospy.logerr(f"Error publishing image: {e}")
+ time.sleep(1.0)
+
+ rospy.loginfo("Test Image Publisher stopped")
+
+def main():
+ publisher = TestImagePublisher()
+ publisher.run()
+
+if __name__ == '__main__':
+ main()
diff --git a/src/Object_Tracking_Planning/carla_yolo_ros/src/object_detection.py b/src/Object_Tracking_Planning/carla_yolo_ros/src/object_detection.py
new file mode 100644
index 0000000000..644566f6c8
--- /dev/null
+++ b/src/Object_Tracking_Planning/carla_yolo_ros/src/object_detection.py
@@ -0,0 +1,2119 @@
+"""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"""
+
+ # 常量提取
+ DEFAULT_VEHICLE_MODEL = 'model3'
+ DEFAULT_VEHICLE_COLOR = '236,102,17'
+
+ def __init__(self, carla_world, hud, args):
+ """Constructor method"""
+ self.world = carla_world
+ self.hud = hud
+ self.args = args
+
+ self._initialize_map()
+ self._weather_presets = find_weather_presets()
+ self._weather_index = 0
+
+ # 初始化所有传感器和玩家为None
+ self.player = None
+ self.collision_sensor = None
+ self.lane_invasion_sensor = None
+ self.gnss_sensor = None
+ self.camera_manager = None
+
+ self.recording_enabled = False
+ self.recording_start = 0
+
+ self.restart()
+ self.world.on_tick(hud.on_world_tick)
+
+ def _initialize_map(self):
+ """初始化地图,处理可能的错误"""
+ try:
+ self.map = self.world.get_map()
+ except RuntimeError as error:
+ print(f'RuntimeError: {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)
+
+ def restart(self):
+ """Restart the world"""
+ # 保存相机状态
+ cam_index = self.camera_manager.index if self.camera_manager else 0
+ cam_pos_id = self.camera_manager.transform_index if self.camera_manager else 0
+
+ # 设置随机种子
+ if self.args.seed is not None:
+ random.seed(self.args.seed)
+
+ # 创建并配置车辆
+ blueprint = self._create_vehicle_blueprint()
+
+ # 生成玩家车辆
+ self._spawn_player(blueprint)
+
+ # 设置传感器
+ self._setup_sensors(cam_index, cam_pos_id)
+
+ def _create_vehicle_blueprint(self):
+ """创建并配置车辆蓝图"""
+ blueprint_lib = self.world.get_blueprint_library()
+ blueprint = blueprint_lib.filter(self.DEFAULT_VEHICLE_MODEL)[0]
+ blueprint.set_attribute('role_name', 'hero')
+
+ if blueprint.has_attribute('color'):
+ blueprint.set_attribute('color', self.DEFAULT_VEHICLE_COLOR)
+
+ return blueprint
+
+ def _spawn_player(self, blueprint):
+ """生成玩家车辆"""
+ # 如果玩家已存在,在当前位置重新生成
+ if self.player is not None:
+ spawn_point = self._get_respawn_point()
+ self.destroy() # 销毁现有actor
+ self.player = self.world.try_spawn_actor(blueprint, spawn_point)
+
+ # 首次生成或重新生成失败时
+ if self.player is None:
+ spawn_point = self._get_random_spawn_point()
+ self.player = self.world.try_spawn_actor(blueprint, spawn_point)
+
+ def _get_respawn_point(self):
+ """获取重新生成点(当前位置上方)"""
+ spawn_point = self.player.get_transform()
+ spawn_point.location.z += 2.0
+ spawn_point.rotation.roll = 0.0
+ spawn_point.rotation.pitch = 0.0
+ return spawn_point
+
+ def _get_random_spawn_point(self):
+ """获取随机生成点"""
+ spawn_points = self.map.get_spawn_points()
+
+ if not 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)
+
+ return random.choice(spawn_points)
+
+ def _setup_sensors(self, cam_index, cam_pos_id):
+ """设置所有传感器"""
+ 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.args.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):
+ """切换到下一个天气预设"""
+ 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(f'Weather: {preset[1]}')
+ self.world.set_weather(preset[0])
+
+ def tick(self, clock):
+ """每一帧的更新方法"""
+ self.hud.tick(self, clock)
+
+ def render(self, display):
+ """渲染世界"""
+ self.camera_manager.render(display)
+ self.hud.render(display)
+
+ def destroy_sensors(self):
+ """销毁所有传感器"""
+ if self.camera_manager and self.camera_manager.sensor:
+ self.camera_manager.sensor.destroy()
+ self.camera_manager.sensor = None
+ self.camera_manager.index = None
+
+ def destroy(self):
+ """销毁所有actor"""
+ actors = [
+ self.camera_manager.sensor if self.camera_manager else None,
+ self.collision_sensor.sensor if self.collision_sensor else None,
+ self.lane_invasion_sensor.sensor if self.lane_invasion_sensor else None,
+ self.gnss_sensor.sensor if self.gnss_sensor else None,
+ self.player
+ ]
+
+ for actor in actors:
+ if actor is not None:
+ actor.destroy()
+
+ # 重置引用
+ self.player = None
+ self.collision_sensor = None
+ self.lane_invasion_sensor = None
+ self.gnss_sensor = None
+ self.camera_manager = None
+
+
+# ==============================================================================
+# -- 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"""
+
+ # 常量定义
+ ALPHA_MULTIPLIER = 500.0 # 透明度计算系数
+ TEXT_OFFSET = (10, 11) # 文本偏移量
+ DEFAULT_COLOR = (255, 255, 255) # 默认文本颜色
+ DEFAULT_SECONDS = 2.0 # 默认显示时间
+
+ def __init__(self, font, dim, pos):
+ """Constructor method"""
+ self.font = font
+ self.dim = dim
+ self.pos = pos
+ self.seconds_left = 0
+ self._init_surface()
+
+ def _init_surface(self):
+ """初始化Surface"""
+ self.surface = pygame.Surface(self.dim)
+ self._clear_surface()
+
+ def _clear_surface(self):
+ """清空Surface"""
+ self.surface.fill((0, 0, 0, 0))
+
+ def set_text(self, text, color=None, seconds=None):
+ """Set fading text"""
+ # 使用默认值
+ if color is None:
+ color = self.DEFAULT_COLOR
+ if seconds is None:
+ seconds = self.DEFAULT_SECONDS
+
+ # 渲染文本
+ text_texture = self.font.render(text, True, color)
+
+ # 准备Surface
+ self.surface = pygame.Surface(self.dim)
+ self.seconds_left = seconds
+ self._clear_surface()
+
+ # 绘制文本
+ self.surface.blit(text_texture, self.TEXT_OFFSET)
+
+ def tick(self, _, clock):
+ """Fading text method for every tick"""
+ # 计算经过的时间
+ delta_seconds = clock.get_time() / 1000.0 # 转换为秒
+
+ # 更新时间
+ self.seconds_left = max(0.0, self.seconds_left - delta_seconds)
+
+ # 更新透明度
+ self._update_alpha()
+
+ def _update_alpha(self):
+ """更新Surface的透明度"""
+ alpha_value = self.seconds_left * self.ALPHA_MULTIPLIER
+ self.surface.set_alpha(alpha_value)
+
+ def render(self, display):
+ """Render fading text method"""
+ if self.seconds_left > 0:
+ 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.
+
+ Args:
+ vehicle: Actor to apply local planner logic onto
+ K_P: Proportional term coefficient (default: 1.0)
+ K_D: Differential term coefficient (default: 0.0)
+ K_I: Integral term coefficient (default: 0.0)
+ dt: Time differential in seconds (default: 0.03)
+ """
+ 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, waypoint):
+ """
+ Execute one step of lateral control to steer the vehicle towards a waypoint.
+
+ Args:
+ waypoint: Target waypoint
+
+ Returns:
+ float: Steering control in 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 steering angle based on PID equations.
+
+ Args:
+ waypoint: Target waypoint
+ vehicle_transform: Current transform of the vehicle
+
+ Returns:
+ float: Steering control in range [-1, 1]
+ """
+ # Calculate vehicle direction vector
+ v_begin = vehicle_transform.location
+ yaw_rad = math.radians(vehicle_transform.rotation.yaw)
+ v_end = v_begin + carla.Location(
+ x=math.cos(yaw_rad),
+ y=math.sin(yaw_rad)
+ )
+
+ # Calculate vectors
+ 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
+ ])
+
+ # Calculate angle difference
+ v_norm = np.linalg.norm(v_vec)
+ w_norm = np.linalg.norm(w_vec)
+ dot_product = np.dot(w_vec, v_vec) / (v_norm * w_norm)
+ dot_product_clipped = np.clip(dot_product, -1.0, 1.0)
+ angle_diff = math.acos(dot_product_clipped)
+
+ # Determine sign based on cross product
+ cross_product = np.cross(v_vec, w_vec)
+ if cross_product[2] < 0:
+ angle_diff *= -1.0
+
+ # Update error buffer and calculate PID terms
+ self._error_buffer.append(angle_diff)
+
+ # Calculate derivative and integral terms
+ derivative_error = self._calculate_derivative_error()
+ integral_error = self._calculate_integral_error()
+
+ # PID calculation with clamping
+ pid_output = (
+ self._k_p * angle_diff +
+ self._k_d * derivative_error +
+ self._k_i * integral_error
+ )
+
+ return np.clip(pid_output, -1.0, 1.0)
+
+ def _calculate_derivative_error(self):
+ """Calculate derivative error term."""
+ if len(self._error_buffer) >= 2:
+ return (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt
+ return 0.0
+
+ def _calculate_integral_error(self):
+ """Calculate integral error term."""
+ return sum(self._error_buffer) * self._dt
+
+
+# ==============================================================================
+# -- 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:
+ """Base class to define agents in CARLA"""
+
+ def __init__(self, vehicle):
+ """
+ Constructor method.
+
+ Args:
+ vehicle: Actor to apply local planner logic onto
+ """
+ self._vehicle = vehicle
+ self._proximity_tlight_threshold = 5.0
+ self._proximity_vehicle_threshold = 10.0
+ self._local_planner = None
+ self._world = self._vehicle.get_world()
+ self._last_traffic_light = None
+ self._map = None
+
+ self._init_map()
+
+ def _init_map(self):
+ """Initialize the map with error handling."""
+ try:
+ self._map = self._world.get_map()
+ except RuntimeError as error:
+ self._handle_map_error(error)
+
+ def _handle_map_error(self, error):
+ """Handle map initialization error."""
+ error_msg = (
+ f'RuntimeError: {error}\n'
+ ' The server could not send the OpenDRIVE (.xodr) file:\n'
+ ' Make sure it exists, has the same name of your town, and is correct.'
+ )
+ print(error_msg)
+ sys.exit(1)
+
+ def get_local_planner(self):
+ """Get method for protected member local planner"""
+ return self._local_planner
+
+ @property
+ def local_planner(self):
+ """Get the local planner instance (property version)."""
+ return self._local_planner
+
+ @staticmethod
+ def run_step(debug=False):
+ """
+ Execute one step of navigation.
+
+ Args:
+ debug: Boolean flag for debugging
+
+ Returns:
+ control: Vehicle control command
+ """
+ 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.
+
+ Returns:
+ control: Emergency stop control command
+ """
+ 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.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
+
+ # 等待世界更新
+ if not world.world.wait_for_tick(10.0):
+ continue
+
+ # 根据代理类型执行不同的逻辑
+ if args.agent in ["Roaming", "Basic"]:
+ world.world.wait_for_tick(10.0) # 等待服务器
+ world.tick(clock)
+ world.render(display)
+ pygame.display.flip()
+
+ # 为Roaming/Basic代理生成控制
+ control = agent.run_step()
+ control.manual_gear_shift = False
+ world.player.apply_control(control)
+ continue # 跳过后续的控制应用
+
+ # 其他代理类型的逻辑
+ agent.update_information(world)
+ world.tick(clock)
+ world.render(display)
+ pygame.display.flip()
+
+ # 检查是否到达目的地
+ 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 = 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 __doc__:
+ print(__doc__)
+
+ # 运行主循环
+ try:
+ game_loop(args)
+ except KeyboardInterrupt:
+ print('\nCancelled by user. Bye!')
+
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file