def __init__(self):
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.pub_state = rospy.Publisher("~current_led_state",
Float32,
queue_size=1)
self.sub_pattern = rospy.Subscriber("~change_color_pattern", String,
self.changePattern)
self.sub_switch = rospy.Subscriber("~switch", BoolStamped,
self.cbSwitch)
self.cycle = None
self.is_on = False
self.active = True
self.protocol = rospy.get_param(
"~LED_protocol") #should be a list of tuples
self.pattern_off = [[0, 0, 0]] * 5
scale = 0.5
for _, c in self.protocol['colors'].items():
for i in range(3):
c[i] = c[i] * scale
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(.1),
self.cycleTimer)
self.current_pattern_name = None
self.changePattern_('CAR_SIGNAL_A')
def __init__(self):
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.active = True
self.pattern = [[0,0,0]]*5
self.current_pattern_name = 'OFF'
self.changePattern_(self.current_pattern_name)
# Import protocol
self.protocol = rospy.get_param("~LED_protocol")
# If True, the LED turn on and off. Else, they are always on
self.onOff = True
if self.onOff:
self.cycle = 1.0/self.protocol['signals']['CAR_SIGNAL_A']['frequency']
self.is_on = False
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(self.cycle/(2.0)),self.cycleTimer)
# Publish
#self.pub_state = rospy.Publisher("~current_led_state",Float32,queue_size=1)
self.pub_state = rospy.Publisher("~current_led_state",String,queue_size=1)
# Subscribe
self.sub_pattern = rospy.Subscriber("~change_color_pattern",String,self.changePattern)
self.sub_switch = rospy.Subscriber("~switch",BoolStamped,self.cbSwitch)
# Scale intensity of the LEDs
scale = 0.8
for _, c in self.protocol['colors'].items():
for i in range(3):
c[i] = c[i]*scale
def __init__(self):
# Save the name of the node
self.node_name = rospy.get_name()
self.led = RGB_LED()
self.turn_LEFT = 0
self.turn_RIGHT = 2
self.turn_STRAIGHT = 1
self.turn_NONE = 3
self.blink = False
self.availableTurns = []
self.turn_direction = self.turn_NONE
self.joy_forward = 0
self.timeout = 5 # seconds
rospy.loginfo("[%s] Initializing." % self.node_name)
# Setup publishers
self.pub_turn_type = rospy.Publisher("~turn_type",
Int16,
queue_size=1,
latch=True)
self.pub_done = rospy.Publisher("~intersection_done",
BoolStamped,
queue_size=1)
# Setup subscribers
self.sub_topic_mode = rospy.Subscriber("~mode",
FSMState,
self.cbMode,
queue_size=1)
self.sub_topic_tag = rospy.Subscriber("~tag",
AprilTagsWithInfos,
self.cbTag,
queue_size=1)
self.sub_joy = rospy.Subscriber("~joy", Joy, self.cbJoy, queue_size=1)
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(.5),
self.blinkTimer)
rospy.loginfo("[%s] Initialized." % self.node_name)
self.rate = rospy.Rate(30) # 10hz
# led setup
self.led.setRGB(4, [.2, .2, .2])
self.led.setRGB(2, [.2, .2, .2])
self.led.setRGB(3, [.3, 0, 0])
self.led.setRGB(1, [.3, 0, 0])
self.led.setRGB(0, [.2, .2, .2])
def __init__(self):
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.cycle = None
self.protocol =self.setupParameter("~LED_protocol",[]) #should be a list of tuples
self.greenlight_freq = self.protocol['signals']['traffic_light_go']['frequency']
self.redlight_freq = self.protocol['signals']['traffic_light_stop']['frequency']
self.traffic_light_list = self.setupParameter("~traffic_light_list",[0,2,3,1]); #order of lights
self.greenlight_duration = self.setupParameter("~greenlight_duration",5) #in seconds
self.allred_duration = self.setupParameter("~allred_duration",4) #in seconds
self.redlight_t = 1.0/self.redlight_freq
self.greenlight_t = 1.0/self.greenlight_freq
self.green_on=False
self.green_i = 0;
self.green = self.traffic_light_list[self.green_i]
self.green_color = [0,1,0] #Hardcoded but should be parameter
self.yellow_color = [1,0.65,0] #Hardcoded but should be parameter
self.redlightlist = self.traffic_light_list[:self.green_i] + self.traffic_light_list[(self.green_i+1):];
self.traffic_light_state = {0:False,1:False,2:False,3:False} #All LEDs are off
self.yellowlightlist = []
self.traffic_cycle = rospy.Timer(rospy.Duration((self.greenlight_duration+self.allred_duration)),self.switchGreen)
self.redLED_cycle = rospy.Timer(rospy.Duration(0.5*self.redlight_t),self.freqred)
self.greenLED_cycle = rospy.Timer(rospy.Duration(0.5*self.greenlight_t),self.freqgreen)
def __init__(self):
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.pub_state = rospy.Publisher("~current_led_state",
Float32,
queue_size=1)
self.sub_pattern = rospy.Subscriber("~change_color_pattern", String,
self.changePattern)
self.cycle = None
self.is_on = False
self.changePattern_('CAR_SIGNAL_A')
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(.1),
self.cycleTimer)
def __init__(self):
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.cycle = None
self.protocol = self.setupParameter("~LED_protocol",
[]) #should be a list of tuples
self.greenlight_freq = self.protocol['signals']['traffic_light_go'][
'frequency']
self.redlight_freq = self.protocol['signals']['traffic_light_stop'][
'frequency']
self.traffic_light_list = self.setupParameter("~traffic_light_list",
[0, 2, 3, 1])
#order of lights
self.greenlight_duration = self.setupParameter("~greenlight_duration",
5) #in seconds
self.allred_duration = self.setupParameter("~allred_duration",
4) #in seconds
self.redlight_t = 1.0 / self.redlight_freq
self.greenlight_t = 1.0 / self.greenlight_freq
self.green_on = False
self.green_i = 0
self.green = self.traffic_light_list[self.green_i]
self.green_color = [0, 1, 0] #Hardcoded but should be parameter
self.yellow_color = [1, 0.65, 0] #Hardcoded but should be parameter
self.redlightlist = self.traffic_light_list[:self.
green_i] + self.traffic_light_list[
(self.green_i + 1):]
self.traffic_light_state = {
0: False,
1: False,
2: False,
3: False
} #All LEDs are off
self.yellowlightlist = []
self.traffic_cycle = rospy.Timer(
rospy.Duration((self.greenlight_duration + self.allred_duration)),
self.switchGreen)
self.redLED_cycle = rospy.Timer(rospy.Duration(0.5 * self.redlight_t),
self.freqred)
self.greenLED_cycle = rospy.Timer(
rospy.Duration(0.5 * self.greenlight_t), self.freqgreen)
def __init__(self):
# Hardcoded color values to configure traffic light
# ATTENTION: This skript uses GRB instead of RGB logic to work with the
# newest version of the traffic lights with surface mounted LEDs.
self.green_color = [1, 0, 0]
self.red_color = [0, 1, 0]
self.yellow_color = [1, 1, 0]
self.black_color = [0, 0, 0]
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.cycle = None
self.protocol = self.setupParameter(
"~LED_protocol", []) # should be a list of tuples
self.greenlight_freq = self.protocol['signals']['traffic_light_go']['frequency']
self.redlight_freq = self.protocol['signals']['traffic_light_stop']['frequency']
self.traffic_light_list = self.setupParameter(
"~traffic_light_list", [0, 2, 3, 4]) # order of lights
self.greenlight_duration = self.setupParameter(
"~greenlight_duration", 5) # in seconds
self.allred_duration = self.setupParameter(
"~allred_duration", 4) # in seconds
self.redlight_t = 1.0/self.redlight_freq
self.greenlight_t = 1.0/self.greenlight_freq
self.green_on = False
self.green_i = 0
self.green = self.traffic_light_list[self.green_i]
self.redlightlist = self.traffic_light_list[:self.green_i] + \
self.traffic_light_list[(self.green_i+1):]
self.traffic_light_state = {
0: False, 4: False, 2: False, 3: False} # All LEDs are off
self.yellowlightlist = []
self.traffic_cycle = rospy.Timer(rospy.Duration(
(self.greenlight_duration+self.allred_duration)), self.switchGreen)
self.redLED_cycle = rospy.Timer(
rospy.Duration(0.5*self.redlight_t), self.freqred)
self.greenLED_cycle = rospy.Timer(
rospy.Duration(0.5*self.greenlight_t), self.freqgreen)
def __init__(self):
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.pub_state = rospy.Publisher("~current_led_state",Float32,queue_size=1)
self.sub_pattern = rospy.Subscriber("~change_color_pattern", String, self.changePattern)
self.cycle = None
self.is_on = False
self.changePattern_('CAR_SIGNAL_A')
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(.1), self.cycleTimer)
def __init__(self):
# Save the name of the node
self.node_name = rospy.get_name()
self.led = RGB_LED()
self.turn_LEFT = 0
self.turn_RIGHT = 2
self.turn_STRAIGHT = 1
self.turn_NONE = 3
self.blink = False
self.availableTurns = []
self.turn_direction = self.turn_NONE
self.joy_forward = 0
self.timeout = 5 # seconds
rospy.loginfo("[%s] Initializing." % self.node_name)
# Setup publishers
self.pub_turn_type = rospy.Publisher("~turn_type", Int16, queue_size=1, latch=True)
self.pub_done = rospy.Publisher("~intersection_done",BoolStamped,queue_size=1)
# Setup subscribers
self.sub_topic_mode = rospy.Subscriber("~mode", FSMState, self.cbMode, queue_size=1)
self.sub_topic_tag = rospy.Subscriber("~tag", AprilTagsWithInfos, self.cbTag, queue_size=1)
self.sub_joy = rospy.Subscriber("~joy", Joy, self.cbJoy, queue_size=1)
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(.5), self.blinkTimer)
rospy.loginfo("[%s] Initialized." % self.node_name)
self.rate = rospy.Rate(30) # 10hz
# led setup
self.led.setRGB(4, [.2, .2, .2])
self.led.setRGB(2, [.2, .2, .2])
self.led.setRGB(3, [.3, 0, 0])
self.led.setRGB(1, [.3, 0, 0])
self.led.setRGB(0, [.2, .2, .2])
def __init__(self):
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.pub_state = rospy.Publisher("~current_led_state",Float32,queue_size=1)
self.sub_pattern = rospy.Subscriber("~change_color_pattern", String, self.changePattern)
self.sub_switch = rospy.Subscriber("~switch",BoolStamped,self.cbSwitch)
self.cycle = None
self.is_on = False
self.active = True
self.protocol = rospy.get_param("~LED_protocol") #should be a list of tuples
self.pattern_off = [[0,0,0]] * 5
scale = 0.5
for _, c in self.protocol['colors'].items():
for i in range(3):
c[i] = c[i] * scale
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(.1), self.cycleTimer)
self.current_pattern_name = None
self.changePattern_('CAR_SIGNAL_A')
class LEDEmitterNode(Node):
"""Node for controlling LEDs.
Calls the low-level functions of class :obj:`RGB_LED` that creates the PWM
signal used to change the color of the LEDs. The desired behavior is specified by
the LED index (Duckiebots and watchtowers have multiple of these) and a pattern.
A pattern is a combination of colors and blinking frequency.
Duckiebots have 5 LEDs that are indexed and positioned as following:
+------------------+------------------------------------------+
| Index | Position (rel. to direction of movement) |
+==================+==========================================+
| 0 | Front left |
+------------------+------------------------------------------+
| 1 | Rear left |
+------------------+------------------------------------------+
| 2 | Top / Front middle |
+------------------+------------------------------------------+
| 3 | Rear right |
+------------------+------------------------------------------+
| 4 | Front right |
+------------------+------------------------------------------+
A pattern is specified via 5 parameters:
- its name
- frequency: blinking frequency in Hz, should be set to 0 for a solid (non-blinking) behavior
- color_list: a list of 5 colour names (see below), one for each LED ordered as above, or a single string with
a single color name that would be applied to all LEDs
- frequency_mask: a list of 5 binary flags (0 or 1) that specify which of the LEDs should be blinking,
used only if the frequency is not 0. The LEDs with the flag set to 0, will maintain their solid color.
The defaut patterns are defined in the `LED_protocol.yaml` configuration file for the node.
Currently supported colors are: `green`, `red`, `blue`, `white`, `yellow`, `purple`, `cyan`,
`pink`, `switchedoff`. More colors can be defined in the node's configuration file.
Examples:
To change the pattern to one of the predefined patterns (you can see them using `rosparam list`)
use a variant of the following::
rosservice call /HOSTNAME/led_emitter_node/set_pattern "pattern_name: {data: RED}"
Other pre-defined patterns you can use are: `WHITE`, `GREEN`, `BLUE`, `LIGHT_OFF`, `CAR_SIGNAL_PRIORITY`,
`CAR_SIGNAL_SACRIFICE_FOR_PRIORITY`, `CAR_SIGNAL_SACRIFICE_FOR_PRIORITY`, `CAR_SIGNAL_SACRIFICE_FOR_PRIORITY`,
`CAR_DRIVING`.
To add a custom pattern and switch to it use a variant of the following::
rosservice call /HOSTNAME/led_emitter_node/set_custom_pattern "pattern: {color_list: ['green','yellow','pink','orange','blue'], color_mask: [1,1,1,1,1], frequency: 1.0, frequency_mask: [1,0,1,0,1]}"
Configuration:
~LED_protocol (nested dictionary): Nested dictionary that describes the LED protocols (patterns). The
default can be seen in the `LED_protocol.yaml` configuration file for the node.
~LED_scale (:obj:`float`): A scaling factor (between 0 and 1) that is applied to the colors in order
to reduce the overall LED brightness, default is 0.8.
~channel_order (:obj:`str`): A string that controls the order in which the 3 color channels should be
communicated to the LEDs. Should be one of 'RGB', `RBG`, `GBR`, `GRB`, `BGR`, `BRG`. Typically
for a duckiebot this should be the default `RGB` and for traffic lights should be `GRB`, default is `RGB`.
Publishers:
~current_led_state (:obj:`String` message): Publishes the name of the current pattern used. Published
only when the selected pattern changes.
Services:
~set_custom_pattern: Allows setting a custom protocol. Will be named `custom`. See an example of a call
in :obj:`srvSetCustomLEDPattern`.
input:
pattern (:obj:`LEDPattern` message): The desired new LEDPattern
~set_pattern: Switch to a different pattern protocol.
input:
pattern_name (:obj:`String` message): The new pattern name, should match one of the patterns in
the `LED_protocol` parameter (or be `custom` if a custom pattern has been defined via a call to
the `~change_led` service.
"""
def __init__(self):
super().__init__("led_emitter_node")
self.node_name = self.get_name()
self.log = self.get_logger()
self.log.info("Initializing....")
self.led = RGB_LED()
self.robot_type = rospy.get_param("~robot_type")
# Add the node parameters to the parameters dictionary and load their default values
# self._LED_protocol = rospy.get_param('~LED_protocol')
# self._LED_scale = rospy.get_param('~LED_scale')
# self._channel_order = rospy.get_param('~channel_order')
# Initialize LEDs to be off
self.pattern = [[0, 0, 0]] * 5
self.frequency_mask = [0] * 5
self.current_pattern_name = 'LIGHT_OFF'
self.changePattern(self.current_pattern_name)
# Initialize the timer
self.frequency = 1.0 / self._LED_protocol['signals']['CAR_SIGNAL_A'][
'frequency']
self.is_on = False
self.cycle_timer = rospy.Timer(
rospy.Duration.from_sec(self.frequency / 2.0), self._cycle_timer)
# Publishers
self.pub_state = rospy.Publisher("~current_led_state",
String,
queue_size=1,
dt_topic_type=TopicType.DRIVER)
# Services
self.srv_set_LED_ = rospy.Service("~set_custom_pattern",
SetCustomLEDPattern,
self.srvSetCustomLEDPattern)
self.srv_set_pattern_ = rospy.Service("~set_pattern", ChangePattern,
self.srvSetPattern)
# Scale intensity of the LEDs
for name, c in self._LED_protocol['colors'].items():
for i in range(3):
c[i] = c[i] * self._LED_scale
# Remap colors if robot does not have an RGB ordering
if self._channel_order[self.robot_type] != "RGB":
protocol = self._LED_protocol
for name, col in self._LED_protocol['colors'].items():
protocol['colors'][name] = self.remapColors(col)
# update LED_protocol
self._LED_protocol = protocol
rospy.set_param("~LED_protocol", protocol)
self.log("Colors remapped to " +
str(self._channel_order[self.robot_type]))
# Turn on the LEDs
self.changePattern('WHITE')
self.log("Initialized.")
def srvSetCustomLEDPattern(self, req):
"""Service to set a custom pattern.
Sets the LEDs to a custom pattern. The :obj:`LEDPattern` message from
:obj:`duckietown_msgs` is used for that.
Args:
req (SetCustomLEDPatternRequest): the requested pattern
"""
# Update the protocol
protocol = self._LED_protocol
protocol['signals']['custom'] = {
'color_mask': req.pattern.color_mask,
'color_list': req.pattern.color_list,
'frequency_mask': req.pattern.frequency_mask,
'frequency': req.pattern.frequency
}
# update LED_protocol
self._LED_protocol = protocol
rospy.set_param("~LED_protocol", protocol)
self.log(
"Custom pattern updated: "
"color_mask: %s, " %
str(self._LED_protocol['signals']['custom']['color_mask']) +
"color_list: %s, " %
str(self._LED_protocol['signals']['custom']['color_list']) +
"frequency_mask: %s, " %
str(self._LED_protocol['signals']['custom']['frequency_mask']) +
"frequency: %s" %
str(self._LED_protocol['signals']['custom']['frequency']))
# Perform the actual change
self.changePattern('custom')
# ---
return SetCustomLEDPatternResponse()
def _cycle_timer(self, event):
"""Timer.
Calls updateLEDs according to the frequency of the current pattern.
Args:
event (TimerEvent): event generated by the timer.
"""
self.updateLEDs()
def updateLEDs(self):
"""Switches the LEDs to the requested signal.
If the pattern is static, changes the color of LEDs according to
the color specified in self.color_list. If a nonzero frequency is set,
toggles on/off the LEDs specified on self.frequency_mask.
"""
# Do nothing if inactive
if not self.switch:
return
elif not self.frequency:
# No oscillation
for i in range(5):
colors = self.pattern[i]
self.led.setRGB(i, colors)
else:
# Oscillate
if self.is_on:
for i in range(5):
if self.frequency_mask[i]:
self.led.setRGB(i, [0, 0, 0])
self.is_on = False
else:
for i in range(5):
colors = self.pattern[i]
self.led.setRGB(i, colors)
self.is_on = True
def srvSetPattern(self, msg):
"""Changes the current pattern according to the pattern name sent in the message.
Args:
msg (String): requested pattern name
"""
self.changePattern(str(msg.pattern_name.data))
return ChangePatternResponse()
def changePattern(self, pattern_name):
"""Change the current LED pattern.
Checks if the requested pattern is different from the current one,
if so changes colors and frequency of LEDs accordingly and
publishes the new current pattern. If the requested pattern name
is not found, it will not change the pattern and will publish ROS
Error log message.
Args:
pattern_name (string): Name of the wanted pattern
"""
if pattern_name:
# No need to change if we already have the right pattern, unless it is other, because
# we might have updated its definition
if self.current_pattern_name == pattern_name and pattern_name != 'custom':
return
elif pattern_name.strip("'").strip(
'"') in self._LED_protocol['signals']:
self.current_pattern_name = pattern_name
else:
self.log(
"Pattern name %s not found in the list of patterns. Change of "
"pattern not executed." % pattern_name,
type='err')
self.log(self._LED_protocol['signals'], type='err')
return
# Extract the color from the protocol config file
color_list = self._LED_protocol['signals'][pattern_name][
'color_list']
if type(color_list) is str:
self.pattern = [self._LED_protocol['colors'][color_list]] * 5
else:
if len(color_list) != 5:
self.log(
"The color list should be a string or a list of length 5. Change of "
"pattern not executed.",
type='err')
return
self.pattern = [[0, 0, 0]] * 5
for i in range(len(color_list)):
self.pattern[i] = self._LED_protocol['colors'][
color_list[i]]
# Extract the frequency from the protocol
self.frequency_mask = self._LED_protocol['signals'][pattern_name][
'frequency_mask']
self.frequency = self._LED_protocol['signals'][pattern_name][
'frequency']
# If static behavior, updated LEDs
if self.frequency == 0:
self.updateLEDs()
# Anyway modify the frequency (to stop timer if static)
self.changeFrequency()
# Loginfo
self.log('Pattern changed to (%r), cycle: %s ' %
(pattern_name, self.frequency))
# Publish current pattern
self.pub_state.publish(self.current_pattern_name)
def changeFrequency(self):
"""Changes current frequency of LEDs
Stops the current cycle_timer, and starts a new one with the
frequency specified in self.frequency. If the frequency is zero,
stops the callback timer.
"""
if self.frequency == 0:
self.cycle_timer.shutdown()
else:
try:
self.cycle_timer.shutdown()
# below, convert to hz
d = 1.0 / (2.0 * self.frequency)
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(d),
self._cycle_timer)
except ValueError as e:
self.frequency = None
self.current_pattern_name = None
def remapColors(self, color):
"""
Remaps a color from RGB to the channel ordering currently set in the
`channel_order` configuration parameter.
Args:
color (:obj:`list` of :obj:`float`): A color triplet
Returns:
:obj:`list` of :obj:`float`: The triplet with reordered channels
"""
# Verify that the requested reordering is valid
allowed_orderings = ['RGB', 'RBG', 'GBR', 'GRB', 'BGR', 'BRG']
requested_ordering = self._channel_order[self.robot_type]
if requested_ordering not in allowed_orderings:
self.log("The current channel order %s is not supported, use one of %s. "
"The remapping was not performed." % \
(requested_ordering, str(allowed_orderings)), type='warn')
return color
reordered_triplet = list()
rgb_map = {'R': 0, 'G': 1, 'B': 2}
for channel_color in requested_ordering:
reordered_triplet.append(color[rgb_map[channel_color]])
return reordered_triplet
def on_shutdown(self):
"""Shutdown procedure.
At shutdown, changes the LED pattern to `LIGHT_OFF`.
"""
# Turn off the lights when the node dies
self.changePattern('LIGHT_OFF')
def __init__(self):
super().__init__("led_emitter_node")
self.node_name = self.get_name()
self.log = self.get_logger()
self.log.info("Initializing....")
self.led = RGB_LED()
self.robot_type = rospy.get_param("~robot_type")
# Add the node parameters to the parameters dictionary and load their default values
# self._LED_protocol = rospy.get_param('~LED_protocol')
# self._LED_scale = rospy.get_param('~LED_scale')
# self._channel_order = rospy.get_param('~channel_order')
# Initialize LEDs to be off
self.pattern = [[0, 0, 0]] * 5
self.frequency_mask = [0] * 5
self.current_pattern_name = 'LIGHT_OFF'
self.changePattern(self.current_pattern_name)
# Initialize the timer
self.frequency = 1.0 / self._LED_protocol['signals']['CAR_SIGNAL_A'][
'frequency']
self.is_on = False
self.cycle_timer = rospy.Timer(
rospy.Duration.from_sec(self.frequency / 2.0), self._cycle_timer)
# Publishers
self.pub_state = rospy.Publisher("~current_led_state",
String,
queue_size=1,
dt_topic_type=TopicType.DRIVER)
# Services
self.srv_set_LED_ = rospy.Service("~set_custom_pattern",
SetCustomLEDPattern,
self.srvSetCustomLEDPattern)
self.srv_set_pattern_ = rospy.Service("~set_pattern", ChangePattern,
self.srvSetPattern)
# Scale intensity of the LEDs
for name, c in self._LED_protocol['colors'].items():
for i in range(3):
c[i] = c[i] * self._LED_scale
# Remap colors if robot does not have an RGB ordering
if self._channel_order[self.robot_type] != "RGB":
protocol = self._LED_protocol
for name, col in self._LED_protocol['colors'].items():
protocol['colors'][name] = self.remapColors(col)
# update LED_protocol
self._LED_protocol = protocol
rospy.set_param("~LED_protocol", protocol)
self.log("Colors remapped to " +
str(self._channel_order[self.robot_type]))
# Turn on the LEDs
self.changePattern('WHITE')
self.log("Initialized.")
class IntersectionSupervisorNode(object):
def __init__(self):
# Save the name of the node
self.node_name = rospy.get_name()
self.led = RGB_LED()
self.turn_LEFT = 0
self.turn_RIGHT = 2
self.turn_STRAIGHT = 1
self.turn_NONE = 3
self.blink = False
self.availableTurns = []
self.turn_direction = self.turn_NONE
self.joy_forward = 0
self.timeout = 5 # seconds
rospy.loginfo("[%s] Initializing." % self.node_name)
# Setup publishers
self.pub_turn_type = rospy.Publisher("~turn_type",
Int16,
queue_size=1,
latch=True)
self.pub_done = rospy.Publisher("~intersection_done",
BoolStamped,
queue_size=1)
# Setup subscribers
self.sub_topic_mode = rospy.Subscriber("~mode",
FSMState,
self.cbMode,
queue_size=1)
self.sub_topic_tag = rospy.Subscriber("~tag",
AprilTagsWithInfos,
self.cbTag,
queue_size=1)
self.sub_joy = rospy.Subscriber("~joy", Joy, self.cbJoy, queue_size=1)
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(.5),
self.blinkTimer)
rospy.loginfo("[%s] Initialized." % self.node_name)
self.rate = rospy.Rate(30) # 10hz
# led setup
self.led.setRGB(4, [.2, .2, .2])
self.led.setRGB(2, [.2, .2, .2])
self.led.setRGB(3, [.3, 0, 0])
self.led.setRGB(1, [.3, 0, 0])
self.led.setRGB(0, [.2, .2, .2])
def blinkTimer(self, event):
if self.turn_direction == self.turn_LEFT:
if self.blink:
self.led.setRGB(1, [1, 0, 0])
self.led.setRGB(0, [1, 1, 1])
self.blink = False
else:
self.led.setRGB(1, [.3, 0, 0])
self.led.setRGB(0, [.2, .2, .2])
self.blink = True
if self.turn_direction == self.turn_RIGHT:
if self.blink:
self.led.setRGB(3, [1, 0, 0])
self.led.setRGB(4, [1, 1, 1])
self.blink = False
else:
self.led.setRGB(3, [.3, 0, 0])
self.led.setRGB(4, [.2, .2, .2])
self.blink = True
if self.turn_direction == self.turn_STRAIGHT:
# lights back to normal
self.led.setRGB(0, [.2, .2, .2])
self.led.setRGB(4, [.2, .2, .2])
self.led.setRGB(3, [.3, 0, 0])
self.led.setRGB(1, [.3, 0, 0])
def cbJoy(self, msg):
self.joy_forward = msg.axes[1]
if msg.buttons[
2] and self.turn_LEFT in self.availableTurns: # or self.joy.axes[3] > 0.2:
if self.turn_direction == self.turn_LEFT:
self.turn_direction = self.turn_STRAIGHT
else:
self.turn_direction = self.turn_LEFT
elif msg.buttons[
1] and self.turn_RIGHT in self.availableTurns: # or self.joy.axes[3] < -0.2:
if self.turn_direction == self.turn_RIGHT:
self.turn_direction = self.turn_STRAIGHT
else:
self.turn_direction = self.turn_RIGHT
if self.turn_direction == self.turn_STRAIGHT and self.turn_STRAIGHT not in self.availableTurns and len(
self.availableTurns) > 0:
self.turn_direction = self.turn_NONE
if self.turn_direction == self.turn_NONE and self.turn_STRAIGHT in self.availableTurns:
self.turn_direction = self.turn_STRAIGHT
def cbTag(
self, tag_msgs
): # this and random_april_tag_turns are duplicated should be turned into a library
if self.fsm_mode == "INTERSECTION_CONTROL":
# loop through list of april tags
for taginfo in tag_msgs.infos:
print taginfo
rospy.loginfo("[%s] tag info." % taginfo)
if taginfo.tag_type == taginfo.SIGN:
self.availableTurns = []
# go through possible intersection types
signType = taginfo.traffic_sign_type
if signType == taginfo.NO_RIGHT_TURN or signType == taginfo.LEFT_T_INTERSECT:
self.availableTurns = [
self.turn_STRAIGHT, self.turn_LEFT
]
elif signType == taginfo.NO_LEFT_TURN or signType == taginfo.RIGHT_T_INTERSECT:
self.availableTurns = [
self.turn_RIGHT, self.turn_STRAIGHT
]
elif signType == taginfo.FOUR_WAY:
self.availableTurns = [
self.turn_RIGHT, self.turn_STRAIGHT, self.turn_LEFT
]
elif signType == taginfo.T_INTERSECTION:
self.availableTurns = [self.turn_RIGHT, self.turn_LEFT]
def cbMode(self, mode_msg):
self.fsm_mode = mode_msg.state
if (self.fsm_mode == "INTERSECTION_CONTROL"):
# self.availableTurns = [0,1,2] # just to test without april tags, set to [] for actual
# brake lights
self.led.setRGB(3, [1, 0, 0])
self.led.setRGB(1, [1, 0, 0])
# force to stop for 2 seconds
rospy.sleep(2)
# default to straight if nothing pressed
self.turn_direction = self.turn_NONE
# if no straight turn avaliable wait until another is choosen
# publish once user presses forward on joystick
time_max = rospy.get_time() + self.timeout
while (self.turn_direction == self.turn_NONE
or not self.joy_forward > 0
) and not (rospy.get_time() > time_max
and self.availableTurns == []):
pass
# turn off brake lights
self.led.setRGB(1, [.3, 0, 0])
self.led.setRGB(3, [.3, 0, 0])
if self.availableTurns == []:
#if timeout and no available turns, just leave instersection control
done = BoolStamped()
done.header.stamp = rospy.Time.now()
done.data = True
self.pub_done.publish(done)
else:
self.pub_turn_type.publish(self.turn_direction)
rospy.loginfo("[%s] Turn type: %i" %
(self.node_name, self.turn_direction))
if self.fsm_mode != "INTERSECTION_CONTROL":
# on exit intersection control, stop blinking
self.availableTurns = []
self.turn_direction = self.turn_NONE
# led setup
self.led.setRGB(4, [.2, .2, .2])
self.led.setRGB(2, [.2, .2, .2])
self.led.setRGB(3, [.3, 0, 0])
self.led.setRGB(1, [.3, 0, 0])
self.led.setRGB(0, [.2, .2, .2])
def setupParameter(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
rospy.set_param(param_name,
value) # Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
def on_shutdown(self):
rospy.loginfo("[%s] Shutting down." % (self.node_name))
class LEDEmitter(object):
def __init__(self):
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.pub_state = rospy.Publisher("~current_led_state",Float32,queue_size=1)
self.sub_pattern = rospy.Subscriber("~change_color_pattern", String, self.changePattern)
self.sub_switch = rospy.Subscriber("~switch",BoolStamped,self.cbSwitch)
self.cycle = None
self.is_on = False
self.active = True
self.protocol = rospy.get_param("~LED_protocol") #should be a list of tuples
self.pattern_off = [[0,0,0]] * 5
scale = 0.5
for _, c in self.protocol['colors'].items():
for i in range(3):
c[i] = c[i] * scale
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(.1), self.cycleTimer)
self.current_pattern_name = None
self.changePattern_('CAR_SIGNAL_A')
def cbSwitch(self, switch_msg): # active/inactive switch from FSM
self.active = switch_msg.data
def cycleTimer(self,event):
if not self.active:
return
if self.is_on:
for i in range(5):
self.led.setRGB(i, [0, 0, 0])
self.is_on = False
else:
for i in range(5):
self.led.setRGB(i, self.pattern[i])
self.is_on = True
def changePattern(self, msg):
self.changePattern_(msg.data)
def changePattern_(self, pattern_name):
if pattern_name:
if (self.current_pattern_name == pattern_name):
return
else:
self.current_pattern_name = pattern_name
rospy.loginfo('changePattern(%r)' % pattern_name)
color = self.protocol['signals'][pattern_name]['color']
self.cycle = self.protocol['signals'][pattern_name]['frequency']
print("color: %s, freq (Hz): %s "%(color, self.cycle))
self.pattern = [[0,0,0]] * 5
self.pattern[2] = self.protocol['colors'][color]
#print(self.pattern)
if pattern_name in ['traffic_light_go', 'traffic_light_stop']:
self.pattern = [self.protocol['colors'][color]] * 5
self.changeFrequency()
def changeFrequency(self):
try:
#self.cycle = msg.data
self.cycle_timer.shutdown()
#below, convert to hz
d = 1.0/(2.0*self.cycle)
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(d), self.cycleTimer)
except ValueError as e:
self.cycle = None
self.current_pattern_name = None
self.pub_state.publish(float(self.cycle))
def __init__(self):
# Hardcoded color values to configure traffic light
# ATTENTION: This skript uses GRB instead of RGB logic to work with the
# newest version of the traffic lights with surface mounted LEDs.
self.green_color = [1, 0, 0]
self.red_color = [0, 1, 0]
self.yellow_color = [1, 1, 0]
self.black_color = [0, 0, 0]
self.blue_color = [0, 0, 1]
# GRB logic
self.purple_color = [0, 1, 1]
self.cyan_color = [1, 0, 1]
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.cycle = None
# Getting the list of reference colors and frequencies
self.protocol = self.setupParameter("~LED_protocol", []) # should be a list of tuples
# FREQUENCIES
# Get red and green light frequency
self.stop_light_freq = self.protocol['signals']['traffic_light_stop']['frequency']
self.go_right_freq = self.protocol['signals']['CAR_SIGNAL_B']['frequency']
self.go_forward_freq = self.protocol['signals']['traffic_light_go']['frequency']
self.go_left_freq = self.protocol['signals']['CAR_SIGNAL_C']['frequency']
# Get the order of the lights
self.traffic_light_list = self.setupParameter("~traffic_light_list", [0, 2, 3, 4])
# LIGHT DURATIONS
# Get go_light duration in seconds
self.go_light_duration = self.setupParameter("~go_light_duration", 5)
# get stop_light duration in seconds
self.stop_light_duration = self.setupParameter("~stop_light_duration", 4)
# PERIODS
self.stop_t = 1.0 / self.stop_light_freq
self.go_forward_t = 1.0 / self.go_forward_freq
self.go_right_t = 1.0 / self.go_right_freq
self.go_left_t = 1.0 / self.go_left_freq
# The particular light is initially red
self.light_on = False
# go_states: FORWARD, RIGHT, LEFT
# go_states initialization
self.go_state = ''
# Initialize all the lights with red
self.stoplightlist = [0,2,3,4]
rospy.loginfo("@start stoplightlist :"+str(self.stoplightlist))
# all LEDs are off
self.traffic_light_state = {0: False, 2: False, 3: False, 4: False}
# yellowlightlist contains all the indices of lights which should emit yellow light
#self.yellowlightlist = []
# Initialise the light_index and its number (not sure if it is necessary in this case)
self.light_index = 2
self.light = self.traffic_light_list[self.light_index] #light = 3
# SUBSCRIBER
# (instead of a string there must be a message type with an index and a string.Index will correspond to the self.light_index and the string will provide information about go_right, go_left or go_forward)
self.sub_switch_light = rospy.Subscriber("~switch_light", String, self.cbSwitchLight)
# CYCLES
rospy.loginfo("cycles start")
self.red_LED_cycle = rospy.Timer(rospy.Duration(0.5 * self.stop_t), self.freqred)
self.goright_LED_cycle = rospy.Timer(rospy.Duration(0.5 * self.go_right_t), self.freq_go_right)
self.goleft_LED_cycle = rospy.Timer(rospy.Duration(0.5 * self.go_left_t), self.freq_go_left)
self.goforward_LED_cycle = rospy.Timer(rospy.Duration(0.5 * self.go_forward_t), self.freq_go_forward)
rospy.loginfo("cycles end")
class TrafficLight(object):
def __init__(self):
# Hardcoded color values to configure traffic light
# ATTENTION: This skript uses GRB instead of RGB logic to work with the
# newest version of the traffic lights with surface mounted LEDs.
self.green_color = [1, 0, 0]
self.red_color = [0, 1, 0]
self.yellow_color = [1, 1, 0]
self.black_color = [0, 0, 0]
self.blue_color = [0, 0, 1]
# GRB logic
self.purple_color = [0, 1, 1]
self.cyan_color = [1, 0, 1]
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.cycle = None
# Getting the list of reference colors and frequencies
self.protocol = self.setupParameter("~LED_protocol", []) # should be a list of tuples
# FREQUENCIES
# Get red and green light frequency
self.stop_light_freq = self.protocol['signals']['traffic_light_stop']['frequency']
self.go_right_freq = self.protocol['signals']['CAR_SIGNAL_B']['frequency']
self.go_forward_freq = self.protocol['signals']['traffic_light_go']['frequency']
self.go_left_freq = self.protocol['signals']['CAR_SIGNAL_C']['frequency']
# Get the order of the lights
self.traffic_light_list = self.setupParameter("~traffic_light_list", [0, 2, 3, 4])
# LIGHT DURATIONS
# Get go_light duration in seconds
self.go_light_duration = self.setupParameter("~go_light_duration", 5)
# get stop_light duration in seconds
self.stop_light_duration = self.setupParameter("~stop_light_duration", 4)
# PERIODS
self.stop_t = 1.0 / self.stop_light_freq
self.go_forward_t = 1.0 / self.go_forward_freq
self.go_right_t = 1.0 / self.go_right_freq
self.go_left_t = 1.0 / self.go_left_freq
# The particular light is initially red
self.light_on = False
# go_states: FORWARD, RIGHT, LEFT
# go_states initialization
self.go_state = ''
# Initialize all the lights with red
self.stoplightlist = [0,2,3,4]
rospy.loginfo("@start stoplightlist :"+str(self.stoplightlist))
# all LEDs are off
self.traffic_light_state = {0: False, 2: False, 3: False, 4: False}
# yellowlightlist contains all the indices of lights which should emit yellow light
#self.yellowlightlist = []
# Initialise the light_index and its number (not sure if it is necessary in this case)
self.light_index = 2
self.light = self.traffic_light_list[self.light_index] #light = 3
# SUBSCRIBER
# (instead of a string there must be a message type with an index and a string.Index will correspond to the self.light_index and the string will provide information about go_right, go_left or go_forward)
self.sub_switch_light = rospy.Subscriber("~switch_light", String, self.cbSwitchLight)
# CYCLES
rospy.loginfo("cycles start")
self.red_LED_cycle = rospy.Timer(rospy.Duration(0.5 * self.stop_t), self.freqred)
self.goright_LED_cycle = rospy.Timer(rospy.Duration(0.5 * self.go_right_t), self.freq_go_right)
self.goleft_LED_cycle = rospy.Timer(rospy.Duration(0.5 * self.go_left_t), self.freq_go_left)
self.goforward_LED_cycle = rospy.Timer(rospy.Duration(0.5 * self.go_forward_t), self.freq_go_forward)
rospy.loginfo("cycles end")
def cbSwitchLight(self, msg):
# Get go_state
# TODO:define a message with the required specifications
self.go_state = msg.data
#self.yellowlightlist = []
# TODO:define a message with the required specifications
#self.light_index = msg.index
self.light = self.traffic_light_list[self.light_index]
self.light_on = True
#self.stoplightlist = self.traffic_light_list[:self.light_index] + self.traffic_light_list[(self.light_index + 1):]
self.stoplightlist =[0,2,4]
rospy.loginfo("sleep start")
rospy.sleep(self.go_light_duration) # Keep the light on$
rospy.loginfo("sleep end")
self.light_on = False
self.stoplightlist = [0,2,3,4]
#self.yellowlightlist = [self.light]
#rospy.sleep(self.stop_light_duration)
rospy.loginfo("cbSwitchLight callback finished")
def freqred(self, event):
rospy.loginfo("stoplightlist:"+str(self.stoplightlist))
rospy.loginfo("traffic_light_state:"+ str(self.traffic_light_state))
for l in self.stoplightlist:
rospy.loginfo("Iterator:"+str(l))
if self.traffic_light_state[l] == True:
self.led.setRGB(l, self.black_color)
self.traffic_light_state[l] = False
else:
self.led.setRGB(l, self.red_color)
rospy.loginfo("RED on")
self.traffic_light_state[l] = True
'''
for l in self.yellowlightlist:
if self.traffic_light_state[l] == True:
self.led.setRGB(l, self.black_color)
self.traffic_light_state[l] = False
else:
self.led.setRGB(l, self.yellow_color)
self.traffic_light_state[l] = True
'''
def freq_go_right(self, event):
if self.light_on == False or self.go_state != 'RIGHT': # Exit if lights should all be red
return
if self.traffic_light_state[self.light] == True:
self.led.setRGB(self.light, self.black_color)
self.traffic_light_state[self.light] = False
else:
self.led.setRGB(self.light, self.purple_color)
self.traffic_light_state[self.light] = True
rospy.loginfo("freq_go_right callback finished")
def freq_go_left(self, event):
if self.light_on == False or self.go_state != 'LEFT': # Exit if lights should all be red
return
if self.traffic_light_state[self.light] == True:
self.led.setRGB(self.light, self.black_color)
self.traffic_light_state[self.light] = False
else:
self.led.setRGB(self.light, self.blue_color)
self.traffic_light_state[self.light] = True
rospy.loginfo("freq_go_left callback finished")
def freq_go_forward(self, event):
if self.light_on == False or self.go_state != 'FORWARD': # Exit if lights should all be red
return
if self.traffic_light_state[self.light] == True:
self.led.setRGB(self.light, self.black_color)
self.traffic_light_state[self.light] = False
else:
self.led.setRGB(self.light, self.green_color)
self.traffic_light_state[self.light] = True
rospy.loginfo("freq_go_forward callback finished")
def setupParameter(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
# Write to parameter server for transparancy
rospy.set_param(param_name, value)
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
class LEDEmitter(object):
def __init__(self):
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.active = True
self.pattern = [[0,0,0]]*5
self.current_pattern_name = 'OFF'
self.changePattern_(self.current_pattern_name)
# Import protocol
self.protocol = rospy.get_param("~LED_protocol")
# If True, the LED turn on and off. Else, they are always on
self.onOff = True
if self.onOff:
self.cycle = 1.0/self.protocol['signals']['CAR_SIGNAL_A']['frequency']
self.is_on = False
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(self.cycle/(2.0)),self.cycleTimer)
# Publish
#self.pub_state = rospy.Publisher("~current_led_state",Float32,queue_size=1)
self.pub_state = rospy.Publisher("~current_led_state",String,queue_size=1)
# Subscribe
self.sub_pattern = rospy.Subscriber("~change_color_pattern",String,self.changePattern)
self.sub_switch = rospy.Subscriber("~switch",BoolStamped,self.cbSwitch)
# Scale intensity of the LEDs
scale = 0.8
for _, c in self.protocol['colors'].items():
for i in range(3):
c[i] = c[i]*scale
def cbSwitch(self, switch_msg): # active/inactive switch from FSM
self.active = switch_msg.data
def cycleTimer(self,event):
if not self.active:
return
elif not self.onOff:
# No oscillation
for i in range(5):
self.led.setRGB(i,[self.pattern[i][0],self.pattern[i][1],self.pattern[i][2]])
else:
# Oscillate
if self.is_on:
for i in range(5):
self.led.setRGB(i,[0,0,0])
self.is_on = False
else:
for i in range(5):
self.led.setRGB(i,[self.pattern[i][0],self.pattern[i][1],self.pattern[i][2]])
self.is_on = True
def changePattern(self, msg):
self.changePattern_(msg.data)
def changePattern_(self, pattern_name):
if pattern_name:
if self.current_pattern_name == pattern_name:
return
else:
self.current_pattern_name = pattern_name
# With joystick
if self.current_pattern_name == 'ON_WHITE':
self.pattern = [self.protocol['colors']['white']]*5
elif self.current_pattern_name == 'ON_RED':
self.pattern = [self.protocol['colors']['red']]*5
elif self.current_pattern_name == 'ON_BLUE':
self.pattern = [self.protocol['colors']['blue']]*5
elif self.current_pattern_name == 'ON_GREEN':
self.pattern = [self.protocol['colors']['green']]*5
elif self.current_pattern_name == 'CAR_SIGNAL_A':
self.current_pattern_name = CoordinationSignal.SIGNAL_A
elif self.current_pattern_name == 'SIGNAL_GREEN':
self.current_pattern_name = CoordinationSignal.SIGNAL_GREEN
elif self.current_pattern_name == 'OFF':
self.current_pattern_name = CoordinationSignal.OFF
else:
self.pattern = [self.protocol['colors']['black']]*5
# With coordination (new)
if self.current_pattern_name == CoordinationSignal.SIGNAL_GREEN:
color = self.protocol['signals'][pattern_name]['color']
self.pattern = [self.protocol['colors'][color]]*5
elif self.current_pattern_name == CoordinationSignal.OFF:
self.pattern = [self.protocol['colors']['black']]*5
else:
color = self.protocol['signals'][pattern_name]['color']
self.pattern = [self.protocol['colors']['black']]*5
self.pattern[2] = self.protocol['colors'][color]
self.pattern[0] = self.protocol['colors'][color]
self.pattern[4] = self.protocol['colors'][color]
# Change frequency (frequency does not change)
self.cycle = self.protocol['signals'][pattern_name]['frequency']
self.changeFrequency()
# Change LEDs
if not self.onOff:
self.cycleTimer([])
# Loginfo
rospy.loginfo('[%s] Pattern changed to (%r), cycle: %s ' %(self.node_name,pattern_name,self.cycle))
# Publish current pattern
self.pub_state.publish(self.current_pattern_name)
def changeFrequency(self):
try:
#self.cycle = msg.data
self.cycle_timer.shutdown()
#below, convert to hz
d = 1.0/(2.0*self.cycle)
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(d), self.cycleTimer)
except ValueError as e:
self.cycle = None
self.current_pattern_name = None
self.pub_state.publish(float(self.cycle))
class IntersectionSupervisorNode(object):
def __init__(self):
# Save the name of the node
self.node_name = rospy.get_name()
self.led = RGB_LED()
self.turn_LEFT = 0
self.turn_RIGHT = 2
self.turn_STRAIGHT = 1
self.turn_NONE = 3
self.blink = False
self.availableTurns = []
self.turn_direction = self.turn_NONE
self.joy_forward = 0
self.timeout = 5 # seconds
rospy.loginfo("[%s] Initializing." % self.node_name)
# Setup publishers
self.pub_turn_type = rospy.Publisher("~turn_type", Int16, queue_size=1, latch=True)
self.pub_done = rospy.Publisher("~intersection_done",BoolStamped,queue_size=1)
# Setup subscribers
self.sub_topic_mode = rospy.Subscriber("~mode", FSMState, self.cbMode, queue_size=1)
self.sub_topic_tag = rospy.Subscriber("~tag", AprilTagsWithInfos, self.cbTag, queue_size=1)
self.sub_joy = rospy.Subscriber("~joy", Joy, self.cbJoy, queue_size=1)
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(.5), self.blinkTimer)
rospy.loginfo("[%s] Initialized." % self.node_name)
self.rate = rospy.Rate(30) # 10hz
# led setup
self.led.setRGB(4, [.2, .2, .2])
self.led.setRGB(2, [.2, .2, .2])
self.led.setRGB(3, [.3, 0, 0])
self.led.setRGB(1, [.3, 0, 0])
self.led.setRGB(0, [.2, .2, .2])
def blinkTimer(self, event):
if self.turn_direction == self.turn_LEFT:
if self.blink:
self.led.setRGB(1, [1, 0, 0])
self.led.setRGB(0, [1, 1, 1])
self.blink = False
else:
self.led.setRGB(1, [.3, 0, 0])
self.led.setRGB(0, [.2, .2, .2])
self.blink = True
if self.turn_direction == self.turn_RIGHT:
if self.blink:
self.led.setRGB(3, [1, 0, 0])
self.led.setRGB(4, [1, 1, 1])
self.blink = False
else:
self.led.setRGB(3, [.3, 0, 0])
self.led.setRGB(4, [.2, .2, .2])
self.blink = True
if self.turn_direction == self.turn_STRAIGHT:
# lights back to normal
self.led.setRGB(0, [.2, .2, .2])
self.led.setRGB(4, [.2, .2, .2])
self.led.setRGB(3, [.3, 0, 0])
self.led.setRGB(1, [.3, 0, 0])
def cbJoy(self, msg):
self.joy_forward = msg.axes[1]
if msg.buttons[2] and self.turn_LEFT in self.availableTurns: # or self.joy.axes[3] > 0.2:
if self.turn_direction == self.turn_LEFT:
self.turn_direction = self.turn_STRAIGHT
else:
self.turn_direction = self.turn_LEFT
elif msg.buttons[1] and self.turn_RIGHT in self.availableTurns: # or self.joy.axes[3] < -0.2:
if self.turn_direction == self.turn_RIGHT:
self.turn_direction = self.turn_STRAIGHT
else:
self.turn_direction = self.turn_RIGHT
if self.turn_direction == self.turn_STRAIGHT and self.turn_STRAIGHT not in self.availableTurns and len(self.availableTurns) > 0:
self.turn_direction = self.turn_NONE
if self.turn_direction == self.turn_NONE and self.turn_STRAIGHT in self.availableTurns:
self.turn_direction = self.turn_STRAIGHT
def cbTag(self, tag_msgs): # this and random_april_tag_turns are duplicated should be turned into a library
if self.fsm_mode == "INTERSECTION_CONTROL":
# loop through list of april tags
for taginfo in tag_msgs.infos:
print taginfo
rospy.loginfo("[%s] tag info." % taginfo)
if taginfo.tag_type == taginfo.SIGN:
self.availableTurns = []
# go through possible intersection types
signType = taginfo.traffic_sign_type
if signType == taginfo.NO_RIGHT_TURN or signType == taginfo.LEFT_T_INTERSECT:
self.availableTurns = [self.turn_STRAIGHT, self.turn_LEFT]
elif signType == taginfo.NO_LEFT_TURN or signType == taginfo.RIGHT_T_INTERSECT:
self.availableTurns = [self.turn_RIGHT, self.turn_STRAIGHT]
elif signType == taginfo.FOUR_WAY:
self.availableTurns = [self.turn_RIGHT, self.turn_STRAIGHT, self.turn_LEFT]
elif signType == taginfo.T_INTERSECTION:
self.availableTurns = [self.turn_RIGHT, self.turn_LEFT]
def cbMode(self, mode_msg):
self.fsm_mode = mode_msg.state
if (self.fsm_mode == "INTERSECTION_CONTROL"):
# self.availableTurns = [0,1,2] # just to test without april tags, set to [] for actual
# brake lights
self.led.setRGB(3, [1, 0, 0])
self.led.setRGB(1, [1, 0, 0])
# force to stop for 2 seconds
rospy.sleep(2)
# default to straight if nothing pressed
self.turn_direction = self.turn_NONE
# if no straight turn avaliable wait until another is choosen
# publish once user presses forward on joystick
time_max = rospy.get_time() + self.timeout
while (self.turn_direction == self.turn_NONE or not self.joy_forward > 0) and not (rospy.get_time() > time_max and self.availableTurns ==[]):
pass
# turn off brake lights
self.led.setRGB(1, [.3, 0, 0])
self.led.setRGB(3, [.3, 0, 0])
if self.availableTurns == []:
#if timeout and no available turns, just leave instersection control
done = BoolStamped()
done.header.stamp = rospy.Time.now()
done.data = True
self.pub_done.publish(done)
else:
self.pub_turn_type.publish(self.turn_direction)
rospy.loginfo("[%s] Turn type: %i" % (self.node_name, self.turn_direction))
if self.fsm_mode != "INTERSECTION_CONTROL":
# on exit intersection control, stop blinking
self.availableTurns = []
self.turn_direction = self.turn_NONE
# led setup
self.led.setRGB(4, [.2, .2, .2])
self.led.setRGB(2, [.2, .2, .2])
self.led.setRGB(3, [.3, 0, 0])
self.led.setRGB(1, [.3, 0, 0])
self.led.setRGB(0, [.2, .2, .2])
def setupParameter(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
rospy.set_param(param_name, value) # Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
def on_shutdown(self):
rospy.loginfo("[%s] Shutting down." % (self.node_name))
class TrafficLight(object):
def __init__(self):
# Hardcoded color values to configure traffic light
# ATTENTION: This skript uses GRB instead of RGB logic to work with the
# newest version of the traffic lights with surface mounted LEDs.
self.green_color = [1, 0, 0]
self.red_color = [0, 1, 0]
self.yellow_color = [1, 1, 0]
self.black_color = [0, 0, 0]
self.blue_color = [0, 0, 1]
# GRB logic
self.purple_color = [0, 1, 1]
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.cycle = None
self.protocol = self.setupParameter("~LED_protocol", [])
#FREQUENCIES
# f1 = 1.9
self.greenlight_freq = self.protocol['signals']['CAR_SIGNAL_A_OLD']['frequency']
#f2 = 4
self.purplelight_freq = self.protocol['signals']['CAR_SIGNAL_A']['frequency']
#f3 = 5.7
self.redlight_freq =self.protocol['signals']['CAR_SIGNAL_GREEN']['frequency']
#f4 = 7.8
self.yellowlight_freq = self.protocol['signals']['traffic_light_go']['frequency']
#PERIODS
self.redlight_t = 1.0/self.redlight_freq
self.greenlight_t = 1.0/self.greenlight_freq
self.purplelight_t = 1.0/self.purplelight_freq
self.yellowlight_t = 1.0/self.yellowlight_freq
#GREEN TIME
self.green_time = 30
rospy.loginfo("Green Time is "+ str(self.green_time))
#List of red lights
self.redlight_list = [0,2,3,4]
self.light_number_special = 0
self.light_color_special = "red"
#Get the static April Tags (right now it is hardcoded)
#keys : apriltag ID values: positions
#TODO: Find a way to get the tag ids automaticaly via a YAML file...etc
#self.staticAprilTags = defaultdict(dict)
self.initial_point = Point()
self.initial_point.x = 0.0
self.initial_point.y = 0.0
self.initial_point.z = 0.0
self.staticAprilTags = {'350':{'Position':self.initial_point},'339':{'Position':self.initial_point},'309':{'Position':self.initial_point}}
#Save the moving April Tags as {'tagID':{'Position':Point,'Neighbor':tagIDNeighbor}}
#this dictionary will be updated periodicaly
self.movingAprilTags = defaultdict(dict)
'''
#TODO:change the lanch file accordingly to load the parameters
#path :catkin_ws/src/20-indefinite-navigation/apriltags_ros/signs_and_tags/apriltagsDB.yaml
self.AprilTags = self.self.setupParameter("~apriltagsDB", [])
self.VehicleTags =str( [obj['tag_id'] for obj in self.AprilTags if obj[tag_type] == 'Vehicle'])
self.LocalizationTags =str( [obj['tag_id'] for obj in self.AprilTags if obj[tag_type] == 'Localization'])
'''
#Light States
self.light_state_dict = {0:False , 2:False, 3:False, 4:False}
#Subscriber
self.sub_traffic_light_switch = rospy.Subscriber("~traffic_light_switch",String,self.cbTrafficLight_switch)
self.sub_green_time = rospy.Subscriber("~green_time",Int8,self.cbGreenTime)
self.sub_poses = rospy.Subscriber("/poses_acquisition/poses",AprilTagDetection,self.cbPoses)
self.charging_1_switch = rospy.Subscriber("~charging_1_switch", Int8, self.cbcharging_1_switch)
self.charging_2_switch = rospy.Subscriber("~charging_2_switch", Int8, self.cbcharging_2_switch)
#self.sub_traffic_light_switch = rospy.Subscriber("~traffic_light_switch",String,self.cbDebug)
rospy.loginfo("endless loop start")
self.timer_red = rospy.Timer(rospy.Duration(0.5*self.redlight_t),self.cbTimerRed)
rospy.loginfo("endless loop end")
self.charger1_capacity = 8
self.charger2_capacity = 8
self.charger1_size = 3
self.charger2_size = 3
self.charger1_size_old = self.charger1_size
self.charger2_size_old = self.charger2_size
self.charger_next_free = 2
rospy.Timer(rospy.Duration(1),self.cbChargingManager) #starting timer for ChargingManager
def cbGreenTime(self,msg):
self.green_time = msg.data
rospy.loginfo("Green Time is: "+str(self.green_time))
#Time Threshold for finding out which position was the last one (3ms)
self.threshold = 0.000001
#
self.bookkeeping_time = 0.1
#'duckiebot':timestamp_secs will be saved in dictionaries of chargers
self.chargers ={'charger1':{},'charger2':{}}
self.sub_poses = rospy.Subscriber("/poses_acquisition/poses",AprilTagDetection,self.cbPoses)
def cbcharging_1_switch(self,msg):
self.charger1_size = msg.data
def cbcharging_2_switch(self,msg):
self.charger2_size = msg.data
def lightToggle(self,light_number,light_color):
#rospy.loginfo("lightToggle function started")
if(self.light_state_dict[light_number] == True):
self.led.setRGB(light_number, self.black_color)
self.light_state_dict[light_number] = False
#traffic light is switched off
else:
if(light_color == "red"):
self.led.setRGB(light_number, self.red_color)
elif(light_color == "green"):
self.led.setRGB(light_number, self.red_color)
elif(light_color == "purple"):
self.led.setRGB(light_number, self.red_color)
else:
self.led.setRGB(light_number, self.red_color)
self.light_state_dict[light_number] = True
#traffic light is switched on
#rospy.loginfo("lightToggle function ended")
#decision making where duckiebot at entrance should go
def cbChargingManager(self, event):
if((self.charger1_size != self.charger1_size_old) or (self.charger2_size != self.charger2_size_old)): #only allowing check if charger_next_free was updated
if(self.charger1_size > self.charger2_size):
self.charger_next_free = 2
rospy.loginfo("[" + self.node_name + "]" + " Next free charger: " + str(self.charger_next_free))
self.timer_red.shutdown()
self.timer_red = rospy.Timer(rospy.Duration(0.5*self.redlight_t),self.cbTimerRed)
else :
self.charger_next_free = 1
rospy.loginfo("[" + self.node_name + "]" + " Next free charger: " + str(self.charger_next_free))
self.timer_red.shutdown()
self.timer_red = rospy.Timer(rospy.Duration(0.5*self.greenlight_t),self.cbTimerRed)
self.charger1_size_old = self.charger1_size
self.charger2_size_old = self.charger2_size
def cbTimerRed(self,event):
for light_number in self.redlight_list:
self.lightToggle(light_number,"red")
def cbTimerSpecial(self,event):
self.lightToggle(self.light_number_special,self.light_color_special)
def cbTrafficLight_switch(self,msg):
rospy.loginfo("cbTrafficlight start")
#get message string
#color is a string and number is an integer
color, number = self.getMSG(msg)
self.light_number_special = number
self.light_color_special = color
rospy.loginfo("published: "+color+" "+str(number))
#remove the light which should blink differently
if(number in self.redlight_list):
self.redlight_list.remove(number)
#kill the old timer
self.timer_red.shutdown()
rospy.loginfo("timer_red killed")
#start two new timers
#get color frequency and set timer
if(color == "green"):
self.timer_special = rospy.Timer(rospy.Duration(0.5*self.greenlight_t),self.cbTimerSpecial)
elif(color == "purple"):
self.timer_special = rospy.Timer(rospy.Duration(0.5*self.purplelight_t),self.cbTimerSpecial)
else:
self.timer_special = rospy.Timer(rospy.Duration(0.5*self.yellowlight_t),self.cbTimerSpecial)
#timer_special is the timer of the light which will blink differently than the other ones
self.timer_others = rospy.Timer(rospy.Duration(0.5*self.redlight_t),self.cbTimerRed)
rospy.loginfo("timer_special and timer_others started")
#wait for self.green_time seconds
rospy.sleep(rospy.Duration(45))
self.timer_special.shutdown()
self.timer_others.shutdown()
rospy.loginfo("timer_special and timer_others killed")
#put the number back into the list and sort the list
self.redlight_list.append(number)
self.redlight_list.sort()
self.timer_red = rospy.Timer(rospy.Duration(0.5*self.redlight_t),self.cbTimerRed)
rospy.loginfo("timer_red started")
rospy.loginfo("cbTrafficlight end ")
def getMSG(self,msg):#TODO: extract "" from the message string
#msg should be "#NUMBER#COLOR#"
tmp = str(msg.data).split("#")
number = int(tmp[1])
color = tmp[2]
return color, number
def cbDebug(self,msg):
rospy.loginfo(str(msg))
color, number = self.getMSG(msg)
rospy.loginfo("color "+color+ "number : "+ str(number) )
#-----------------------------LED Management END-----------------------------#
#-----------------------------Charger Management START-----------------------------#
def DebugLists(self,event):
rospy.loginfo("STATIC AT:"+ str(self.staticAprilTags))
rospy.loginfo("MOVING AT:"+str(self.movingAprilTags))
def updateChargerSizes(self,event):
for charger in self.chargers.keys():
if(charger == 'charger1'):
self.current_size1 = len(self.chargers[charger].keys())
elif(charger == 'charger2'):
self.current_size1 = len(self.chargers[charger].keys())
else:
rospy.loginfo("["+self.node_name+"]Something unexpected has happened in updateChargerSizes")
#Delete the tagID from self.movingAprilTags dictionary
def deleteBot(self,tagID):
for botID in self.movingAprilTags.keys():
if(tagID == botID):
try:
del self.movingAprilTags[tagID]
except KeyError:
rospy.loginfo("["+self.node_name+"] tagID "+ str(tagID)+" could not be found in "+str(self.movingAprilTags)+" movingAprilTags: "+str(self.movingAprilTags))
def updateLastNeighbor(self,event):
current_time = rospy.get_rostime().to_sec()
for botID in self.movingAprilTags.keys():
#If the last time stamp is not updated for a long time, take action according to the last neighbor
rospy.loginfo(str(abs(current_time-self.movingAprilTags[botID]['timestamp']))+ " time difference "+ str(botID))
if(abs(current_time-self.movingAprilTags[botID]['timestamp'])> self.threshold):
if(self.movingAprilTags[botID]['neighbor'] == self.direction_CH1):
rospy.loginfo(str(botID)+" on its way to charger 1")
self.chargers['charger1'][botID] = self.movingAprilTags[botID]['timestamp']
self.deleteBot(botID) #deletes bot from self.movingAprilTags
elif(self.movingAprilTags[botID]['neighbor'] == self.direction_CH2):
rospy.loginfo(str(botID)+" on its way to charger 2")
self.chargers['charger2'][botID] = self.movingAprilTags[botID]['timestamp']
self.deleteBot(botID) #deletes bot from self.movingAprilTags
#self.entrance normally self.exit
elif(self.movingAprilTags[botID]['neighbor'] == self.entrance):
rospy.loginfo(str(botID)+" on its way to exit")
self.releasePlace(botID)
self.deleteBot(botID)
rospy.loginfo("CHARGERS "+str(self.chargers))
def releasePlace(self,tagID):
#delete the tagID from self.chargers
for charger in self.chargers.keys():
#List of bots in charger
bots = list(self.chargers[charger].keys())
#Delete the tagID from chargers dictionary
if(tagID in bots):
try:
del self.chargers[charger][tagID]
rospy.loginfo(str(tagID)+" released charger"+str(charger))
except KeyError :
rospy.loginfo("["+self.node_name+"] tagID "+ str(tagID)+" could not be found in "+str(charger)+". Charger has "+str(bots))
def isDuckieBot(self,tagID):
#look up whether the april tag is owned by a duckiebot
'''
if(tagID in self.VehicleTags):
return True
else:
return False
'''
return tagID == '400'
def isStaticTag(self,tagID):
'''
if(tagID in self.LocalizationTags):
return True
else:
return False
'''
return tagID == '339' or tagID =='350' or tagID == '309'
#Finds the neighbor april tag to the given moving april tag
def NearestNeighbor(self,tagPose):
nearestTag = ''
nearestPosition = 16000000
#{'tagID':{'position':Point,'neighbor':tagIDNeighbor,'timestamp':time,'charger':chargerID}}
for tagIDNeighbor, Neighbor in self.staticAprilTags.items():
#absolute value of distance between a static tag and amoving tag squared
d = (Neighbor['Position'].x -tagPose.x)**2 + (Neighbor['Position'].y -tagPose.y)**2
if(d < nearestPosition):
nearestPosition = d
nearestTag = tagIDNeighbor
return nearestTag
'''
def ChargingManager(self,tagID,tagInfo):
position = tagInfo['Position']
neighbor = tagInfo['Neighbor']
if(len(neighbor) > 0 ):
rospy.loginfo("["+self.node_name+"] Duckiebot "+tagID+" is near to the Localization Tag "+neighbor)
else:
rospy.loginfo("["+self.node_name+"] Something unexpected has happened.")
#TODO: save the tag id in a dictionary where the charger names and duckiebots in those chargers are documented
'''
def cbPoses(self,msg):
#rospy.loginfo("["+self.node_name+"]"+" cbPoses message arrived: TagId: "+str(msg.tag_id)+" center: "+str(msg.center))
current_time = rospy.get_rostime().to_sec()
tagID = str(msg.tag_id)
#3D Point just using x and y to save the pixels of the center of an april tag
tagPose = Point()
tagPose.x = msg.center[0]
tagPose.y = msg.center[1]
tagPose.z = 0
if(tagID in self.staticAprilTags.keys()):
self.staticAprilTags[tagID]['position'] = tagPose
if(self.isDuckieBot(tagID)):
self.movingAprilTags[tagID]['position'] = tagPose
self.movingAprilTags[tagID]['timestamp'] = current_time
self.movingAprilTags[tagID]['neighbor'] = self.NearestNeighbor(tagPose)
def setupParameter(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
# Write to parameter server for transparancy
rospy.set_param(param_name, value)
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
def __init__(self, use_leds=False):
rospy.init_node('parking_detection', anonymous=True)
rospy.on_shutdown(self.shutdown)
self.cam_sub = rospy.Subscriber('/camera_info', CameraInfo,
self.cam_info_callback)
self.pub_steer = rospy.Publisher('/obstacle_steering',
Float32,
queue_size=10)
self.use_leds = use_leds
self.P = None
direction = 0
if self.use_leds:
# Only import if we are running on raspberry pi
# error otherwise
from rgb_led import RGB_LED
self.led = RGB_LED()
# rear indicator LEDS
self.led.setRGB(3, [1, 1, 1])
self.led.setRGB(1, [1, 1, 1])
# turn on front LEDS
self.led.setRGB(0, [1, 1, 1])
self.led.setRGB(2, [1, 1, 1])
self.led.setRGB(4, [1, 1, 1])
# Remember up to 1 second in the past
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
rate = rospy.Rate(2.0)
while not rospy.is_shutdown():
try:
transform = tfBuffer.lookup_transform('yield_sign', 'camera',
rospy.Time(0))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rate.sleep()
continue
transform_time = transform.header.stamp
current_time = rospy.Time.now()
x = transform.transform.translation.x
y = transform.transform.translation.y
z = transform.transform.translation.z
dist = math.sqrt(x**2 + y**2)
image_coords = np.matmul(self.P,
np.array([x, y, z, 1]).reshape(4, 1))
x_img = image_coords[0] / image_coords[2]
y_img = image_coords[1] / image_coords[2]
print('x: ' + str(x_img))
print('y: ' + str(y_img))
if current_time.secs - transform_time.secs < 2 and dist < 0.2: # and within distance
print('Obstacle in view')
if direction == 1 or ((x_img > self.width / 2)
and direction == 0):
direction = 1
target = 7 * self.width / 8
#target = self.width
error = x_img - target
if error > 0:
error = 0
if self.use_leds:
self.led.setRGB(3, [1, 1, 1])
else:
if self.use_leds:
self.led.setRGB(3, [1, 0, 0])
self.led.setRGB(1, [1, 1, 1])
elif direction == -1 or ((x_img < self.width / 2)
and direction == 0):
direction = -1
target = self.width / 8
#target = 0
error = x_img - target
if error < 0:
error = 0
if self.use_leds:
self.led.setRGB(1, [1, 1, 1])
else:
if self.use_leds:
self.led.setRGB(1, [1, 0, 0])
self.led.setRGB(3, [1, 1, 1])
#error = x_img - target
self.pub_steer.publish(error)
print('error: ' + str(error))
else:
direction = 0
if self.use_leds:
self.led.setRGB(3, [1, 1, 1])
self.led.setRGB(1, [1, 1, 1])
print('Obstacle not in view')
self.pub_steer.publish(0)
print('---')
class LEDEmitter(object):
def __init__(self):
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.pub_state = rospy.Publisher("~current_led_state",
Float32,
queue_size=1)
self.sub_pattern = rospy.Subscriber("~change_color_pattern", String,
self.changePattern)
self.cycle = None
self.is_on = False
self.changePattern_('CAR_SIGNAL_A')
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(.1),
self.cycleTimer)
# take current time
# self.t0 = <time>
def cycleTimer(self, event):
if self.is_on:
for i in range(5):
self.led.setRGB(i, [0, 0, 0])
self.is_on = False
else:
for i in range(5):
self.led.setRGB(i, self.pattern[i])
self.is_on = True
def changePattern(self, msg):
self.changePattern_(msg.data)
def changePattern_(self, pattern_name):
rospy.loginfo('changePattern(%r)' % pattern_name)
colors = {
'green': [0, 1, 0],
'red': [1, 0, 0],
'blue': [0, 0, 1],
'white': [0, 0, 0],
'yellow': [1, 1, 0],
'purple': [1, 0, 1],
'cyan': [0, 1, 1],
'black': [0, 0, 0],
}
scale = 0.5
for _, c in colors.items():
for i in range(3):
c[i] = c[i] * scale
f1 = 2.8
f2 = 4.1
f3 = 5.0
if pattern_name in [
'off', 'CAR_SIGNAL_A', 'CAR_SIGNAL_B', 'CAR_SIGNAL_C'
]:
m = {
'CAR_SIGNAL_A': ('green', f1),
'CAR_SIGNAL_B': ('purple', f2),
'CAR_SIGNAL_C': ('yellow', f3),
'off': ('black', f3),
}
color, self.cycle = m[pattern_name]
self.pattern = [[0, 0, 0]] * 5
self.pattern[2] = colors[color]
self.pattern_off = [[0, 0, 0]] * 5
elif pattern_name in ['traffic_light_go', 'traffic_light_stop']:
m = {
'traffic_light_go': ('green', f1),
'traffic_light_stop': ('red', f3),
}
color, self.cycle = m[pattern_name]
self.pattern = [colors[color]] * 5
self.pattern_off = [[0, 0, 0]] * 5
def changeState(self, msg):
if msg.data == self.cycle:
self.cycle = msg.data
# It is the same nothing changed
else:
try:
self.cycle = msg.data
self.cycle_timer.shutdown()
#below, convert to hz
d = 1.0 / (2.0 * self.cycle)
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(d),
self.cycleTimer)
except ValueError as e:
self.cycle = None
self.pub_state.publish(float(self.cycle))
pass
# PROGRAM START
database.Load_Map()
encoder_offsets = database.Load_Calibration()
# Positional encoders - used to select latitude and longitude
encoders_thread = Positional_Encoders(2, "Encoders", encoder_offsets[0],
encoder_offsets[1])
encoders_thread.start()
display_thread = Display(3, "Display")
display_thread.start()
rgb_led = RGB_LED(20, "RGB_LED")
rgb_led.start()
scheduler = Scheduler(50, "SCHEDULER")
scheduler.start()
set_volume(volume)
while True:
if state == "start":
# Entry - setup state
if state_entry:
state_entry = False
display_thread.message(line_1="Radio Globe",
line_2="Made for DesignSpark",
line_3="by Jude Pullen and",
from rgb_led import RGB_LED from time import sleep from rgb_led import * from duckietown_lights import * led = RGB_LED() led.setRGB(TOP, GREEN) led.setRGB(BACK_LEFT, RED) led.setRGB(BACK_RIGHT, RED) led.setRGB(FRONT_LEFT, WHITE) led.setRGB(FRONT_RIGHT, WHITE) sleep(1000*1000) del led
class TrafficLight(object):
def __init__(self):
# Hardcoded color values to configure traffic light
# ATTENTION: This skript uses GRB instead of RGB logic to work with the
# newest version of the traffic lights with surface mounted LEDs.
self.green_color = [1, 0, 0]
self.red_color = [0, 1, 0]
self.yellow_color = [1, 1, 0]
self.black_color = [0, 0, 0]
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.cycle = None
self.protocol = self.setupParameter(
"~LED_protocol", []) # should be a list of tuples
self.greenlight_freq = self.protocol['signals']['traffic_light_go']['frequency']
self.redlight_freq = self.protocol['signals']['traffic_light_stop']['frequency']
self.traffic_light_list = self.setupParameter(
"~traffic_light_list", [0, 2, 3, 4]) # order of lights
self.greenlight_duration = self.setupParameter(
"~greenlight_duration", 5) # in seconds
self.allred_duration = self.setupParameter(
"~allred_duration", 4) # in seconds
self.redlight_t = 1.0/self.redlight_freq
self.greenlight_t = 1.0/self.greenlight_freq
self.green_on = False
self.green_i = 0
self.green = self.traffic_light_list[self.green_i]
self.redlightlist = self.traffic_light_list[:self.green_i] + \
self.traffic_light_list[(self.green_i+1):]
self.traffic_light_state = {
0: False, 4: False, 2: False, 3: False} # All LEDs are off
self.yellowlightlist = []
self.traffic_cycle = rospy.Timer(rospy.Duration(
(self.greenlight_duration+self.allred_duration)), self.switchGreen)
self.redLED_cycle = rospy.Timer(
rospy.Duration(0.5*self.redlight_t), self.freqred)
self.greenLED_cycle = rospy.Timer(
rospy.Duration(0.5*self.greenlight_t), self.freqgreen)
def switchGreen(self, event):
self.yellowlightlist = []
self.green_i = (self.green_i+1) % 4 # Move to next light in list
self.green = self.traffic_light_list[self.green_i]
self.green_on = True
self.redlightlist = self.traffic_light_list[:self.green_i] + \
self.traffic_light_list[(self.green_i+1):]
rospy.sleep(self.greenlight_duration) # Keep the green light on
self.green_on = False # Turn off the green light
self.yellowlightlist = [self.green]
rospy.sleep(self.allred_duration)
self.redlightlist = self.traffic_light_list[0:]
def freqred(self, event):
for light in self.redlightlist:
if self.traffic_light_state[light] == True:
self.led.setRGB(light, self.black_color)
self.traffic_light_state[light] = False
else:
self.led.setRGB(light, self.red_color)
self.traffic_light_state[light] = True
for light in self.yellowlightlist:
if self.traffic_light_state[light] == True:
self.led.setRGB(light, self.black_color)
self.traffic_light_state[light] = False
else:
self.led.setRGB(light, self.yellow_color)
self.traffic_light_state[light] = True
def freqgreen(self, event):
if self.green_on == False: # Exit if lights should all be red
return
if self.traffic_light_state[self.green] == True:
self.led.setRGB(self.green, self.black_color)
self.traffic_light_state[self.green] = False
else:
self.led.setRGB(self.green, self.green_color)
self.traffic_light_state[self.green] = True
def setupParameter(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
# Write to parameter server for transparancy
rospy.set_param(param_name, value)
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
class LEDEmitter(object):
def __init__(self):
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.pub_state = rospy.Publisher("~current_led_state",Float32,queue_size=1)
self.sub_pattern = rospy.Subscriber("~change_color_pattern", String, self.changePattern)
self.cycle = None
self.is_on = False
self.changePattern_('CAR_SIGNAL_A')
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(.1), self.cycleTimer)
# take current time
# self.t0 = <time>
def cycleTimer(self,event):
if self.is_on:
for i in range(5):
self.led.setRGB(i, [0, 0, 0])
self.is_on = False
else:
for i in range(5):
self.led.setRGB(i, self.pattern[i])
self.is_on = True
def changePattern(self, msg):
self.changePattern_(msg.data)
def changePattern_(self, pattern_name):
rospy.loginfo('changePattern(%r)' % pattern_name)
colors = {
'green': [0,1,0],
'red': [1,0,0],
'blue': [0,0,1],
'white': [0,0,0],
'yellow': [1,1,0],
'purple': [1,0,1],
'cyan': [0,1,1],
'black': [0,0,0],
}
scale = 0.5
for _, c in colors.items():
for i in range(3):
c[i] = c[i] * scale
f1 = 2.8
f2 = 4.1
f3 = 5.0
if pattern_name in ['off', 'CAR_SIGNAL_A', 'CAR_SIGNAL_B', 'CAR_SIGNAL_C']:
m = {
'CAR_SIGNAL_A': ('green', f1),
'CAR_SIGNAL_B': ('purple', f2),
'CAR_SIGNAL_C': ('yellow', f3),
'off': ('black', f3),
}
color, self.cycle = m[pattern_name]
self.pattern = [[0,0,0]] * 5
self.pattern[2] = colors[color]
self.pattern_off = [[0,0,0]]*5
elif pattern_name in ['traffic_light_go', 'traffic_light_stop']:
m = {
'traffic_light_go': ('green', f1),
'traffic_light_stop': ('red', f3),
}
color, self.cycle = m[pattern_name]
self.pattern = [colors[color]] * 5
self.pattern_off = [[0,0,0]]*5
def changeState(self,msg):
if msg.data == self.cycle:
self.cycle = msg.data
# It is the same nothing changed
else:
try:
self.cycle = msg.data
self.cycle_timer.shutdown()
#below, convert to hz
d = 1.0/(2.0*self.cycle)
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(d), self.cycleTimer)
except ValueError as e:
self.cycle = None
self.pub_state.publish(float(self.cycle))
def __init__(self):
# Hardcoded color values to configure traffic light
# ATTENTION: This skript uses GRB instead of RGB logic to work with the
# newest version of the traffic lights with surface mounted LEDs.
self.green_color = [1, 0, 0]
self.red_color = [0, 1, 0]
self.yellow_color = [1, 1, 0]
self.black_color = [0, 0, 0]
self.blue_color = [0, 0, 1]
# GRB logic
self.purple_color = [0, 1, 1]
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.cycle = None
self.protocol = self.setupParameter("~LED_protocol", [])
#FREQUENCIES
# f1 = 1.9
self.greenlight_freq = self.protocol['signals']['CAR_SIGNAL_A_OLD']['frequency']
#f2 = 4
self.purplelight_freq = self.protocol['signals']['CAR_SIGNAL_A']['frequency']
#f3 = 5.7
self.redlight_freq =self.protocol['signals']['CAR_SIGNAL_GREEN']['frequency']
#f4 = 7.8
self.yellowlight_freq = self.protocol['signals']['traffic_light_go']['frequency']
#PERIODS
self.redlight_t = 1.0/self.redlight_freq
self.greenlight_t = 1.0/self.greenlight_freq
self.purplelight_t = 1.0/self.purplelight_freq
self.yellowlight_t = 1.0/self.yellowlight_freq
#GREEN TIME
self.green_time = 30
rospy.loginfo("Green Time is "+ str(self.green_time))
#List of red lights
self.redlight_list = [0,2,3,4]
self.light_number_special = 0
self.light_color_special = "red"
#Get the static April Tags (right now it is hardcoded)
#keys : apriltag ID values: positions
#TODO: Find a way to get the tag ids automaticaly via a YAML file...etc
#self.staticAprilTags = defaultdict(dict)
self.initial_point = Point()
self.initial_point.x = 0.0
self.initial_point.y = 0.0
self.initial_point.z = 0.0
self.staticAprilTags = {'350':{'Position':self.initial_point},'339':{'Position':self.initial_point},'309':{'Position':self.initial_point}}
#Save the moving April Tags as {'tagID':{'Position':Point,'Neighbor':tagIDNeighbor}}
#this dictionary will be updated periodicaly
self.movingAprilTags = defaultdict(dict)
'''
#TODO:change the lanch file accordingly to load the parameters
#path :catkin_ws/src/20-indefinite-navigation/apriltags_ros/signs_and_tags/apriltagsDB.yaml
self.AprilTags = self.self.setupParameter("~apriltagsDB", [])
self.VehicleTags =str( [obj['tag_id'] for obj in self.AprilTags if obj[tag_type] == 'Vehicle'])
self.LocalizationTags =str( [obj['tag_id'] for obj in self.AprilTags if obj[tag_type] == 'Localization'])
'''
#Light States
self.light_state_dict = {0:False , 2:False, 3:False, 4:False}
#Subscriber
self.sub_traffic_light_switch = rospy.Subscriber("~traffic_light_switch",String,self.cbTrafficLight_switch)
self.sub_green_time = rospy.Subscriber("~green_time",Int8,self.cbGreenTime)
self.sub_poses = rospy.Subscriber("/poses_acquisition/poses",AprilTagDetection,self.cbPoses)
self.charging_1_switch = rospy.Subscriber("~charging_1_switch", Int8, self.cbcharging_1_switch)
self.charging_2_switch = rospy.Subscriber("~charging_2_switch", Int8, self.cbcharging_2_switch)
#self.sub_traffic_light_switch = rospy.Subscriber("~traffic_light_switch",String,self.cbDebug)
rospy.loginfo("endless loop start")
self.timer_red = rospy.Timer(rospy.Duration(0.5*self.redlight_t),self.cbTimerRed)
rospy.loginfo("endless loop end")
self.charger1_capacity = 8
self.charger2_capacity = 8
self.charger1_size = 3
self.charger2_size = 3
self.charger1_size_old = self.charger1_size
self.charger2_size_old = self.charger2_size
self.charger_next_free = 2
rospy.Timer(rospy.Duration(1),self.cbChargingManager) #starting timer for ChargingManager
def __init__(self, node_name):
# Initialize the DTROS parent class
super(LEDEmitterNode, self).__init__(node_name=node_name)
self.led = RGB_LED()
self.robot_type = rospy.get_param("~robot_type")
# Add the node parameters to the parameters dictionary and load their default values
self.parameters['~LED_protocol'] = None
self.parameters['~LED_scale'] = None
self.parameters['~channel_order'] = None
self.updateParameters()
# Import protocol
self.protocol = rospy.get_param("~LED_protocol")
# Import parameters
self.scale = rospy.get_param("~LED_scale")
# Initialize LEDs to be off
self.pattern = [[0, 0, 0]] * 5
self.frequency_mask = [0] * 5
self.current_pattern_name = 'LIGHT_OFF'
self.changePattern(self.current_pattern_name)
# Initialize the timer
self.frequency = 1.0 / self.parameters['~LED_protocol']['signals'][
'CAR_SIGNAL_A']['frequency']
self.is_on = False
self.cycle_timer = rospy.Timer(
rospy.Duration.from_sec(self.frequency / 2.0), self.cycleTimer_)
# Publishers
self.pub_state = self.publisher("~current_led_state",
String,
queue_size=1)
# Services
self.srv_set_LED_ = rospy.Service("~set_custom_pattern",
SetCustomLEDPattern,
self.srvSetCustomLEDPattern)
self.srv_set_pattern_ = rospy.Service("~set_pattern", ChangePattern,
self.srvSetPattern)
# Scale intensity of the LEDs
for name, c in self.parameters['~LED_protocol']['colors'].items():
for i in range(3):
c[i] = c[i] * self.parameters['~LED_scale']
# Remap colors if robot does not have an RGB ordering
if self.parameters['~channel_order'][self.robot_type] is not "RGB":
protocol = self.parameters['~LED_protocol']
for name, col in self.parameters['~LED_protocol']['colors'].items(
):
protocol['colors'][name] = self.remapColors(col)
rospy.set_param("~LED_protocol", protocol)
self.updateParameters()
self.log("Colors remapped to " +
str(self.parameters['~channel_order'][self.robot_type]))
# Turn on the LEDs
self.changePattern('WHITE')
self.log("Initialized.")
class TrafficLight(object):
def __init__(self):
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.cycle = None
self.protocol =self.setupParameter("~LED_protocol",[]) #should be a list of tuples
self.greenlight_freq = self.protocol['signals']['traffic_light_go']['frequency']
self.redlight_freq = self.protocol['signals']['traffic_light_stop']['frequency']
self.traffic_light_list = self.setupParameter("~traffic_light_list",[0,2,3,1]); #order of lights
self.greenlight_duration = self.setupParameter("~greenlight_duration",5) #in seconds
self.allred_duration = self.setupParameter("~allred_duration",4) #in seconds
self.redlight_t = 1.0/self.redlight_freq
self.greenlight_t = 1.0/self.greenlight_freq
self.green_on=False
self.green_i = 0;
self.green = self.traffic_light_list[self.green_i]
self.green_color = [0,1,0] #Hardcoded but should be parameter
self.yellow_color = [1,0.65,0] #Hardcoded but should be parameter
self.redlightlist = self.traffic_light_list[:self.green_i] + self.traffic_light_list[(self.green_i+1):];
self.traffic_light_state = {0:False,1:False,2:False,3:False} #All LEDs are off
self.yellowlightlist = []
self.traffic_cycle = rospy.Timer(rospy.Duration((self.greenlight_duration+self.allred_duration)),self.switchGreen)
self.redLED_cycle = rospy.Timer(rospy.Duration(0.5*self.redlight_t),self.freqred)
self.greenLED_cycle = rospy.Timer(rospy.Duration(0.5*self.greenlight_t),self.freqgreen)
def switchGreen(self,event):
self.yellowlightlist=[]
self.green_i = (self.green_i+1)%4 #Move to next light in list
self.green = self.traffic_light_list[self.green_i]
self.green_color = [0,1,0]
self.green_on=True
self.redlightlist = self.traffic_light_list[:self.green_i] + self.traffic_light_list[(self.green_i+1):];
rospy.sleep(self.greenlight_duration) #Keep the green light on
self.green_on = False #Turn off the green light
self.yellowlightlist = [self.green]
rospy.sleep(self.allred_duration)
self.redlightlist = self.traffic_light_list[0:]
def freqred(self,event):
for light in self.redlightlist:
if self.traffic_light_state[light]==True:
self.led.setRGB(light,[0,0,0])
self.traffic_light_state[light]=False
else:
self.led.setRGB(light,[1,0,0])
self.traffic_light_state[light]=True
for light in self.yellowlightlist:
if self.traffic_light_state[light]==True:
self.led.setRGB(light,[0,0,0])
self.traffic_light_state[light]=False
else:
self.led.setRGB(light,[1,1,0])
self.traffic_light_state[light]=True
def freqgreen(self,event):
if self.green_on==False: #Exit if lights should all be red
return
if self.traffic_light_state[self.green]==True:
self.led.setRGB(self.green,[0,0,0])
self.traffic_light_state[self.green]=False
else:
self.led.setRGB(self.green,self.green_color)
self.traffic_light_state[self.green]=True
def setupParameter(self,param_name,default_value):
value = rospy.get_param(param_name,default_value)
rospy.set_param(param_name,value) #Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value))
return value
class LEDEmitter(object):
def __init__(self):
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.pub_state = rospy.Publisher("~current_led_state",
Float32,
queue_size=1)
self.sub_pattern = rospy.Subscriber("~change_color_pattern", String,
self.changePattern)
self.sub_switch = rospy.Subscriber("~switch", BoolStamped,
self.cbSwitch)
self.cycle = None
self.is_on = False
self.active = True
self.protocol = rospy.get_param(
"~LED_protocol") #should be a list of tuples
self.pattern_off = [[0, 0, 0]] * 5
scale = 0.5
for _, c in self.protocol['colors'].items():
for i in range(3):
c[i] = c[i] * scale
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(.1),
self.cycleTimer)
self.current_pattern_name = None
self.changePattern_('CAR_SIGNAL_A')
def cbSwitch(self, switch_msg): # active/inactive switch from FSM
self.active = switch_msg.data
def cycleTimer(self, event):
if not self.active:
return
if self.is_on:
for i in range(5):
self.led.setRGB(i, [0, 0, 0])
self.is_on = False
else:
for i in range(5):
self.led.setRGB(i, self.pattern[i])
self.is_on = True
def changePattern(self, msg):
self.changePattern_(msg.data)
def changePattern_(self, pattern_name):
if pattern_name:
if (self.current_pattern_name == pattern_name):
return
else:
self.current_pattern_name = pattern_name
rospy.loginfo('changePattern(%r)' % pattern_name)
color = self.protocol['signals'][pattern_name]['color']
self.cycle = self.protocol['signals'][pattern_name]['frequency']
print("color: %s, freq (Hz): %s " % (color, self.cycle))
self.pattern = [[0, 0, 0]] * 5
self.pattern[2] = self.protocol['colors'][color]
#print(self.pattern)
if pattern_name in ['traffic_light_go', 'traffic_light_stop']:
self.pattern = [self.protocol['colors'][color]] * 5
self.changeFrequency()
def changeFrequency(self):
try:
#self.cycle = msg.data
self.cycle_timer.shutdown()
#below, convert to hz
d = 1.0 / (2.0 * self.cycle)
self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(d),
self.cycleTimer)
except ValueError as e:
self.cycle = None
self.current_pattern_name = None
self.pub_state.publish(float(self.cycle))
