Exemple #1
0
 def __init__(self):
     """
         Class to control all leds
         Available leds are:
         * 'left_outer_light',
         * 'left_inner_light',
         * 'right_outer_light',
         * 'right_inner_light',
         * 'torso_left_outer_light',
         * 'torso_left_inner_light',
         * 'torso_right_outer_light',
         * 'torso_right_inner_light'
     """
     self.post = Post(self)
     self.led_names = [
         'left_outer_light', 'left_inner_light', 'right_outer_light',
         'right_inner_light', 'torso_left_outer_light',
         'torso_left_inner_light', 'torso_right_outer_light',
         'torso_right_inner_light'
     ]
     self.led_handle = {}
     self.led_state = {}
     try:
         for led in self.led_names:
             self.led_handle[led] = baxter_interface.DigitalIO(led)
             self.led_state[led] = 0
     except:
         rospy.logwarn("Leds not initialized.")
Exemple #2
0
class TabArms():
    def __init__(self,gui):
        self.ui = gui.ui
        self.post = Post(self)
        self.arm = {"right": SimpleLimb('right'),
                    "left":SimpleLimb('left')
                    }
        self.ui.btn_arms_test.clicked.connect(self.__armTest)
        
    def __armTest(self):
        self.post.armTest()
        
    def armTest(self):
        joint_moves = (
                [ 0.0, -0.55, 0.0, 0.75, 0.0, 1.26,  0.0],
                [ 0.5,  -0.8, 2.8, 0.15, 0.0,  1.9,  2.8],
                [-0.1,  -0.8,-1.0, 2.5,  0.0, -1.4, -2.8],
                [ 0.0, -0.55, 0.0, 0.75, 0.0, 1.26,  0.0],
                )
        
        
        for move in joint_moves:# todo try to use the basic interface
            side = "left"
            th_left = self.arm[side].post.move_to_joint_positions(dict(zip(self.arm[side].joint_names(),move)))
            side = "right"    
            self.arm[side].move_to_joint_positions(dict(zip(self.arm[side].joint_names(),move)))
            th_left.join()
class TabArms():
    def __init__(self, gui):
        self.ui = gui.ui
        self.post = Post(self)
        self.arm = {"right": SimpleLimb('right'), "left": SimpleLimb('left')}
        self.ui.btn_arms_test.clicked.connect(self.__armTest)

    def __armTest(self):
        self.post.armTest()

    def armTest(self):
        joint_moves = (
            [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0],
            [0.5, -0.8, 2.8, 0.15, 0.0, 1.9, 2.8],
            [-0.1, -0.8, -1.0, 2.5, 0.0, -1.4, -2.8],
            [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0],
        )

        for move in joint_moves:  # todo try to use the basic interface
            side = "left"
            th_left = self.arm[side].post.move_to_joint_positions(
                dict(zip(self.arm[side].joint_names(), move)))
            side = "right"
            self.arm[side].move_to_joint_positions(
                dict(zip(self.arm[side].joint_names(), move)))
            th_left.join()
Exemple #4
0
class TabCamera():
    def __init__(self,gui,datapath):
        self.post = Post(self)
        self.gui = gui
        self.ui = gui.ui
        self.lbl = None
        self.initCameraImage()
        self.cam = CameraWrapper(datapath)
        self.cam.updateCalled.connect(self.updateImage)
        self.ui.cb_left_camera.clicked.connect(self.updateCamera)
        self.ui.cb_right_camera.clicked.connect(self.updateCamera)
        self.ui.cb_head_camera.clicked.connect(self.updateCamera)
        self.ui.btn_save_image.clicked.connect(self.saveImage)
        self.ui.btn_test_all_cameras.clicked.connect(self.testAllCameras)

    def saveImage(self):
        self.cam.save_image = True
        
    def testCam(self,name):
        self.cam.open(name)
        rospy.sleep(2)
        self.cam.save_image = True
        while self.cam.save_image:
            rospy.sleep(0.1)
            
    def testAllCameras(self):
        self.post._testAllCameras()

    def _testAllCameras(self):
        for name in ["left_hand_camera","right_hand_camera","head_camera"]:
            self.testCam(name)
        

    def updateCamera(self):
        if self.ui.cb_left_camera.isChecked():
            print "opening left cam"
            self.cam.open("left_hand_camera")
        elif self.ui.cb_right_camera.isChecked():
            print "opening right cam"
            self.cam.open("right_hand_camera")
        elif self.ui.cb_head_camera.isChecked():
            print "opening head cam"
            self.cam.open("head_camera")
            
    def initCameraImage(self):      
        hbox = QtGui.QHBoxLayout(self.ui.camera_image)
        self.lbl = QtGui.QLabel(self.gui)
        hbox.addWidget(self.lbl)
        self.gui.setLayout(hbox)
        
    @Slot(QtGui.QImage)
    def updateImage(self,image):
        if not self.lbl is None: 
            pixmap = QtGui.QPixmap.fromImage(image)
            self.lbl.setPixmap(pixmap)
