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)