接入信号灯与Carla实现硬件在环

sophisticatedtj

已于 2024-07-10 16:17:54 修改

阅读量173

点赞数 5

文章标签： python

于 2024-07-10 16:16:23 首次发布

本文链接：https://blog.csdn.net/sophisticatedtj/article/details/140323621

版权

实现逻辑很简单，Carla有支持更改信号灯和读取信号灯的功能，通过自定义消息读取Carla信号状态并发布给信号灯，信号灯接收并发布指令，反过来也是一样的。

一、话题通信

信号灯与Carla之间通过ROS话题进行通信，消息是自定义的：

# TrafficLights.msg
float64 timestamp
uint32 traffic_id
uint32 light

二、Carla交通灯→信号灯

我提取了第1个交通灯进行发布，并显示灯的位置，代码如下：

#!/usr/bin/env python3
import rospy
from traffic_ros.msg import TrafficLights
import carla

def show_point(world, point_location):
    world.debug.draw_string(point_location, 'X', draw_shadow=False,
                                        color=carla.Color(r=0, g=255, b=0), life_time=50,
                                        persistent_lines=True)

def main():
    # 连接到CARLA仿真器
    rospy.init_node('traffic_light_publisher', anonymous=True)
    pub = rospy.Publisher('traffic_sub', TrafficLights, queue_size=10)
    client = carla.Client('localhost', 2000)
    client.set_timeout(5.0)  # 设置超时时间

    # 获取世界对象和地图
    world = client.get_world()
    map = world.get_map()

    # 获取所有存在的交通信号灯
    traffic_lights = world.get_actors().filter('traffic.traffic_light')

    # 获取第一个交通信号灯的位置
    if len(traffic_lights) > 0:
        traffic_light = traffic_lights[0]  # 如果你需要获取第一个信号灯位置，索引应该为0而不是1
        location = traffic_light.get_location()
        print("交通信号灯位置: ", location)
    else:
        print("未找到交通信号灯")

    # 在地图中可视化信号灯位置
    show_point(world, location)

    # 发布频率
    rate = rospy.Rate(1)

    while not rospy.is_shutdown():
        # 输出信号状态
        state = traffic_light.get_state()
        if state == carla.TrafficLightState.Red:
            state_value = 0
        elif state == carla.TrafficLightState.Green:
            state_value = 1
        elif state == carla.TrafficLightState.Yellow:
            state_value = 2

        traffic_msg = TrafficLights()
        traffic_msg.timestamp = rospy.get_time()
        traffic_msg.traffic_id = traffic_light.id
        traffic_msg.light = state_value

        pub.publish(traffic_msg)
        print(traffic_msg)
        # rospy.loginfo("发布信号灯状态: %s", state_value)
        rate.sleep()
        world.tick()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

信号灯的部分，智能信号灯的价格都比较贵，所以买了三个简易的灯（下图）自己串起来：

控制部分代码如下：

#!/usr/bin/env python3

import rospy
from traffic_ros.msg import TrafficLights 
import serial
import binascii
import time

# 配置串口参数
def configure_serial(port, baudrate):
    try:
        serial_port = serial.Serial(port, baudrate, timeout=1, bytesize=8, stopbits=serial.STOPBITS_ONE)
        print(f"串口已打开: {port}")
        return serial_port
    except Exception as e:
        print(f"无法打开串口: {e}")
        exit(1)

# 发送命令并读取响应
def send_and_receive(serial_port, command):
    try:
        bytes_written = serial_port.write(command)
        print(f"指令已发送,字节数: {bytes_written}")
    except Exception as e:
        print(f"发送指令失败: {e}")
        serial_port.close()
        exit(1)

# 回调函数,接收消息并控制灯的状态
def callback(data):
    print(data)
    traffic_id = data.traffic_id
    light = data.light

    if light == 0:
        send_and_receive(serial_port2, command)  # 黄灯灭
        send_and_receive(serial_port1, command3)  # 红灯亮
        print("0:red light")
    elif light == 1:
        send_and_receive(serial_port1, command)  # 红灯灭
        send_and_receive(serial_port3, command1)  # 绿灯亮
        print("0:green light")
    elif light == 2:
        send_and_receive(serial_port3, command)  # 红灯亮
        send_and_receive(serial_port2, command6)  # 黄灯亮
        print("0:yellow light")


