Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
63 changes: 63 additions & 0 deletions src/car_tracking/carla_visualization/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,63 @@
cmake_minimum_required(VERSION 3.0.2)
project(carla_visualization)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
rospy
sensor_msgs
std_msgs
cv_bridge
)

## Uncomment this if the package has a setup.py
# catkin_python_setup()

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES carla_visualization
# CATKIN_DEPENDS rospy sensor_msgs std_msgs cv_bridge
# DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)

#############
## Install ##
#############

# Mark executable scripts (Python) for installation
# in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/publisher.py
scripts/subscriber.py
scripts/utils.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

# Mark launch files for installation
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
)
11 changes: 11 additions & 0 deletions src/car_tracking/carla_visualization/launch/main.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
<!-- launch/main.launch -->
<launch>
<!-- 启动Carla数据发布节点 -->
<node name="carla_publisher" pkg="carla_visualization" type="publisher.py" output="screen" />

<!-- 启动数据订阅与可视化节点 -->
<node name="carla_subscriber" pkg="carla_visualization" type="subscriber.py" output="screen" />

<!-- 可选:启动rviz可视化(如果需要) -->
<!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find carla_visualization)/config/rviz_config.rviz" /> -->
</launch>
26 changes: 26 additions & 0 deletions src/car_tracking/carla_visualization/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
<?xml version="1.0"?>
<package format="2">
<name>carla_visualization</name>
<version>0.0.0</version>
<description>Carla visualization with ROS 1</description>
<maintainer email="you@example.com">Your Name</maintainer>
<license>MIT</license>

<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>cv_bridge</build_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>cv_bridge</build_export_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>cv_bridge</exec_depend>

<export>
<build_type>catkin</build_type>
</export>
</package>
189 changes: 189 additions & 0 deletions src/car_tracking/carla_visualization/scripts/publisher.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,189 @@
# scripts/publisher.py
#!/usr/bin/env python3
import carla
import queue
import rospy
import time
import numpy as np
from sensor_msgs.msg import Image, CompressedImage
from std_msgs.msg import String, Float32MultiArray, Bool
from cv_bridge import CvBridge
from utils import (
CAMERA_WIDTH, CAMERA_HEIGHT, WEATHER_SWITCH_INTERVAL, SUPPORTED_WEATHERS,
EDGES, build_projection_matrix, get_image_point, point_in_canvas,
get_2d_box_from_3d_edges, get_vehicle_color, clear_npc, clear_static_vehicle,
set_weather, get_random_weather, SHOW_VEHICLES_3D, SHOW_TRAFFIC_LIGHTS, SHOW_TRAFFIC_LIGHTS_STATE
)

# 全局变量
bridge = CvBridge()
image_queue = queue.Queue()

def camera_callback(image, queue):
"""相机回调函数,将图像存入队列"""
queue.put(np.reshape(np.copy(image.raw_data), (image.height, image.width, 4)))

def carla_publisher_node():
# 初始化ROS节点
rospy.init_node('carla_data_publisher', anonymous=True)

# 创建发布者
pub_image = rospy.Publisher('/carla/camera/image_raw', Image, queue_size=10) # 原始图像
pub_compressed_image = rospy.Publisher('/carla/camera/image_compressed', CompressedImage, queue_size=10) # 压缩图像
pub_vehicle_data = rospy.Publisher('/carla/vehicle/track_data', Float32MultiArray, queue_size=10) # 车辆跟踪数据
pub_weather = rospy.Publisher('/carla/weather/current', String, queue_size=10) # 天气信息
pub_3d_switch = rospy.Publisher('/carla/3d/switch', Bool, queue_size=10) # 3D显示开关(示例)

# 初始化Carla客户端
client = carla.Client('localhost', 2000)
client.set_timeout(10.0)
world = client.get_world()

# 设置同步模式
settings = world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = 0.05
world.apply_settings(settings)

# 生成主车辆
bp_lib = world.get_blueprint_library()
vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020')
spawn_points = world.get_map().get_spawn_points()
vehicle = world.try_spawn_actor(vehicle_bp, carla.Transform(spawn_points[0]))
if not vehicle:
rospy.logerr("主车辆生成失败!")
return
vehicle.set_autopilot(True)

# 生成相机
camera_bp = bp_lib.find('sensor.camera.rgb')
camera_bp.set_attribute('image_size_x', str(CAMERA_WIDTH))
camera_bp.set_attribute('image_size_y', str(CAMERA_HEIGHT))
camera_init_trans = carla.Transform(carla.Location(x=1, z=2))
camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle)
camera.listen(lambda image: camera_callback(image, image_queue))

# 清理NPC
clear_npc(world)
clear_static_vehicle(world)

