本教程适用于采用编译下载安装方式安装carla-ros-bridge 的用户。
1.修改信号灯
1.1 修改原理
我们要通过API过滤出所有绿灯的actor信息,然后修改他们的状态为常绿。
查阅API网站可知traffic_light具有set_state(self,state)
方法
https://carla.readthedocs.io/en/latest/python_api/#carlatrafficlight
官网提供了一段代码,可以参考:
world = client.get_world()
spectator = world.get_spectator()
vehicle_bp = random.choice(world.get_blueprint_library().filter('vehicle.bmw.*'))
transform = random.choice(world.get_map().get_spawn_points())
vehicle = world.try_spawn_actor(vehicle_bp, transform)
# Wait for world to get the vehicle actor
world.tick()
world_snapshot = world.wait_for_tick()
actor_snapshot = world_snapshot.find(vehicle.id)
# Set spectator at given transform (vehicle transform)
spectator.set_transform(actor_snapshot.get_transform())
# ...# ...
if vehicle_actor.is_at_traffic_light():
traffic_light = vehicle_actor.get_traffic_light()
if traffic_light.get_state() == carla.TrafficLightState.Red:
# world.hud.notification("Traffic light changed! Good to go!")
traffic_light.set_state(carla.TrafficLightState.Green)
需要注意的是,set_state()
只能暂时把灯变绿,之后灯还会变回来。所以为了实现信号灯一直是绿色,还需要锁住信号灯,采用.freeze(True)
.
参考这个方法:
freeze(self, freeze)
Stops all the traffic lights in the scene at their current state.
Parameters:
freeze (bool)
1.2 具体代码
了解了上述原理,之后,下面就介绍如何修改代码。
首先定位到文件位置为:
carla-ros-bridge-0.9.12/catkin_ws/src/ros-bridge/carla_ros_bridge/src/carla_ros_bridge/bridge.py
在444行:
carla_bridge.initialize_bridge(carla_client.get_world(), parameters)
之后,加入以下代码:
# lbw add:turn off trafficLight
carla_world = carla_client.get_world()
actors=carla_world.get_actors()
light_actor_list=actors.filter('*traffic_light*')
carla_bridge.loginfo(">>>>>>>>>>>>>>>>light_actor number:{}".format(len(light_actor_list)))
for light_actor in light_actor_list:
light_actor.set_state(carla.TrafficLightState.Green)
light_actor.freeze(True)
carla_bridge.loginfo(light_actor)
截图:
修改保存后,重新catkin_make后运行
roslaunch carla_ros_bridge carla_ros_bridge_with_example_ego_vehicle.launch town:="town02"
可以看到24个交通信号灯的打印的信息:
[INFO] [1670599386.103029, 0.000000]: >>>>>>>>>>>>>>>>light_actor number:24
[INFO] [1670599386.104971, 0.000000]: Created TrafficLight(id=531)
[INFO] [1670599386.106751, 0.000000]: Actor(id=551, type=traffic.traffic_light)
[INFO] [1670599386.109619, 0.000000]: Created TrafficLight(id=532)
[INFO] [1670599386.111681, 0.000000]: Actor(id=550, type=traffic.traffic_light)
[INFO] [1670599386.114241, 0.000000]: Created TrafficLight(id=533)
[INFO] [1670599386.116681, 0.000000]: Actor(id=549, type=traffic.traffic_light)
[INFO] [1670599386.121154, 0.000000]: Created TrafficLight(id=534)
略去一部分...
打印信号灯的状态:
rostopic echo /carla/traffic_lights/status
可以看到,所有灯的状态都变成了2。
---
traffic_lights:
id: 531
state: 2
-
id: 532
state: 2
-
id: 533
state: 2
-
id: 534
state: 2
-
...略去一部分
可以看到,信号灯都已经是常绿了~~~