class TabCamera():
    def __init__(self, gui, datapath):
        self.post = Post(self)
        self.gui = gui
        self.ui = gui.ui
        self.lbl = None
        self.initCameraImage()
        self.cam = CameraWrapper(datapath)
        self.cam.updateCalled.connect(self.updateImage)
        self.ui.cb_left_camera.clicked.connect(self.updateCamera)
        self.ui.cb_right_camera.clicked.connect(self.updateCamera)
        self.ui.cb_head_camera.clicked.connect(self.updateCamera)
        self.ui.btn_save_image.clicked.connect(self.saveImage)
        self.ui.btn_test_all_cameras.clicked.connect(self.testAllCameras)

    def saveImage(self):
        self.cam.save_image = True

    def testCam(self, name):
        self.cam.open(name)
        rospy.sleep(2)
        self.cam.save_image = True
        while self.cam.save_image:
            rospy.sleep(0.1)

    def testAllCameras(self):
        self.post._testAllCameras()

    def _testAllCameras(self):
        for name in ["left_hand_camera", "right_hand_camera", "head_camera"]:
            self.testCam(name)

    def updateCamera(self):
        if self.ui.cb_left_camera.isChecked():
            print "opening left cam"
            self.cam.open("left_hand_camera")
        elif self.ui.cb_right_camera.isChecked():
            print "opening right cam"
            self.cam.open("right_hand_camera")
        elif self.ui.cb_head_camera.isChecked():
            print "opening head cam"
            self.cam.open("head_camera")

    def initCameraImage(self):
        hbox = QtGui.QHBoxLayout(self.ui.camera_image)
        self.lbl = QtGui.QLabel(self.gui)
        hbox.addWidget(self.lbl)
        self.gui.setLayout(hbox)

    @Slot(QtGui.QImage)
    def updateImage(self, image):
        if not self.lbl is None:
            pixmap = QtGui.QPixmap.fromImage(image)
            self.lbl.setPixmap(pixmap)
 def __init__(self, gui, datapath):
     self.post = Post(self)
     self.gui = gui
     self.ui = gui.ui
     self.lbl = None
     self.initCameraImage()
     self.cam = CameraWrapper(datapath)
     self.cam.updateCalled.connect(self.updateImage)
     self.ui.cb_left_camera.clicked.connect(self.updateCamera)
     self.ui.cb_right_camera.clicked.connect(self.updateCamera)
     self.ui.cb_head_camera.clicked.connect(self.updateCamera)
     self.ui.btn_save_image.clicked.connect(self.saveImage)
     self.ui.btn_test_all_cameras.clicked.connect(self.testAllCameras)
 def __init__(self, gui):
     self.ui = gui.ui
     self.post = Post(self)
     self._display_pub = rospy.Publisher('/robot/xdisplay',
                                         Image,
                                         queue_size=1)
     self.ui.cb_display_white.clicked.connect(
         lambda: self.setImage("white"))
     self.ui.cb_display_black.clicked.connect(
         lambda: self.setImage("black"))
     self.ui.cb_display_blue.clicked.connect(lambda: self.setImage("blue"))
     self.ui.cb_display_green.clicked.connect(
         lambda: self.setImage("green"))
     self.ui.cb_display_red.clicked.connect(lambda: self.setImage("red"))
     self.ui.btn_display_show_all.clicked.connect(self.__showAll)
Exemple #8
0
 def __init__(self,gui):
     self.ui = gui.ui
     self.post = Post(self)
     self.arm = {"right": SimpleLimb('right'),
                 "left":SimpleLimb('left')
                 }
     self.ui.btn_arms_test.clicked.connect(self.__armTest)
