def btnLoadClicked(self): self.blockSignals(True) filename = QtGui.QFileDialog.getOpenFileName( self, 'Open file', os.path.join(path, 'data'), 'Mesh Files (*.stl)')[0] print 'Filename:', filename self.setWindowTitle(filename) self.robpath.load_mesh(filename) self.mesh_size = self.robpath.mesh.bpoint2 - self.robpath.mesh.bpoint1 self.updatePosition(self.robpath.mesh.bpoint1) # Rename to position self.updateSize(self.robpath.mesh.bpoint2 - self.robpath.mesh.bpoint1) self.marker_array = MarkerArray() #self.marker = MeshMarker(mesh_resource="file://"+filename, frame_id="/workobject") self.mesh = TriangleListMarker() self.mesh.set_frame('/workobject') self.mesh.set_points(0.001 * np.vstack(self.robpath.mesh.triangles)) self.mesh.set_color((0.66, 0.66, 0.99, 0.66)) self.marker_array.markers.append(self.mesh.marker) self.path = LinesMarker() self.path.set_frame('/workobject') self.path.set_color((1.0, 0.0, 0.0, 1.0)) self.marker_array.markers.append(self.path.marker) for id, m in enumerate(self.marker_array.markers): m.id = id self.npoints = 0 # #rospy.loginfo() self.pub_marker_array.publish(self.marker_array) #TODO: Change bpoints. #self.lblInfo.setText('Info:\n') #self.plot.drawMesh(self.robpath.mesh) #TODO: Add info from velocity estimation module. self.blockSignals(False)
def __init__(self, parent=None): QtGui.QWidget.__init__(self, parent) loadUi(os.path.join(path, 'resources', 'path.ui'), self) try: rospy.wait_for_service('robot_send_command', timeout=5) self.send_command = rospy.ServiceProxy('robot_send_command', SrvRobotCommand) except: rospy.loginfo('ERROR connecting to service robot_send_command.') #self.pub = rospy.Publisher( # 'robot_command_json', MsgRobotCommand, queue_size=10) self.btnLoadPath.clicked.connect(self.btnLoadPathClicked) icon = QtGui.QIcon.fromTheme('document-open') self.btnLoadPath.setIcon(icon) self.btnSavePath.clicked.connect(self.btnSavePathClicked) icon = QtGui.QIcon.fromTheme('document-save') self.btnSavePath.setIcon(icon) self.btnRunPath.clicked.connect(self.btnRunPathClicked) icon = QtGui.QIcon.fromTheme('media-playback-start') self.btnRunPath.setIcon(icon) self.btnDelete.clicked.connect(self.btnDeleteClicked) self.btnLoadPose.clicked.connect(self.btnLoadPoseClicked) self.btnStep.clicked.connect(self.btnStepClicked) self.btnCancel.clicked.connect(self.btnCancelClicked) self.listWidgetPoses.itemSelectionChanged.connect(self.lstPosesClicked) self.listWidgetPoses.itemDoubleClicked.connect(self.qlistDoubleClicked) self.jason = Jason() self.ok_command = "OK" self.offset_position = 100 self.quat = [0, np.sin(np.deg2rad(45)), 0, np.cos(np.deg2rad(45))] self.quat_inv = [0, -np.sin(np.deg2rad(45)), 0, np.cos(np.deg2rad(45))] self.pub_marker_array = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10) self.marker_array = MarkerArray() self.lines = LinesMarker() self.lines.set_size(0.005) self.lines.set_color((1, 0, 0, 1)) self.lines.set_frame('/workobject') self.marker_array.markers.append(self.lines.marker) self.arrow = ArrowMarker(0.1) self.arrow.set_color((0, 0, 1, 1)) self.arrow.set_frame('/workobject') # self.arrow.set_position((0.2, 0.2, 0.2)) # self.arrow.set_orientation((0, 0, 0, 1)) self.marker_array.markers.append(self.arrow.marker) for id, m in enumerate(self.marker_array.markers): m.id = id self.tmrRunPath = QtCore.QTimer(self) self.tmrRunPath.timeout.connect(self.timeRunPathEvent)