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