def create_light(self, x, y, z, yaw, state): light = TrafficLight() light.header = Header() light.header.stamp = rospy.Time.now() light.header.frame_id = '/world' light.pose = self.create_pose(x, y, z, yaw) light.state = state return light
def image_cb(self, msg): """Identifies red lights in the incoming camera image and publishes the index of the waypoint closest to the red light's stop line to /traffic_waypoint Args: msg (Image): image from car-mounted camera """ self.has_image = True self.camera_image = msg light_wp, state = self.process_traffic_lights() #print(light_wp) #print(state) #print() ''' Publish upcoming red lights at camera frequency. Each predicted state has to occur `STATE_COUNT_THRESHOLD` number of times till we start using it. Otherwise the previous stable state is used. ''' our_msg = TL_State() our_light = TrafficLight() our_light.header = Header() our_light.header.stamp = rospy.Time.now() our_light.header.frame_id = '/world' if self.state != state: self.state_count = 0 self.state = state elif self.state_count >= STATE_COUNT_THRESHOLD: self.last_state = self.state light_wp = light_wp if (state == TrafficLight.RED or state == TrafficLight.YELLOW) else -1 self.last_wp = light_wp self.upcoming_red_light_pub.publish(Int32(light_wp)) our_msg.waypoint = light_wp our_light.state = state our_msg.light = our_light self.custom_state_pub.publish(our_msg) else: self.upcoming_red_light_pub.publish(Int32(self.last_wp)) our_msg.waypoint = self.last_wp our_light.state = self.last_state our_msg.light = our_light self.custom_state_pub.publish(our_msg) self.state_count += 1
def create_tl(self, yaw, state, x, y, z): traffic_light = TrafficLight() traffic_light.header = Header() traffic_light.pose.header = Header() traffic_light.pose = PoseStamped() traffic_light.state = state traffic_light.pose.pose.position.x = x traffic_light.pose.pose.position.y = y traffic_light.pose.pose.position.z = z traffic_light.pose.header.stamp = rospy.Time.now() traffic_light.pose.header.frame_id = 'world' traffic_light.header.stamp = rospy.Time.now() traffic_light.header.frame_id = 'world' q = tf.transformations.quaternion_from_euler(0.0, 0.0, math.pi * yaw / 180.0) traffic_light.pose.pose.orientation = Quaternion(*q) return traffic_light
def create_light(self, x, y, z, yaw, state): """Creates a new TrafficLight object Args: x (float): x coordinate of light y (float): y coordinate of light z (float): z coordinate of light yaw (float): angle of light around z axis state (int): ID of traffic light color (specified in styx_msgs/TrafficLight) Returns: light (TrafficLight): new TrafficLight object """ light = TrafficLight() light.header = Header() light.header.stamp = rospy.Time.now() light.header.frame_id = 'world' light.pose = self.create_pose(x, y, z, yaw) light.state = state return light
def light_loc(self, state, lx, ly, lz, lyaw): # light state initialization light = TrafficLight() # light.state = state # header position light.header = Header() light.header.stamp = rospy.Time.now() light.header.frame_id = 'world' # pose position light.pose = PoseStamped() light.pose.header.stamp = rospy.Time.now() light.pose.header.frame_id = 'world' light.pose.pose.position.x = lx light.pose.pose.position.y = ly light.pose.pose.position.z = lz q_from_euler = tf.transformations.quaternion_from_euler( 0.0, 0.0, math.pi * lyaw / 180.0) light.pose.pose.orientation = Quaternion(*q_from_euler) return light