class TabHead():
    def __init__(self, gui):
        self.ui = gui.ui
        self.post = Post(self)
        self.head = baxter_interface.Head()
        self.ui.btn_head_test.clicked.connect(self.__headTest)

    def __headTest(self):
        self.post.headTest()

    def headTest(self):
        self.head.set_pan(0.0, 1)
        self.head.set_pan(math.pi / 2, 1)
        self.head.set_pan(-math.pi / 2, 1)
        self.head.set_pan(0.0, 1)
        for i in range(3):
            self.head.command_nod()
    def __init__(self, gui):
        QObject.__init__(self)
        self.sonar = Sonar()
        self.gui = gui
        self.ui = gui.ui
        self.post = Post(self)
        self.ui.btn_sonar_enable.clicked.connect(self.sonar.enable)
        self.ui.btn_sonar_disable.clicked.connect(self.sonar.disable)
        self.updateDistances.connect(self.updateGui)
        self.ui.btn_sonar_reset.clicked.connect(self.reset)

        for i in range(12):
            newItem = QtGui.QTableWidgetItem("Sensor " + str(i))
            self.ui.tbl_sonar.setItem(i, 0, newItem)
        self.reset()

        self.post.callback()
Exemple #11
0
class TabHead():
    def __init__(self,gui):
        self.ui = gui.ui
        self.post = Post(self)
        self.head = baxter_interface.Head()
        self.ui.btn_head_test.clicked.connect(self.__headTest)
    
    def __headTest(self):
        self.post.headTest()
        
    def headTest(self):
        self.head.set_pan(0.0, 1)
        self.head.set_pan(math.pi/2, 1)
        self.head.set_pan(-math.pi/2, 1)
        self.head.set_pan(0.0, 1)
        for i in range(3):
            self.head.command_nod()
Exemple #12
0
 def __init__(self,gui):
     self.ui = gui.ui
     self.post = Post(self)
     self._display_pub= rospy.Publisher('/robot/xdisplay',Image, queue_size=1)
     self.ui.cb_display_white.clicked.connect(lambda: self.setImage("white"))
     self.ui.cb_display_black.clicked.connect(lambda: self.setImage("black"))
     self.ui.cb_display_blue.clicked.connect(lambda: self.setImage("blue"))
     self.ui.cb_display_green.clicked.connect(lambda: self.setImage("green"))
     self.ui.cb_display_red.clicked.connect(lambda: self.setImage("red"))
     self.ui.btn_display_show_all.clicked.connect(self.__showAll)
Exemple #13
0
 def __init__(self):
     self.pub = {}
     self.pub["red"] = rospy.Publisher(
         "/robot/sonar/head_sonar/lights/set_red_level",
         Float32,
         queue_size=1)
     self.pub["green"] = rospy.Publisher(
         "/robot/sonar/head_sonar/lights/set_green_level",
         Float32,
         queue_size=1)
     self.post = Post(self)
class TabDisplay():
    def __init__(self, gui):
        self.ui = gui.ui
        self.post = Post(self)
        self._display_pub = rospy.Publisher('/robot/xdisplay',
                                            Image,
                                            queue_size=1)
        self.ui.cb_display_white.clicked.connect(
            lambda: self.setImage("white"))
        self.ui.cb_display_black.clicked.connect(
            lambda: self.setImage("black"))
        self.ui.cb_display_blue.clicked.connect(lambda: self.setImage("blue"))
        self.ui.cb_display_green.clicked.connect(
            lambda: self.setImage("green"))
        self.ui.cb_display_red.clicked.connect(lambda: self.setImage("red"))
        self.ui.btn_display_show_all.clicked.connect(self.__showAll)

    def __showAll(self):
        self.post.showAll()

    def showAll(self):
        for color in ["white", "black", "blue", "green", "red"]:
            print "setting color", color
            self.setImage(color)
            rospy.sleep(10)
        print "done"

    def setImage(self, color):
        blank_image = np.zeros((600, 1024, 3), np.uint8)
        if color == "black":
            pass
        elif color == "white":
            blank_image[:, :] = (255, 255, 255)
        elif color == "blue":
            blank_image[:, :] = (255, 0, 0)
        elif color == "green":
            blank_image[:, :] = (0, 255, 0)
        elif color == "red":
            blank_image[:, :] = (0, 0, 255)
        data = cv_bridge.CvBridge().cv2_to_imgmsg(blank_image, encoding="bgr8")
        self._display_pub.publish(data)
Exemple #15
0
 def __init__(self,gui,datapath):
     self.post = Post(self)
     self.gui = gui
     self.ui = gui.ui
     self.lbl = None
     self.initCameraImage()
     self.cam = CameraWrapper(datapath)
     self.cam.updateCalled.connect(self.updateImage)
     self.ui.cb_left_camera.clicked.connect(self.updateCamera)
     self.ui.cb_right_camera.clicked.connect(self.updateCamera)
     self.ui.cb_head_camera.clicked.connect(self.updateCamera)
     self.ui.btn_save_image.clicked.connect(self.saveImage)
     self.ui.btn_test_all_cameras.clicked.connect(self.testAllCameras)
