def __init__( self ): scriptPath = os.path.dirname( __file__ ) self.cameraImagePixBuf = None self.servoConfigDict = getArmServoConfig() self.wristAngle = 0.0 # Connect to the robot via ROS # TODO: Move arm to all dictionary servoConfigList = [ self.servoConfigDict[ servoName ] for servoName in self.servoConfigDict ] self.roboticArm = RoboticArm( gaffa_teleop.LynxmotionArmDescription.ARM_DH_PROXIMAL, servoConfigList ) self.cameraImageTopic = rospy.Subscriber( "/camera/image", sensor_msgs.msg.Image, self.cameraImageCallback ) self.jointScreenHomography = JointScreenHomography( self.CORNER_JOINT_ARRAYS, self.CORNER_SCREEN_POSITIONS, self.roboticArm ) # Setup the GUI builder = gtk.Builder() builder.add_from_file( scriptPath + "/GUI/VideoTeleop.glade" ) self.window = builder.get_object( "winMain" ) self.dwgCameraImage = builder.get_object( "dwgCameraImage" ) builder.connect_signals( self ) updateLoop = self.update() gobject.idle_add( updateLoop.next ) self.window.show()
def __init__(self): scriptPath = os.path.dirname(__file__) self.cameraImagePixBuf = None self.servoConfigDict = getArmServoConfig() self.wristAngle = 0.0 # Connect to the robot via ROS # TODO: Move arm to all dictionary servoConfigList = [ self.servoConfigDict[servoName] for servoName in self.servoConfigDict ] self.roboticArm = RoboticArm( gaffa_teleop.LynxmotionArmDescription.ARM_DH_PROXIMAL, servoConfigList) self.cameraImageTopic = rospy.Subscriber("/camera/image", sensor_msgs.msg.Image, self.cameraImageCallback) self.jointScreenHomography = JointScreenHomography( self.CORNER_JOINT_ARRAYS, self.CORNER_SCREEN_POSITIONS, self.roboticArm) # Setup the GUI builder = gtk.Builder() builder.add_from_file(scriptPath + "/GUI/VideoTeleop.glade") self.window = builder.get_object("winMain") self.dwgCameraImage = builder.get_object("dwgCameraImage") builder.connect_signals(self) updateLoop = self.update() gobject.idle_add(updateLoop.next) self.window.show()
class MainWindow: OPTICAL_FLOW_BLOCK_WIDTH = 8 OPTICAL_FLOW_BLOCK_HEIGHT = 8 OPTICAL_FLOW_RANGE_WIDTH = 8 # Range to look outside of a block for motion OPTICAL_FLOW_RANGE_HEIGHT = 8 GRIPPER_WAVE_FREQUENCY = 1.0 # Waves per second GRIPPER_NUM_WAVES = 3.0 GRIPPER_WAVE_AMPLITUDE = math.radians(20.0) / 4.0 BUFFER_TIME_LENGTH = 10.0 SAMPLES_PER_SECOND = 15.0 MAX_CORRELATION_LAG = 1.0 GRIPPER_DETECTION_TIME = 9.0 # Should be less than BUFFER_TIME_LENGTH GRIPPER_DETECTION_WAVE_START_TIME = 3.0 # 49, 81 -> 52.05, 15.62, -34.16, 81.33 - Top Left # 218, 76 -> 28.19, 58.89, -105.32, 48.80 - Top Right # 283, 135 -> 79.16, 67.70, -119.55, 48.80 - Bottom Right # 56, 142 -> 78.61, 29.24, -50.29, 65.60 - Bottom Left CORNER_JOINT_ARRAYS = [ [ math.radians(52.05), # Top Left math.radians(15.62), math.radians(-34.16), math.radians(81.33), math.radians(0.0) ], [ math.radians(28.19), # Top Right math.radians(58.89), math.radians(-105.32), math.radians(48.80), math.radians(0.0) ], [ math.radians(79.16), # Bottom Right math.radians(67.70), math.radians(-119.55), math.radians(48.80), math.radians(0.0) ], [ math.radians(78.61), # Bottom Left math.radians(29.24), math.radians(-50.29), math.radians(65.60), math.radians(0.0) ] ] CORNER_SCREEN_POSITIONS = [ (49, 81), # Top Left (218, 76), # Top Right (283, 135), # Bottom Right (56, 142) ] # Bottom Left #--------------------------------------------------------------------------- def __init__(self): scriptPath = os.path.dirname(__file__) self.cameraImagePixBuf = None self.servoConfigDict = getArmServoConfig() self.wristAngle = 0.0 # Connect to the robot via ROS # TODO: Move arm to all dictionary servoConfigList = [ self.servoConfigDict[servoName] for servoName in self.servoConfigDict ] self.roboticArm = RoboticArm( gaffa_teleop.LynxmotionArmDescription.ARM_DH_PROXIMAL, servoConfigList) self.cameraImageTopic = rospy.Subscriber("/camera/image", sensor_msgs.msg.Image, self.cameraImageCallback) self.jointScreenHomography = JointScreenHomography( self.CORNER_JOINT_ARRAYS, self.CORNER_SCREEN_POSITIONS, self.roboticArm) # Setup the GUI builder = gtk.Builder() builder.add_from_file(scriptPath + "/GUI/VideoTeleop.glade") self.window = builder.get_object("winMain") self.dwgCameraImage = builder.get_object("dwgCameraImage") builder.connect_signals(self) updateLoop = self.update() gobject.idle_add(updateLoop.next) self.window.show() #--------------------------------------------------------------------------- def onWinMainDestroy(self, widget, data=None): gtk.main_quit() #--------------------------------------------------------------------------- def main(self): # All PyGTK applications must have a gtk.main(). Control ends here # and waits for an event to occur (like a key press or mouse event). gtk.main() #--------------------------------------------------------------------------- def cameraImageCallback(self, rosImage): if rosImage.encoding == "rgb8" or rosImage.encoding == "bgr8": # Display the image self.cameraImagePixBuf = gtk.gdk.pixbuf_new_from_data( rosImage.data, gtk.gdk.COLORSPACE_RGB, False, 8, rosImage.width, rosImage.height, rosImage.step) # Resize the drawing area if necessary if self.dwgCameraImage.get_size_request() != (rosImage.width, rosImage.height): self.dwgCameraImage.set_size_request(rosImage.width, rosImage.height) self.dwgCameraImage.queue_draw() else: rospy.logerr("Unhandled image encoding - " + rosImage.encoding) #--------------------------------------------------------------------------- def moveArmToJointAnglePosition(self, servoAnglesDict, jointSpeed): self.wristAngle = servoAnglesDict["wrist_rotate"] self.roboticArm.setJointAngles(servoAnglesDict, jointSpeed) #--------------------------------------------------------------------------- def onBtnGotoSafePosClicked(self, widget, data=None): servoAnglesDict = { "base_rotate": math.radians(72.65), "shoulder_rotate": 2.3561944901923448, "elbow_rotate": -2.748893571891069, "wrist_rotate": 1.9583014768641922, "gripper_rotate": -0.0004890796739715704 } self.moveArmToJointAnglePosition(servoAnglesDict, math.radians(1.5)) #--------------------------------------------------------------------------- def onDwgCameraImageButtonPressEvent(self, widget, data): if self.cameraImagePixBuf != None: imgRect = self.getImageRectangleInWidget( widget, self.cameraImagePixBuf.get_width(), self.cameraImagePixBuf.get_height()) screenX = data.x - imgRect.x screenY = data.y - imgRect.y jointArray = self.jointScreenHomography.convertScreenPosToJointPos( (screenX, screenY)) jointArrayDegrees = [math.degrees(angle) for angle in jointArray] print "press at", screenX, screenY, "->", jointArrayDegrees print self.roboticArm.GetEndEffectorPos(jointArray) servoAnglesDict = { "base_rotate": jointArray[0], "shoulder_rotate": jointArray[1], "elbow_rotate": jointArray[2], "wrist_rotate": jointArray[3], "gripper_rotate": jointArray[4] } self.moveArmToJointAnglePosition(servoAnglesDict, math.radians(0.5)) #--------------------------------------------------------------------------- def onDwgCameraImageExposeEvent(self, widget, data=None): if self.cameraImagePixBuf != None: imgRect = self.getImageRectangleInWidget( widget, self.cameraImagePixBuf.get_width(), self.cameraImagePixBuf.get_height()) imgOffsetX = imgRect.x imgOffsetY = imgRect.y # Get the total area that needs to be redrawn imgRect = imgRect.intersect(data.area) srcX = imgRect.x - imgOffsetX srcY = imgRect.y - imgOffsetY widget.window.draw_pixbuf( widget.get_style().fg_gc[gtk.STATE_NORMAL], self.cameraImagePixBuf, srcX, srcY, imgRect.x, imgRect.y, imgRect.width, imgRect.height) #--------------------------------------------------------------------------- def getImageRectangleInWidget(self, widget, imageWidth, imageHeight): # Centre the image inside the widget widgetX, widgetY, widgetWidth, widgetHeight = widget.get_allocation() imgRect = gtk.gdk.Rectangle(0, 0, widgetWidth, widgetHeight) if widgetWidth > imageWidth: imgRect.x = (widgetWidth - imageWidth) / 2 imgRect.width = imageWidth if widgetHeight > imageHeight: imgRect.y = (widgetHeight - imageHeight) / 2 imgRect.height = imageHeight return imgRect #--------------------------------------------------------------------------- def update(self): UPDATE_FREQUENCY = self.SAMPLES_PER_SECOND # Updates in Hz lastTime = time.clock() while 1: curTime = time.clock() if curTime - lastTime >= 1.0 / UPDATE_FREQUENCY: # Save the time lastTime = curTime yield True yield False
class MainWindow: OPTICAL_FLOW_BLOCK_WIDTH = 8 OPTICAL_FLOW_BLOCK_HEIGHT = 8 OPTICAL_FLOW_RANGE_WIDTH = 8 # Range to look outside of a block for motion OPTICAL_FLOW_RANGE_HEIGHT = 8 GRIPPER_WAVE_FREQUENCY = 1.0 # Waves per second GRIPPER_NUM_WAVES = 3.0 GRIPPER_WAVE_AMPLITUDE = math.radians( 20.0 )/4.0 BUFFER_TIME_LENGTH = 10.0 SAMPLES_PER_SECOND = 15.0 MAX_CORRELATION_LAG = 1.0 GRIPPER_DETECTION_TIME = 9.0 # Should be less than BUFFER_TIME_LENGTH GRIPPER_DETECTION_WAVE_START_TIME = 3.0 # 49, 81 -> 52.05, 15.62, -34.16, 81.33 - Top Left # 218, 76 -> 28.19, 58.89, -105.32, 48.80 - Top Right # 283, 135 -> 79.16, 67.70, -119.55, 48.80 - Bottom Right # 56, 142 -> 78.61, 29.24, -50.29, 65.60 - Bottom Left CORNER_JOINT_ARRAYS = [ [ math.radians( 52.05 ), # Top Left math.radians( 15.62 ), math.radians( -34.16 ), math.radians( 81.33 ), math.radians( 0.0 ) ], [ math.radians( 28.19 ), # Top Right math.radians( 58.89 ), math.radians( -105.32 ), math.radians( 48.80 ), math.radians( 0.0 ) ], [ math.radians( 79.16 ), # Bottom Right math.radians( 67.70 ), math.radians( -119.55 ), math.radians( 48.80 ), math.radians( 0.0 ) ], [ math.radians( 78.61 ), # Bottom Left math.radians( 29.24 ), math.radians( -50.29 ), math.radians( 65.60 ), math.radians( 0.0 ) ] ] CORNER_SCREEN_POSITIONS = [ ( 49, 81 ), # Top Left ( 218, 76 ), # Top Right ( 283, 135 ), # Bottom Right ( 56, 142 ) ] # Bottom Left #--------------------------------------------------------------------------- def __init__( self ): scriptPath = os.path.dirname( __file__ ) self.cameraImagePixBuf = None self.servoConfigDict = getArmServoConfig() self.wristAngle = 0.0 # Connect to the robot via ROS # TODO: Move arm to all dictionary servoConfigList = [ self.servoConfigDict[ servoName ] for servoName in self.servoConfigDict ] self.roboticArm = RoboticArm( gaffa_teleop.LynxmotionArmDescription.ARM_DH_PROXIMAL, servoConfigList ) self.cameraImageTopic = rospy.Subscriber( "/camera/image", sensor_msgs.msg.Image, self.cameraImageCallback ) self.jointScreenHomography = JointScreenHomography( self.CORNER_JOINT_ARRAYS, self.CORNER_SCREEN_POSITIONS, self.roboticArm ) # Setup the GUI builder = gtk.Builder() builder.add_from_file( scriptPath + "/GUI/VideoTeleop.glade" ) self.window = builder.get_object( "winMain" ) self.dwgCameraImage = builder.get_object( "dwgCameraImage" ) builder.connect_signals( self ) updateLoop = self.update() gobject.idle_add( updateLoop.next ) self.window.show() #--------------------------------------------------------------------------- def onWinMainDestroy( self, widget, data = None ): gtk.main_quit() #--------------------------------------------------------------------------- def main( self ): # All PyGTK applications must have a gtk.main(). Control ends here # and waits for an event to occur (like a key press or mouse event). gtk.main() #--------------------------------------------------------------------------- def cameraImageCallback( self, rosImage ): if rosImage.encoding == "rgb8" or rosImage.encoding == "bgr8": # Display the image self.cameraImagePixBuf = gtk.gdk.pixbuf_new_from_data( rosImage.data, gtk.gdk.COLORSPACE_RGB, False, 8, rosImage.width, rosImage.height, rosImage.step ) # Resize the drawing area if necessary if self.dwgCameraImage.get_size_request() != ( rosImage.width, rosImage.height ): self.dwgCameraImage.set_size_request( rosImage.width, rosImage.height ) self.dwgCameraImage.queue_draw() else: rospy.logerr( "Unhandled image encoding - " + rosImage.encoding ) #--------------------------------------------------------------------------- def moveArmToJointAnglePosition( self, servoAnglesDict, jointSpeed ): self.wristAngle = servoAnglesDict[ "wrist_rotate" ] self.roboticArm.setJointAngles( servoAnglesDict, jointSpeed ) #--------------------------------------------------------------------------- def onBtnGotoSafePosClicked( self, widget, data = None ): servoAnglesDict = { "base_rotate" : math.radians( 72.65 ), "shoulder_rotate" : 2.3561944901923448, "elbow_rotate" : -2.748893571891069, "wrist_rotate" : 1.9583014768641922, "gripper_rotate" : -0.0004890796739715704 } self.moveArmToJointAnglePosition( servoAnglesDict, math.radians( 1.5 ) ) #--------------------------------------------------------------------------- def onDwgCameraImageButtonPressEvent( self, widget, data ): if self.cameraImagePixBuf != None: imgRect = self.getImageRectangleInWidget( widget, self.cameraImagePixBuf.get_width(), self.cameraImagePixBuf.get_height() ) screenX = data.x - imgRect.x screenY = data.y - imgRect.y jointArray = self.jointScreenHomography.convertScreenPosToJointPos( ( screenX, screenY ) ) jointArrayDegrees = [ math.degrees( angle ) for angle in jointArray ] print "press at", screenX, screenY, "->", jointArrayDegrees print self.roboticArm.GetEndEffectorPos( jointArray ) servoAnglesDict = { "base_rotate" : jointArray[ 0 ], "shoulder_rotate" : jointArray[ 1 ], "elbow_rotate" : jointArray[ 2 ], "wrist_rotate" : jointArray[ 3 ], "gripper_rotate" : jointArray[ 4 ] } self.moveArmToJointAnglePosition( servoAnglesDict, math.radians( 0.5 ) ) #--------------------------------------------------------------------------- def onDwgCameraImageExposeEvent( self, widget, data = None ): if self.cameraImagePixBuf != None: imgRect = self.getImageRectangleInWidget( widget, self.cameraImagePixBuf.get_width(), self.cameraImagePixBuf.get_height() ) imgOffsetX = imgRect.x imgOffsetY = imgRect.y # Get the total area that needs to be redrawn imgRect = imgRect.intersect( data.area ) srcX = imgRect.x - imgOffsetX srcY = imgRect.y - imgOffsetY widget.window.draw_pixbuf( widget.get_style().fg_gc[ gtk.STATE_NORMAL ], self.cameraImagePixBuf, srcX, srcY, imgRect.x, imgRect.y, imgRect.width, imgRect.height ) #--------------------------------------------------------------------------- def getImageRectangleInWidget( self, widget, imageWidth, imageHeight ): # Centre the image inside the widget widgetX, widgetY, widgetWidth, widgetHeight = widget.get_allocation() imgRect = gtk.gdk.Rectangle( 0, 0, widgetWidth, widgetHeight ) if widgetWidth > imageWidth: imgRect.x = (widgetWidth - imageWidth) / 2 imgRect.width = imageWidth if widgetHeight > imageHeight: imgRect.y = (widgetHeight - imageHeight) / 2 imgRect.height = imageHeight return imgRect #--------------------------------------------------------------------------- def update( self ): UPDATE_FREQUENCY = self.SAMPLES_PER_SECOND # Updates in Hz lastTime = time.clock() while 1: curTime = time.clock() if curTime - lastTime >= 1.0 / UPDATE_FREQUENCY: # Save the time lastTime = curTime yield True yield False