# 主函数
def main():


    global serial_port1, serial_port2, serial_port3, command, command1, command2, command3, command4, command5, command6

    port1 = '/dev/<串口名>'
    port2 = '/dev/<串口名>'
    port3 = '/dev/<串口名>'
    baudrate = 9600
    command = bytes.fromhex('<灯熄灭的控制指令>')      # blow out
    command1 = bytes.fromhex("<绿灯的控制指令>")   # green
    command2 = bytes.fromhex("<绿灯闪烁的控制指令>")   # green shan
    command3 = bytes.fromhex("<红灯的控制指令>")    # red
    command4 = bytes.fromhex("<红灯闪烁的控制指令>")    # red shan
    command5 = bytes.fromhex("<黄灯的控制指令>")    # yellow 
    command6 = bytes.fromhex("<黄灯闪烁的控制指令>")    # yellow shan
    
    serial_port1 = configure_serial(port1, baudrate)
    serial_port2 = configure_serial(port2, baudrate)
    serial_port3 = configure_serial(port3, baudrate)
    
    rospy.init_node('traffic_light_controller', anonymous=True)
    rospy.Subscriber('traffic_sub', TrafficLights, callback)

    rospy.spin()

if __name__ == "__main__":
    main()

在终端中输入 ls /dev 可以查看串口信息

效果如下：信号灯-Carla_哔哩哔哩_bilibili

三、信号灯→.Carla交通灯

信号灯发布代码：

#!/usr/bin/env python3

import serial
import binascii
import time
from traffic_ros.msg import TrafficLights 
import rospy

# 配置串口参数
def configure_serial(port, baudrate):
    try:
        serial_port = serial.Serial(port, baudrate, timeout=1, bytesize=8, stopbits=serial.STOPBITS_ONE)
        print(f"串口已打开: {port}")
        return serial_port
    except Exception as e:
        print(f"无法打开串口: {e}")
        exit(1)

# 发送命令并读取响应
def send_and_receive(serial_port, command):
    try:
        bytes_written = serial_port.write(command)
        
        print(f"指令已发送,字节数: {bytes_written}")
    except Exception as e:
        print(f"发送指令失败: {e}")
        serial_port.close()
        exit(1)
    

# 主函数
def main():
    port1 = '/dev/<串口名>'
    port2 = '/dev/<串口名>'
    port3 = '/dev/<串口名>'
    baudrate = 9600
    command = bytes.fromhex('……')      # blow out
    command1 = bytes.fromhex("……")   # green
    command2 = bytes.fromhex("……")   # green shan
    command3 = bytes.fromhex("……")    # red
    command4 = bytes.fromhex("……")    # red shan
    command5 = bytes.fromhex("……")    # yellow 
    command6 = bytes.fromhex("……")    # yellow shan
        
    serial_port1 = configure_serial(port1, baudrate)
    serial_port2 = configure_serial(port2, baudrate)
    serial_port3 = configure_serial(port3, baudrate)
    
    rospy.init_node('traffic_light_publisher', anonymous=True)
    pub = rospy.Publisher('traffic_sub', TrafficLights, queue_size=10)    
    
    traffic_light_id = 0
    traffic_msg = TrafficLights()
    traffic_msg.traffic_id = traffic_light_id
    try:
        # 重复发送指令和读取响应
        while not rospy.is_shutdown():  # 重复发送
            # 红灯亮 5 秒
            send_and_receive(serial_port1, command3)
            traffic_msg.light = 0
            traffic_msg.timestamp = rospy.get_time()
            pub.publish(traffic_msg)
            print(traffic_msg)
            time.sleep(20)

            # 红灯熄灭,同时绿灯亮
            send_and_receive(serial_port1, command)
            send_and_receive(serial_port2, command1)
            traffic_msg.light = 1
            traffic_msg.timestamp = rospy.get_time()
            pub.publish(traffic_msg)
            print(traffic_msg)
            # 绿灯亮 5 秒
            time.sleep(10)

            # 绿灯熄灭,同时黄灯亮
            send_and_receive(serial_port2, command)
            send_and_receive(serial_port3, command6)
            traffic_msg.light = 2
            traffic_msg.timestamp = rospy.get_time()
            pub.publish(traffic_msg)
            print(traffic_msg)
            time.sleep(4)

            # 黄灯熄灭
            send_and_receive(serial_port3, command)
                
    finally:
        serial_port1.close()
        serial_port2.close()
        serial_port3.close()
        print("串口已关闭")

