def get_given_traffic_lights():
"""
Return given traffic light positions
:return: TrafficLightArray
"""
traffic_lights = TrafficLightArray()
traffic_light_list = []
tl_height = rospy.get_param("/tl_height")
config_string = rospy.get_param("/traffic_light_config")
traffic_light_positions = yaml.load(config_string)["light_positions"]
for traffic_light_index, traffic_light_position in enumerate(
traffic_light_positions):
traffic_light = TrafficLight()
traffic_light.pose.pose.position.x = traffic_light_position[0]
traffic_light.pose.pose.position.y = traffic_light_position[1]
traffic_light.pose.pose.position.z = tl_height
traffic_light.state = TrafficLight.UNKNOWN
traffic_light_list.append(traffic_light)
traffic_lights.lights = traffic_light_list
return traffic_lights
def convert_tl_config_to_lane_msgs():
#print("In __func__")
traffic_lights = TrafficLightArray()
tl_list = []
tl_height = rospy.get_param("/tl_height_sim")
#traffic_light_positions = traffic_light_config.config['light_positions']
for traffic_light_index, traffic_light_position in enumerate(
traffic_light_config.config['light_positions']):
traffic_light = TrafficLight()
traffic_light.pose.pose.position.x = traffic_light_position[0]
traffic_light.pose.pose.position.y = traffic_light_position[1]
traffic_light.pose.pose.position.z = tl_height
traffic_light.state = TrafficLight.UNKNOWN
tl_list.append(traffic_light)
traffic_lights.lights = tl_list
global DEBUG
if DEBUG == True:
for traffic_light_index, traffic_light_position in enumerate(tl_list):
rospy.loginfo('x:%f,y:%f',
traffic_light_position.pose.pose.position.x,
traffic_light_position.pose.pose.position.y)
return traffic_lights
def publish_traffic(self, data):
x, y, z = data['light_pos_x'], data['light_pos_y'], data['light_pos_z'],
yaw = [math.atan2(dy, dx) for dx, dy in zip(data['light_pos_dx'], data['light_pos_dy'])]
status = data['light_state']
lights = TrafficLightArray()
lights.header.stamp = rospy.Time.now()
lights.header.frame_id = '/world'
lights.lights = [self.create_light(*e) for e in zip(x, y, z, yaw, status)]
self.publishers['trafficlights'].publish(lights)
def __init__(self):
rospy.init_node('tl_publisher')
self.traffic_light_pubs = rospy.Publisher('/vehicle/traffic_lights', TrafficLightArray, queue_size=1)
light = self.create_light(20.991, 22.837, 1.524, 0.08, 3)
lights = TrafficLightArray()
lights.header = light.header
lights.lights = [light]
self.lights = lights
def publish_traffic(self, data):
x, y, z = data['light_pos_x'], data['light_pos_y'], data['light_pos_z'],
yaw = [math.atan2(dy, dx) for dx, dy in zip(data['light_pos_dx'], data['light_pos_dy'])]
status = data['light_state']
lights = TrafficLightArray()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = '/world'
lights.lights = [self.create_light(*e) for e in zip(x, y, z, yaw, status)]
self.publishers['trafficlights'].publish(lights)
def __init__(self):
rospy.init_node('tl_publisher')
self.traffic_light_pubs = rospy.Publisher('/vehicle/traffic_lights', TrafficLightArray, queue_size=1)
light = self.create_light(20.991, 22.837, 1.524, 0.08, 3)
lights = TrafficLightArray()
lights.header = light.header
lights.lights = [light]
self.lights = lights
self.loop()
def __init__(self):
rospy.init_node('tl_publisher')
# Publisher
self.traffic_light_pubs = rospy.Publisher('/vehicle/traffic_lights', TrafficLightArray, queue_size=1)
# Initialize
lights = TrafficLightArray() # Create "Traffic Light Array" msg
light = self.create_light(20.991, 22.837, 1.524, 0.08, 3) # Create "Traffic Light" msg
lights.header = light.header
lights.lights = [light]
self.lights = lights
# LOOP
self.loop()
def publish_traffic(self, data):
x, y, z = data["light_pos_x"], data["light_pos_y"], data["light_pos_z"]
yaw = [
math.atan2(dy, dx)
for dx, dy in zip(data["light_pos_dx"], data["light_pos_dy"])
]
status = data["light_state"]
lights = TrafficLightArray()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "/world"
lights.lights = [self.create_light(*e) for e in zip(x, y, z, yaw, status)]
self.publishers["trafficlights"].publish(lights)
def __init__(self):
rospy.init_node('tl_publisher')
self.traffic_light_pubs = rospy.Publisher('/vehicle/traffic_lights',
TrafficLightArray,
queue_size=1)
light = self.create_light(20.991, 22.837, 1.524, 0.08, 3)
lights = TrafficLightArray()
lights.header = light.header
lights.lights = [light]
self.lights = lights
rate = rospy.Rate(50)
while not rospy.is_shutdown():
self.traffic_light_pubs.publish(self.lights)
rate.sleep()
def publish_traffic(self, data):
# Don't bother publishing if traffic light status is the same
# as last time (the light positions never change, but the
# light state changes as the lights turn green/yellow/red)
if data == self.prev_traffic_data:
return
self.prev_traffic_data = data
x, y, z = data['light_pos_x'], data['light_pos_y'], data['light_pos_z'],
yaw = [math.atan2(dy, dx) for dx, dy in zip(data['light_pos_dx'], data['light_pos_dy'])]
status = data['light_state']
# print("traffic change", status)
lights = TrafficLightArray()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = '/world'
lights.lights = [self.create_light(*e) for e in zip(x, y, z, yaw, status)]
self.publishers['trafficlights'].publish(lights)
