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.")
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()
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)
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()
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):
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 __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)
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)
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)
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)
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)
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)
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)