if __name__ == "__main__":
    main()

Carla信号灯接收并更改状态代码：

#!/usr/bin/env python3

import carla
import rospy
from traffic_ros.msg import TrafficLights

# 连接到 CARLA 服务器
client = carla.Client('localhost', 2000)
client.set_timeout(10.0)
world = client.get_world()

# # 存储交通信号灯字典,以traffic_id为键,交通信号灯对象为值
# traffic_light_dict = {}

def show_point(world, point_location):
    world.debug.draw_string(point_location, 'O', draw_shadow=False,
                            color=carla.Color(r=255, g=0, b=0), life_time=10000,
                            persistent_lines=True)

def traffic_light_callback(msg):
    traffic_id = msg.traffic_id
    light_state = msg.light
    traffic_lights = world.get_actors().filter('traffic.traffic_light')
    if len(traffic_lights) > 0:
        # 选择第一个交通信号灯作为视角参考
        reference_traffic_light = traffic_lights[0]
        traffic_light = reference_traffic_light

        # 根据接收到的消息设置交通信号灯状态
        if light_state == 0:  # 红灯
            traffic_light.set_state(carla.TrafficLightState.Red)
        elif light_state == 1:  # 绿灯
            traffic_light.set_state(carla.TrafficLightState.Green)
        elif light_state == 2:  # 黄灯
            traffic_light.set_state(carla.TrafficLightState.Yellow)
        else:
            rospy.logwarn("未知的交通信号灯状态: %d", light_state)

        rospy.loginfo("更新交通信号灯 %d 的状态为 %d", traffic_id, light_state)
    else:
        rospy.logwarn("未找到ID为 %d 的交通信号灯", traffic_id)

def main():
    rospy.init_node('carla_traffic_light_controller', anonymous=True)
    rospy.Subscriber('traffic_sub', TrafficLights, traffic_light_callback)

    # # 获取所有交通信号灯并存储到字典中
    traffic_lights = world.get_actors().filter('traffic.traffic_light')
    # for traffic_light in traffic_lights:
    #     traffic_id = traffic_light.id
    #     traffic_light_dict[traffic_id] = traffic_light

        # # 获取交通信号灯的位置并在地图上显示
        # location = traffic_light.get_location()
        # rospy.loginfo("交通信号灯 %d 的位置: (%.2f, %.2f, %.2f)", traffic_id, location.x, location.y, location.z)
        # show_point(world, location)

    # 设置地图视角为指定交通信号灯的视角
    if len(traffic_lights) > 0:
        # 选择第一个交通信号灯作为视角参考
        reference_traffic_light = traffic_lights[0]
        location = reference_traffic_light.get_location()
        print(location)
        show_point(world, location)
        spectator = world.get_spectator()
        transform = reference_traffic_light.get_transform()
        
        # 调整spectator的位置和旋转以对准交通信号灯
        spectator_location = location + carla.Location(x=-10, y=-4, z=3)  # 稍微抬高视角
        spectator_rotation = carla.Rotation(pitch=10,yaw=-1,roll=0)  # 调整俯仰角
        
        spectator.set_transform(carla.Transform(spectator_location, spectator_rotation))
        rospy.loginfo("地图视角已设置为交通信号灯 %d 的视角", reference_traffic_light.id)
    else:
        rospy.logwarn("未找到任何交通信号灯,无法设置地图视角")

    rospy.loginfo("Carla交通信号灯控制器已启动")
    rospy.spin()

if __name__ == '__main__':
    main()