# 生成NPC车辆
for _ in range(50):
vehicle_bp_list = bp_lib.filter('vehicle')
car_bp = [bp for bp in vehicle_bp_list if int(bp.get_attribute('number_of_wheels')) == 4]
if car_bp:
npc = world.try_spawn_actor(random.choice(car_bp), random.choice(spawn_points))
if npc:
npc.set_autopilot(True)

# 初始化参数
fov = camera_bp.get_attribute("fov").as_float()
K = build_projection_matrix(CAMERA_WIDTH, CAMERA_HEIGHT, fov)
K_b = build_projection_matrix(CAMERA_WIDTH, CAMERA_HEIGHT, fov, is_behind_camera=True)
tracked_vehicles = {}
frame_counter = 0
current_weather = "ClearNoon"
set_weather(world, current_weather)
last_weather_switch_time = time.time()

# 频率设置(与Carla同步,20Hz)
rate = rospy.Rate(20)

try:
while not rospy.is_shutdown():
world.tick()
frame_counter += 1

# 自动切换天气
auto_switch = False
if WEATHER_SWITCH_INTERVAL > 0:
current_time = time.time()
if current_time - last_weather_switch_time >= WEATHER_SWITCH_INTERVAL:
current_weather = get_random_weather()
set_weather(world, current_weather)
last_weather_switch_time = current_time
auto_switch = True

# 发布天气信息
pub_weather.publish(current_weather)

# 处理键盘控制(ROS参数服务器或话题控制,这里简化为示例)
# 可通过订阅话题来修改3D显示开关
pub_3d_switch.publish(SHOW_VEHICLES_3D)

# 获取相机图像
if not image_queue.empty():
image = image_queue.get()
# 转换为ROS Image消息并发布
ros_image = bridge.cv2_to_imgmsg(image, encoding="bgra8")
ros_image.header.stamp = rospy.Time.now()
pub_image.publish(ros_image)
# 发布压缩图像(可选,减少带宽)
compressed_image = bridge.cv2_to_compressed_imgmsg(image[..., :3], "jpg")
pub_compressed_image.publish(compressed_image)

# 采集车辆跟踪数据
vehicle_data = Float32MultiArray()
boxes = []
ids = []
track_data = {}
current_vehicles = 0
max_distance = 0.0

for npc in world.get_actors().filter('*vehicle*'):
if npc.id != vehicle.id:
dist = npc.get_transform().location.distance(vehicle.get_transform().location)
max_distance = max(max_distance, dist)
if dist < 50:
forward_vec = vehicle.get_transform().get_forward_vector()
ray = npc.get_transform().location - vehicle.get_transform().location
if forward_vec.dot(ray) > 0:
verts = [v for v in npc.bounding_box.get_world_vertices(npc.get_transform())]
points_2d = []
world_2_camera = np.array(camera.get_transform().get_inverse_matrix())
for vert in verts:
ray0 = vert - camera.get_transform().location
cam_forward_vec = camera.get_transform().get_forward_vector()
if cam_forward_vec.dot(ray0) > 0:
p = get_image_point(vert, K, world_2_camera)
else:
p = get_image_point(vert, K_b, world_2_camera)
points_2d.append(p)
x_min, x_max, y_min, y_max = get_2d_box_from_3d_edges(
points_2d, EDGES, CAMERA_HEIGHT, CAMERA_WIDTH
)
if (y_max - y_min) * (x_max - x_min) > 100 and (x_max - x_min) > 20:
if point_in_canvas((x_min, y_min), CAMERA_HEIGHT, CAMERA_WIDTH) and \
point_in_canvas((x_max, y_max), CAMERA_HEIGHT, CAMERA_WIDTH):
ids.append(npc.id)
boxes.append([x_min, y_min, x_max, y_max])
if npc.id not in tracked_vehicles:
tracked_vehicles[npc.id] = {'color': get_vehicle_color(npc.id)}
tracked_vehicles[npc.id]['distance'] = dist
track_data[npc.id] = tracked_vehicles[npc.id]
current_vehicles += 1

# 封装车辆数据并发布(示例:车辆数量、最大距离、第一个框的坐标)
vehicle_data.data = [current_vehicles, max_distance] + (boxes[0] if boxes else [0,0,0,0])
pub_vehicle_data.publish(vehicle_data)

rate.sleep()

except rospy.ROSInterruptException:
pass
except Exception as e:
rospy.logerr(f"发布节点出错:{e}")
finally:
# 清理资源
if camera:
camera.stop()
camera.destroy()
if vehicle:
vehicle.destroy()
settings = world.get_settings()
settings.synchronous_mode = False
world.apply_settings(settings)
rospy.loginfo("Carla资源已清理")

if __name__ == '__main__':
carla_publisher_node()
Loading
Loading