def __init__( self ):
"""
Constructor.
"""
QtGui.QMainWindow.__init__( self );
rospy.init_node( 'nao_experiment_setup_gui' );
# Setup the interface.
self.setupUi( self );
# Initialize.
self.__state = self.STATE_NONE;
self.ColorListModel = SetupGUIListModel();
self.ColorList.setModel( self.ColorListModel );
self.BinListModel = SetupGUIListModel();
self.BinList.setModel( self.BinListModel );
# Set the head correctly.
self.setupHead();
# Setup OpenCV.
self.cvBridge = CvBridge();
# Create subscribers.
self.cameraSubscriber = rospy.Subscriber( '/nao_camera_processed/image_raw', Image, self.onImageReceived );
# Add handlers.
self.ButtonImageCrop.clicked.connect( self.buttonImageCropClicked );
self.ButtonImageReset.clicked.connect( self.buttonImageResetClicked );
self.ButtonColorAdd.clicked.connect( self.buttonColorAddClicked );
self.ButtonColorDelete.clicked.connect( self.buttonColorDeleteClicked );
self.ButtonBinAdd.clicked.connect( self.buttonBinAddClicked );
self.ButtonBinDelete.clicked.connect( self.buttonBinDeleteClicked );
self.ActionSave.triggered.connect( self.actionSaveTriggered );
self.ActionQuit.triggered.connect( self.actionQuitTriggered );
# Fugly, but it works for now.
self.ImageWidget.mousePressEvent = self.imageWidgetMousePressEvent;
self.ImageWidget.mouseMoveEvent = self.imageWidgetMouseMoveEvent;
self.ImageWidget.mouseReleaseEvent = self.imageWidgetMouseReleaseEvent;
# Show.
self.show();
class SetupGUI( QtGui.QMainWindow ):
"""
SetupGUI
"""
STATE_NONE = 0;
STATE_CROPPING = 1;
STATE_COLOR_PICKING = 2;
STATE_BIN_PICKING = 3;
def __init__( self ):
"""
Constructor.
"""
QtGui.QMainWindow.__init__( self );
rospy.init_node( 'nao_experiment_setup_gui' );
# Setup the interface.
self.setupUi( self );
# Initialize.
self.__state = self.STATE_NONE;
self.ColorListModel = SetupGUIListModel();
self.ColorList.setModel( self.ColorListModel );
self.BinListModel = SetupGUIListModel();
self.BinList.setModel( self.BinListModel );
# Set the head correctly.
self.setupHead();
# Setup OpenCV.
self.cvBridge = CvBridge();
# Create subscribers.
self.cameraSubscriber = rospy.Subscriber( '/nao_camera_processed/image_raw', Image, self.onImageReceived );
# Add handlers.
self.ButtonImageCrop.clicked.connect( self.buttonImageCropClicked );
self.ButtonImageReset.clicked.connect( self.buttonImageResetClicked );
self.ButtonColorAdd.clicked.connect( self.buttonColorAddClicked );
self.ButtonColorDelete.clicked.connect( self.buttonColorDeleteClicked );
self.ButtonBinAdd.clicked.connect( self.buttonBinAddClicked );
self.ButtonBinDelete.clicked.connect( self.buttonBinDeleteClicked );
self.ActionSave.triggered.connect( self.actionSaveTriggered );
self.ActionQuit.triggered.connect( self.actionQuitTriggered );
# Fugly, but it works for now.
self.ImageWidget.mousePressEvent = self.imageWidgetMousePressEvent;
self.ImageWidget.mouseMoveEvent = self.imageWidgetMouseMoveEvent;
self.ImageWidget.mouseReleaseEvent = self.imageWidgetMouseReleaseEvent;
# Show.
self.show();
def actionSaveTriggered( self ):
"""
actionSaveTriggered
"""
path = os.path.join( roslib.packages.get_pkg_dir( PACKAGE_NAME ), 'cfg' );
# Save color blob file.
if( len( self.ColorListModel.items() ) > 0 ):
filePath = os.path.join( path, 'colors.txt' );
with open( filePath, 'w' ) as file:
colors = '';
thresholds = '';
for colorBlob in self.ColorListModel.items():
colors += colorBlob.formatColor() + '\n';
thresholds += colorBlob.formatThreshold() + '\n';
# Write data.
file.write( '[colors]\n' );
file.write( colors );
file.write( '\n' );
file.write( '[thresholds]\n' );
file.write( thresholds );
rospy.loginfo( 'Saved color blob information to: {filePath}'.format( filePath = filePath ) );
# Save bin file.
if( len( self.BinListModel.items() ) > 0 ):
filePath = os.path.join( path, 'pieces.yaml' );
items = [];
for item in self.BinListModel.items():
items.append( dict( x = item.x(), y = item.y(), width = item.width(), height = item.height() ) );
with open( filePath, 'w' ) as file:
yaml.dump( items, file );
rospy.loginfo( 'Saved bin settings to: {filePath}'.format( filePath = filePath ) );
def actionQuitTriggered( self ):
"""
actionQuitTriggered
"""
pass;
def setupHead( self ):
# Connect to the server.
self.actionClient = actionlib.SimpleActionClient( 'joint_trajectory', JointTrajectoryAction );
if( not self.actionClient.wait_for_server( rospy.Duration( 10 ) ) ):
rospy.logerr( 'Unable to establish a connection to the "joint_trajectory" server.' );
sys.exit( 1 );
# Set the stiffness to the head.
self.stiffnessPublisher = rospy.Publisher( '/joint_stiffness', JointState );
rospy.sleep( rospy.Duration( 1 ) );
self.stiffnessPublisher.publish( JointState( name = [ 'HeadYaw', 'HeadPitch' ], effort = [ 1.0, 1.0 ] ) );
rospy.sleep( rospy.Duration( 1 ) );
# Set the correct position of the head.
goal = JointTrajectoryGoal();
goal.trajectory.joint_names = [ 'HeadYaw', 'HeadPitch' ];
goal.trajectory.points = [];
goal.trajectory.points.append( JointTrajectoryPoint( time_from_start = rospy.Duration( 2 ), positions = [ 0.0, 0.5149 ] ) ); #0.5149
self.actionClient.send_goal_and_wait( goal );
def setStiffness( self, name = None, effort = 1.0 ):
"""
Set the stiffness of one or more joints.
Should be used for any joint except when full body stiffness is needed.
"""
# One second should be enough time for the controller to hook up.
self.stiffnessPublisher = rospy.Publisher( '/joint_stiffness', JointState );
rospy.sleep( rospy.Duration( 1 ) );
self.stiffnessPublisher.publish( JointState( name = list( name ), effort = list( effort ) ) );
rospy.sleep( rospy.Duration( 1 ) );
def handleImageCrop( self, p1, p2 ):
# Calculate settings.
xOffset = min( p1.x(), p2.x() );
yOffset = min( p1.y(), p2.y() );
width = max( p1.x(), p2.x() ) - min( p1.x(), p2.x() );
height = max( p1.y(), p2.y() ) - min( p1.y(), p2.y() );
client = dynamic_reconfigure.client.Client( 'nao_blob_detection' );
client.update_configuration( { 'xOffset': xOffset, 'yOffset': yOffset, 'width': width, 'height': height } );
self.ImageWidget.clearRectangle();
self.reset();
def handleColorAdd( self ):
setupGUIDialog = SetupGUIDialog( self );
if( setupGUIDialog.exec_() == QtGui.QDialog.Accepted ):
name = setupGUIDialog.getName();
color = setupGUIDialog.getColor();
colorBlob = ColorBlob( name, color, self.__colors );
self.ColorListModel.addItem( colorBlob );
self.reset();
def handleBinAdd( self ):
# Add the bin to the list.
piece = PieceRectangular.fromCenter( self.__position.x(), self.__position.y() );
self.BinListModel.addItem( piece );
# Redraw.
self.ImageWidget.drawBins( self.BinListModel );
self.reset();
def reset( self ):
self.__state = self.STATE_NONE;
self.ButtonImageCrop.setDown( False );
self.ButtonColorAdd.setDown( False );
self.ButtonBinAdd.setDown( False );
self.StatusBar.clearMessage();
def buttonImageCropClicked( self ):
"""
buttonImageCropClicked
"""
if( self.__state == self.STATE_NONE ):
self.__state = self.STATE_CROPPING;
self.__position = None;
self.ButtonImageCrop.setDown( True );
def buttonImageResetClicked( self ):
"""
buttonImageResetClicked
"""
# Create a service.
client = dynamic_reconfigure.client.Client( 'nao_blob_detection' );
client.update_configuration( { 'xOffset': 0, 'yOffset': 0, 'width': 0, 'height': 0 } );
self.__state = self.STATE_NONE;
def buttonColorAddClicked( self ):
"""
buttonColorAddClicked
"""
if( self.__state == self.STATE_NONE ):
self.__state = self.STATE_COLOR_PICKING;
self.__colors = [];
self.ButtonColorAdd.setDown( True );
elif( self.__state == self.STATE_COLOR_PICKING ):
self.handleColorAdd();
def buttonColorDeleteClicked( self ):
"""
buttonColorDeleteClicked
"""
# Get the selected indices.
selectedIndices = self.ColorList.selectedIndexes();
for selectedIndex in selectedIndices:
self.ColorListModel.deleteItem( selectedIndex );
def buttonBinAddClicked( self ):
"""
buttonBinAddClicked
"""
if( self.__state == self.STATE_NONE ):
self.__state = self.STATE_BIN_PICKING;
self.__position = None;
self.ButtonBinAdd.setDown( True );
elif( self.__state == self.STATE_BIN_PICKING ):
self.handleBinAdd();
def buttonBinDeleteClicked( self ):
"""
buttonBinDeleteClicked
"""
# Get the selected indices.
selectedIndices = self.BinList.selectedIndexes();
for selectedIndex in selectedIndices:
self.BinListModel.deleteItem( selectedIndex );
# Redraw.
self.ImageWidget.drawBins( self.BinListModel );
def imageWidgetMousePressEvent( self, event ):
"""
imageWidgetMousePressEvent
"""
# When a right mouse button occurs, things have to be finished and then cleaned up.
if( event.button() == QtCore.Qt.RightButton ):
if( self.__state == self.STATE_COLOR_PICKING ):
self.handleColorAdd();
elif( self.__state == self.STATE_BIN_PICKING ):
self.handleBinAdd();
# Reset.
self.reset();
return;
# None.
if( self.__state == self.STATE_NONE ):
pass;
# Cropping.
elif( self.__state == self.STATE_CROPPING ):
self.__position = event.pos();
# Color picking.
elif( self.__state == self.STATE_COLOR_PICKING ):
# Locate the color at the current position.
rgb = cv.Get2D( self.__openCVMatrix, event.pos().y(), event.pos().x() );
self.__colors.append( rgb );
# Bin picking.
elif( self.__state == self.STATE_BIN_PICKING ):
self.__position = event.pos();
self.handleBinAdd();
def imageWidgetMouseMoveEvent( self, event ):
"""
imageWidgetMouseMoveEvent
"""
if( self.__state == self.STATE_CROPPING ):
if( not( self.__position is None ) ):
self.ImageWidget.drawRectangle( self.__position, event.pos() );
else:
self.StatusBar.showMessage( 'Position: ({x}, {y})'.format( x = event.pos().x(), y = event.pos().y() ) );
elif( self.__state == self.STATE_COLOR_PICKING ):
rgb = cv.Get2D( self.__openCVMatrix, event.pos().y(), event.pos().x() );
self.StatusBar.showMessage( 'R:{r} G:{g} B:{b}'.format( r = rgb[ 0 ], g = rgb[ 1 ], b = rgb[ 2 ] ) );
elif( self.__state == self.STATE_BIN_PICKING ):
self.StatusBar.showMessage( 'Position: ({x}, {y})'.format( x = event.pos().x(), y = event.pos().y() ) );
def imageWidgetMouseReleaseEvent( self, event ):
"""
imageWidgetMouseReleaseEvent
"""
if( self.__state == self.STATE_CROPPING ):
self.handleImageCrop( self.__position, event.pos() );
def setupUi(self, MainWindow):
MainWindow.setObjectName(_fromUtf8("MainWindow"))
MainWindow.resize(996, 803)
MainWindow.setWindowTitle(QtGui.QApplication.translate("MainWindow", "Experiment Setup", None, QtGui.QApplication.UnicodeUTF8))
self.CentralWidget = QtGui.QWidget(MainWindow)
self.CentralWidget.setMaximumSize(QtCore.QSize(16777215, 16777215))
self.CentralWidget.setObjectName(_fromUtf8("CentralWidget"))
self.CentralWidgetLayout = QtGui.QHBoxLayout(self.CentralWidget)
self.CentralWidgetLayout.setObjectName(_fromUtf8("CentralWidgetLayout"))
self.MenuWidget = QtGui.QWidget(self.CentralWidget)
self.MenuWidget.setMaximumSize(QtCore.QSize(220, 16777215))
self.MenuWidget.setObjectName(_fromUtf8("MenuWidget"))
self.MenuWidgetLayout = QtGui.QVBoxLayout(self.MenuWidget)
self.MenuWidgetLayout.setMargin(0)
self.MenuWidgetLayout.setObjectName(_fromUtf8("MenuWidgetLayout"))
self.ImageSettingsWidget = QtGui.QWidget(self.MenuWidget)
sizePolicy = QtGui.QSizePolicy(QtGui.QSizePolicy.Preferred, QtGui.QSizePolicy.Preferred)
sizePolicy.setHorizontalStretch(0)
sizePolicy.setVerticalStretch(0)
sizePolicy.setHeightForWidth(self.ImageSettingsWidget.sizePolicy().hasHeightForWidth())
self.ImageSettingsWidget.setSizePolicy(sizePolicy)
self.ImageSettingsWidget.setObjectName(_fromUtf8("ImageSettingsWidget"))
self.ImageSettingsWidgetLayout = QtGui.QVBoxLayout(self.ImageSettingsWidget)
self.ImageSettingsWidgetLayout.setMargin(0)
self.ImageSettingsWidgetLayout.setObjectName(_fromUtf8("ImageSettingsWidgetLayout"))
self.ImageSettingsLabel = QtGui.QLabel(self.ImageSettingsWidget)
font = QtGui.QFont()
font.setPointSize(10)
font.setBold(True)
font.setWeight(75)
self.ImageSettingsLabel.setFont(font)
self.ImageSettingsLabel.setText(QtGui.QApplication.translate("MainWindow", "Image settings", None, QtGui.QApplication.UnicodeUTF8))
self.ImageSettingsLabel.setObjectName(_fromUtf8("ImageSettingsLabel"))
self.ImageSettingsWidgetLayout.addWidget(self.ImageSettingsLabel)
self.ButtonImageCrop = QtGui.QPushButton(self.ImageSettingsWidget)
self.ButtonImageCrop.setText(QtGui.QApplication.translate("MainWindow", "C&rop", None, QtGui.QApplication.UnicodeUTF8))
self.ButtonImageCrop.setObjectName(_fromUtf8("ButtonImageCrop"))
self.ImageSettingsWidgetLayout.addWidget(self.ButtonImageCrop)
self.ButtonImageReset = QtGui.QPushButton(self.ImageSettingsWidget)
self.ButtonImageReset.setText(QtGui.QApplication.translate("MainWindow", "Reset", None, QtGui.QApplication.UnicodeUTF8))
self.ButtonImageReset.setObjectName(_fromUtf8("ButtonImageReset"))
self.ImageSettingsWidgetLayout.addWidget(self.ButtonImageReset)
self.MenuWidgetLayout.addWidget(self.ImageSettingsWidget)
self.ColorSettingsWidget = QtGui.QWidget(self.MenuWidget)
self.ColorSettingsWidget.setObjectName(_fromUtf8("ColorSettingsWidget"))
self.ColorSettingsWidgetLayout = QtGui.QVBoxLayout(self.ColorSettingsWidget)
self.ColorSettingsWidgetLayout.setMargin(0)
self.ColorSettingsWidgetLayout.setObjectName(_fromUtf8("ColorSettingsWidgetLayout"))
self.ColorSettingsLabel = QtGui.QLabel(self.ColorSettingsWidget)
font = QtGui.QFont()
font.setPointSize(10)
font.setBold(True)
font.setWeight(75)
self.ColorSettingsLabel.setFont(font)
self.ColorSettingsLabel.setText(QtGui.QApplication.translate("MainWindow", "Color settings", None, QtGui.QApplication.UnicodeUTF8))
self.ColorSettingsLabel.setObjectName(_fromUtf8("ColorSettingsLabel"))
self.ColorSettingsWidgetLayout.addWidget(self.ColorSettingsLabel)
self.ColorList = QtGui.QListView(self.ColorSettingsWidget)
self.ColorList.setObjectName(_fromUtf8("ColorList"))
self.ColorSettingsWidgetLayout.addWidget(self.ColorList)
self.ColorSettingsButtonLayout = QtGui.QHBoxLayout()
self.ColorSettingsButtonLayout.setObjectName(_fromUtf8("ColorSettingsButtonLayout"))
self.ButtonColorAdd = QtGui.QPushButton(self.ColorSettingsWidget)
self.ButtonColorAdd.setText(QtGui.QApplication.translate("MainWindow", "Add &color", None, QtGui.QApplication.UnicodeUTF8))
self.ButtonColorAdd.setObjectName(_fromUtf8("ButtonColorAdd"))
self.ColorSettingsButtonLayout.addWidget(self.ButtonColorAdd)
self.ButtonColorDelete = QtGui.QPushButton(self.ColorSettingsWidget)
self.ButtonColorDelete.setText(QtGui.QApplication.translate("MainWindow", "Delete color", None, QtGui.QApplication.UnicodeUTF8))
self.ButtonColorDelete.setObjectName(_fromUtf8("ButtonColorDelete"))
self.ColorSettingsButtonLayout.addWidget(self.ButtonColorDelete)
self.ColorSettingsWidgetLayout.addLayout(self.ColorSettingsButtonLayout)
self.MenuWidgetLayout.addWidget(self.ColorSettingsWidget)
self.BinSettingsWidget = QtGui.QWidget(self.MenuWidget)
self.BinSettingsWidget.setObjectName(_fromUtf8("BinSettingsWidget"))
self.BinSettingsLayout = QtGui.QVBoxLayout(self.BinSettingsWidget)
self.BinSettingsLayout.setMargin(0)
self.BinSettingsLayout.setObjectName(_fromUtf8("BinSettingsLayout"))
self.BinSettingsLabel = QtGui.QLabel(self.BinSettingsWidget)
font = QtGui.QFont()
font.setPointSize(10)
font.setBold(True)
font.setWeight(75)
self.BinSettingsLabel.setFont(font)
self.BinSettingsLabel.setText(QtGui.QApplication.translate("MainWindow", "Bin settings", None, QtGui.QApplication.UnicodeUTF8))
self.BinSettingsLabel.setObjectName(_fromUtf8("BinSettingsLabel"))
self.BinSettingsLayout.addWidget(self.BinSettingsLabel)
self.BinList = QtGui.QListView(self.BinSettingsWidget)
self.BinList.setObjectName(_fromUtf8("BinList"))
self.BinSettingsLayout.addWidget(self.BinList)
self.BinSettingsButtonLayout = QtGui.QHBoxLayout()
self.BinSettingsButtonLayout.setObjectName(_fromUtf8("BinSettingsButtonLayout"))
self.ButtonBinAdd = QtGui.QPushButton(self.BinSettingsWidget)
self.ButtonBinAdd.setText(QtGui.QApplication.translate("MainWindow", "Add &bin", None, QtGui.QApplication.UnicodeUTF8))
self.ButtonBinAdd.setObjectName(_fromUtf8("ButtonBinAdd"))
self.BinSettingsButtonLayout.addWidget(self.ButtonBinAdd)
self.ButtonBinDelete = QtGui.QPushButton(self.BinSettingsWidget)
self.ButtonBinDelete.setText(QtGui.QApplication.translate("MainWindow", "Delete bin", None, QtGui.QApplication.UnicodeUTF8))
self.ButtonBinDelete.setObjectName(_fromUtf8("ButtonBinDelete"))
self.BinSettingsButtonLayout.addWidget(self.ButtonBinDelete)
self.BinSettingsLayout.addLayout(self.BinSettingsButtonLayout)
self.MenuWidgetLayout.addWidget(self.BinSettingsWidget)
spacerItem = QtGui.QSpacerItem(20, 40, QtGui.QSizePolicy.Minimum, QtGui.QSizePolicy.Expanding)
self.MenuWidgetLayout.addItem(spacerItem)
self.CentralWidgetLayout.addWidget(self.MenuWidget)
self.ImageWidget = SetupGUIImageWidget(self.CentralWidget) #QtGui.QWidget(self.CentralWidget)
self.ImageWidget.setMouseTracking(True)
sizePolicy = QtGui.QSizePolicy(QtGui.QSizePolicy.Expanding, QtGui.QSizePolicy.Expanding)
sizePolicy.setHorizontalStretch(0)
sizePolicy.setVerticalStretch(0)
sizePolicy.setHeightForWidth(self.ImageWidget.sizePolicy().hasHeightForWidth())
self.ImageWidget.setSizePolicy(sizePolicy)
self.ImageWidget.setObjectName(_fromUtf8("ImageWidget"))
self.CentralWidgetLayout.addWidget(self.ImageWidget)
MainWindow.setCentralWidget(self.CentralWidget)
self.MenuBar = QtGui.QMenuBar(MainWindow)
self.MenuBar.setGeometry(QtCore.QRect(0, 0, 996, 21))
self.MenuBar.setObjectName(_fromUtf8("MenuBar"))
self.MenuFile = QtGui.QMenu(self.MenuBar)
self.MenuFile.setTitle(QtGui.QApplication.translate("MainWindow", "File", None, QtGui.QApplication.UnicodeUTF8))
self.MenuFile.setObjectName(_fromUtf8("MenuFile"))
MainWindow.setMenuBar(self.MenuBar)
self.StatusBar = QtGui.QStatusBar(MainWindow)
self.StatusBar.setObjectName(_fromUtf8("StatusBar"))
MainWindow.setStatusBar(self.StatusBar)
self.ActionSave = QtGui.QAction(MainWindow)
self.ActionSave.setText(QtGui.QApplication.translate("MainWindow", "Save", None, QtGui.QApplication.UnicodeUTF8))
self.ActionSave.setShortcut(QtGui.QApplication.translate("MainWindow", "Ctrl+S", None, QtGui.QApplication.UnicodeUTF8))
self.ActionSave.setObjectName(_fromUtf8("ActionSave"))
self.ActionQuit = QtGui.QAction(MainWindow)
self.ActionQuit.setText(QtGui.QApplication.translate("MainWindow", "Quit", None, QtGui.QApplication.UnicodeUTF8))
self.ActionQuit.setShortcut(QtGui.QApplication.translate("MainWindow", "Ctrl+Q", None, QtGui.QApplication.UnicodeUTF8))
self.ActionQuit.setObjectName(_fromUtf8("ActionQuit"))
self.MenuFile.addAction(self.ActionSave)
self.MenuFile.addSeparator()
self.MenuFile.addAction(self.ActionQuit)
self.MenuBar.addAction(self.MenuFile.menuAction())
def onImageReceived( self, data ):
"""
onImageReceived
@param: data
"""
'''
openCVMatrix = self.cvBridge.imgmsg_to_cv( data, 'rgb8' );
#qtImage = QtGui.QImage( openCVMatrix.tostring(), data.width, data.height, QtGui.QImage.Format_RGB888 );
qtPixmap = QtGui.QPixmap();
qtPixmap.loadFromData( openCVMatrix.tostring() );
self.ImageWidget.setMinimumSize( data.width, data.height );
self.ImageWidget.resize( data.width, data.height );
self.ImageWidget.drawImage( qtPixmap );
'''
self.__openCVMatrix = self.cvBridge.imgmsg_to_cv( data, 'rgb8' );
qImage = QtGui.QImage( self.__openCVMatrix.tostring(), data.width, data.height, self.__openCVMatrix.step, QtGui.QImage.Format_RGB888 );
self.ImageWidget.setMinimumSize( data.width, data.height );
self.ImageWidget.resize( data.width, data.height );
self.ImageWidget.drawImage( qImage );