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 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