class TabSonar(QObject):
    updateDistances = Signal(list)

    def __init__(self, gui):
        QObject.__init__(self)
        self.sonar = Sonar()
        self.gui = gui
        self.ui = gui.ui
        self.post = Post(self)
        self.ui.btn_sonar_enable.clicked.connect(self.sonar.enable)
        self.ui.btn_sonar_disable.clicked.connect(self.sonar.disable)
        self.updateDistances.connect(self.updateGui)
        self.ui.btn_sonar_reset.clicked.connect(self.reset)

        for i in range(12):
            newItem = QtGui.QTableWidgetItem("Sensor " + str(i))
            self.ui.tbl_sonar.setItem(i, 0, newItem)
        self.reset()

        self.post.callback()

    def callback(self):

        while not rospy.is_shutdown():
            self.updateDistances.emit(self.sonar.getRanges())
            rospy.sleep(0.1)

    def reset(self):
        for i in range(12):
            newItem = QtGui.QTableWidgetItem("None")
            self.ui.tbl_sonar.setItem(i, 1, newItem)

    def updateGui(self, distances):
        for i, distance in enumerate(distances):
            if distance is None:
                continue
            newItem = QtGui.QTableWidgetItem(str(distance))
            self.ui.tbl_sonar.setItem(i, 1, newItem)
Exemple #17
0
class TabSonar(QObject):
    updateDistances = Signal(list)
    def __init__(self,gui):
        QObject.__init__(self)
        self.sonar = Sonar()
        self.gui = gui
        self.ui = gui.ui
        self.post = Post(self)
        self.ui.btn_sonar_enable.clicked.connect(self.sonar.enable)
        self.ui.btn_sonar_disable.clicked.connect(self.sonar.disable)
        self.updateDistances.connect(self.updateGui)
        self.ui.btn_sonar_reset.clicked.connect(self.reset)
        
        for i in range(12):
            newItem = QtGui.QTableWidgetItem("Sensor "+str(i))
            self.ui.tbl_sonar.setItem(i, 0, newItem)
        self.reset()
        
        self.post.callback()
            
    def callback(self):

        while not rospy.is_shutdown():
            self.updateDistances.emit(self.sonar.getRanges())
            rospy.sleep(0.1)
     
    def reset(self):
        for i in range(12):
            newItem = QtGui.QTableWidgetItem("None")
            self.ui.tbl_sonar.setItem(i, 1, newItem)
        
    def updateGui(self,distances):
        for i,distance in enumerate(distances):
            if distance is None:
                continue
            newItem = QtGui.QTableWidgetItem(str(distance))
            self.ui.tbl_sonar.setItem(i, 1, newItem)
Exemple #18
0
class TabDisplay():
    def __init__(self,gui):
        self.ui = gui.ui
        self.post = Post(self)
        self._display_pub= rospy.Publisher('/robot/xdisplay',Image, queue_size=1)
        self.ui.cb_display_white.clicked.connect(lambda: self.setImage("white"))
        self.ui.cb_display_black.clicked.connect(lambda: self.setImage("black"))
        self.ui.cb_display_blue.clicked.connect(lambda: self.setImage("blue"))
        self.ui.cb_display_green.clicked.connect(lambda: self.setImage("green"))
        self.ui.cb_display_red.clicked.connect(lambda: self.setImage("red"))
        self.ui.btn_display_show_all.clicked.connect(self.__showAll)
        
    def __showAll(self):
        self.post.showAll()
        
    def showAll(self):
        for color in ["white","black","blue","green","red"]:
            print "setting color",color
            self.setImage(color)
            rospy.sleep(10)
        print "done"
                    
    def setImage(self,color):
        blank_image = np.zeros((600,1024,3), np.uint8)
        if color == "black":
            pass
        elif color == "white":
            blank_image[:,:] = (255,255,255)
        elif color == "blue":
            blank_image[:,:] = (255,0,0)
        elif color == "green":
            blank_image[:,:] = (0,255,0)
        elif color == "red":
            blank_image[:,:] = (0,0,255)
        data = cv_bridge.CvBridge().cv2_to_imgmsg(blank_image,encoding="bgr8")
        self._display_pub.publish(data)
