#!/usr/bin/env python
import rospy
import Adafruit_BBIO.GPIO as GPIO
from vortex_msgs.msg import CameraFeedSelection

GPIO_PIN_MAP = rospy.get_param('/camera/pin_map')

class CameraSelection(object):
    def __init__(self):
        rospy.init_node('camera_selection')
        self.cam_select_sub = rospy.Subscriber('camera_feed_selection',CameraFeedSelection, self.callback)

        #Set pin as output
        for selection_pin in GPIO_PIN_MAP:
            GPIO.setup(GPIO_PIN_MAP[selection_pin], GPIO.OUT)

        #Set to ground
        for selection_pin in GPIO_PIN_MAP:
        	GPIO.output(GPIO_PIN_MAP[selection_pin], GPIO.LOW)

    def callback(self, msg):
        pass


if __name__ == '__main__':
    try:
        camera_selection = CameraSelection()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass


@@ -111,28 +111,86 @@
 ection%0A%0A
-G
 PI
-O_
+N_MAP_FEED0 = rospy.get_param('/camera/pin_map_feed0')%0A
 PIN_MAP
+_FEED1
  = rospy
@@ -216,18 +216,140 @@
 /pin_map
-')
+_feed1')%0APIN_MAP_FEED2 = rospy.get_param('/camera/pin_map_feed2')%0APIN_MAP_LIST = %5BPIN_MAP_FEED0,PIN_MAP_FEED1,PIN_MAP_FEED2%5D
 %0A%0Aclass 
@@ -592,37 +592,32 @@
 for 
-selection_pin in GPIO_
+pin_list in 
 PIN_MAP
+_LIST
 :%0A  
@@ -630,46 +630,59 @@
     
-GPIO.setup(GPIO_PIN_MAP%5Bselection_
+for pin in pin_list:%0A                GPIO.setup(
 pin
-%5D
 , GP
@@ -694,82 +694,167 @@
 T)%0A%0A
+%0A
     
-    #Set to ground%0A        for selection_pin in GPIO_PIN_MAP:%0A
+def callback(self, msg):%0A        feed_pin_map = PIN_MAP_LIST%5Bmsg.feed%5D%0A        for i, level in enumerate(msg.pin_values):%0A            if level:%0A  
         
 %09GPI
@@ -853,9 +853,14 @@
     
-%09
+      
 GPIO
@@ -871,42 +871,30 @@
 put(
-GPIO_PIN_MAP%5Bselection_pin
+feed_pin_map%5Bi
 %5D, GPIO.
 LOW)
@@ -893,55 +893,86 @@
 PIO.
-LOW
+HIGH
 )%0A
-%0A
     
-def callback(self, msg):%0A        pass
+        else%0A                GPIO.output(feed_pin_map%5Bi%5D, GPIO.LOW)%0A
 %0A%0A%0Ai

Add callback, set camera feed selection pins based on msg