Exemple #19
0
 def __init__(self,gui):
     QObject.__init__(self)
     self.sonar = Sonar()
     self.gui = gui
     self.ui = gui.ui
     self.post = Post(self)
     self.ui.btn_sonar_enable.clicked.connect(self.sonar.enable)
     self.ui.btn_sonar_disable.clicked.connect(self.sonar.disable)
     self.updateDistances.connect(self.updateGui)
     self.ui.btn_sonar_reset.clicked.connect(self.reset)
     
     for i in range(12):
         newItem = QtGui.QTableWidgetItem("Sensor "+str(i))
         self.ui.tbl_sonar.setItem(i, 0, newItem)
     self.reset()
     
     self.post.callback()
 def __init__(self, gui):
     self.ui = gui.ui
     self.post = Post(self)
     self.head = baxter_interface.Head()
     self.ui.btn_head_test.clicked.connect(self.__headTest)
Exemple #21
0
class Led():
    def __init__(self):
        """
            Class to control all leds
            Available leds are:
            * 'left_outer_light',
            * 'left_inner_light',
            * 'right_outer_light',
            * 'right_inner_light',
            * 'torso_left_outer_light',
            * 'torso_left_inner_light',
            * 'torso_right_outer_light',
            * 'torso_right_inner_light'
        """
        self.post = Post(self)
        self.led_names = [
            'left_outer_light', 'left_inner_light', 'right_outer_light',
            'right_inner_light', 'torso_left_outer_light',
            'torso_left_inner_light', 'torso_right_outer_light',
            'torso_right_inner_light'
        ]
        self.led_handle = {}
        self.led_state = {}
        try:
            for led in self.led_names:
                self.led_handle[led] = baxter_interface.DigitalIO(led)
                self.led_state[led] = 0
        except:
            rospy.logwarn("Leds not initialized.")

    def enable(self, name):
        """
            Enables a Led
            
            :param name: Name of the led 
            :type name: str
        """
        self.led_handle[name].set_output(True)
        self.led_state[name] = 1

    def disable(self, name):
        """
            Disables a Led
            
            :param name: Name of the led
            :type name: str
        """
        self.led_handle[name].set_output(False)
        self.led_state[name] = 0
        pass

    def disableAll(self):
        """
            Disables all leds
        """
        for led in self.led_names:
            self.disable(led)

    def blink(self,
              name,
              timeout=0.0,
              frequency=2.0):  #timeout <= 0 blinks endlessly
        """
            Blinks a led for a specific time and frequency (blocking)
            
            :param name: Name of the led
            :type name: str
            :param timeout: Duration to let the led blinking. A value <= 0.0 means infinite time
            :type timeout: float
            :param frequency: Rate, how often a led blinks in one second
            :type frequency: float   
        """
        end = rospy.Time.now() + rospy.Duration(timeout)
        self.led_state[name] = 1
        try:
            while not rospy.is_shutdown():
                start = rospy.Time.now()
                if (start > end and timeout > 0) or self.led_state[name] == 0:
                    self.led_handle[name].set_output(False)
                    break
                self.led_handle[name].set_output(True)
                rospy.Rate(frequency * 2).sleep()
                self.led_handle[name].set_output(False)
                rospy.Rate(frequency * 2).sleep()
        except:
            pass

    def blinkAllOuter(self, timeout=0, frequency=2):
        """
            Blinks with all blue leds for a specific time and frequency (blocking)
            
            :param timeout: Duration to let the leds blinking. A value <= 0.0 means infinite time
            :type timeout: float
            :param frequency: Rate, how often all leds blink in one second
            :type frequency: float
        """
        for led in xrange(0, len(self.led_names), 2):
            self.post.blink(self.led_names[led], timeout, frequency)

    def blinkAllInner(self, timeout=0, frequency=2):
        """
            Blinks with all white leds for a specific time and frequency (blocking)
            
            :param timeout: Duration to let the leds blinking. A value <= 0.0 means infinite time
            :type timeout: float
            :param frequency: Rate, how often all leds blink in one second  
            :type frequency: float 
        """
        for led in xrange(1, len(self.led_names), 2):
            self.post.blink(self.led_names[led], timeout, frequency)
Exemple #22
0
 def __init__(self,gui):
     self.ui = gui.ui
     self.post = Post(self)
     self.head = baxter_interface.Head()
     self.ui.btn_head_test.clicked.connect(self.__headTest)
 def __init__(self, gui):
     self.ui = gui.ui
     self.post = Post(self)
     self.arm = {"right": SimpleLimb('right'), "left": SimpleLimb('left')}
     self.ui.btn_arms_test.clicked.connect(self.__armTest)