00001 __author__ = 'tom1231' 00002 from PyQt4.QtGui import * 00003 from BAL.Interface.DeviceFrame import DeviceFrame, EX_DEV, SLAM 00004 from lxml.etree import Element, SubElement, XML 00005 00006 00007 class Slam(DeviceFrame): 00008 def __init__(self, frame, data): 00009 DeviceFrame.__init__(self, EX_DEV, frame, data) 00010 self._tf_map_scanmatch_transform_frame_name = 'scanmatcher_frame' 00011 self._base_frame = 'base_link' 00012 self._odom_frame = 'odom_link' 00013 self._map_frame = 'map' 00014 self._scan_topic = 'scan' 00015 00016 def toDict(self): 00017 data = dict() 00018 00019 data['type'] = SLAM 00020 data['tf'] = self._tf_map_scanmatch_transform_frame_name 00021 data['base'] = self._base_frame 00022 data['odom'] = self._odom_frame 00023 data['map'] = self._map_frame 00024 data['scan'] = self._scan_topic 00025 00026 return data 00027 00028 def showDetails(self, items=None): 00029 self.tf_map_scanmatch_transform_frame_name = QLineEdit(self._tf_map_scanmatch_transform_frame_name) 00030 self.base_frame = QLineEdit(self._base_frame) 00031 self.odom_frame = QLineEdit(self._odom_frame) 00032 self.map_frame = QLineEdit(self._map_frame) 00033 self.scan_topic = QLineEdit(self._scan_topic) 00034 00035 self._frame.layout().addRow(QLabel('Tf map scan match: '), self.tf_map_scanmatch_transform_frame_name) 00036 self._frame.layout().addRow(QLabel('Base frame: '), self.base_frame) 00037 self._frame.layout().addRow(QLabel('Odometry frame: '), self.odom_frame) 00038 self._frame.layout().addRow(QLabel('Map frame: '), self.map_frame) 00039 self._frame.layout().addRow(QLabel('Scan topic: '), self.scan_topic) 00040 00041 00042 def printDetails(self): 00043 self._frame.layout().addRow(QLabel('Tf map scan match: '), QLabel(self._tf_map_scanmatch_transform_frame_name)) 00044 self._frame.layout().addRow(QLabel('Base frame: '), QLabel(self._base_frame)) 00045 self._frame.layout().addRow(QLabel('Odometry frame: '), QLabel(self._odom_frame)) 00046 self._frame.layout().addRow(QLabel('Map frame: '), QLabel(self._map_frame)) 00047 self._frame.layout().addRow(QLabel('Scan topic: '), QLabel(self._scan_topic)) 00048 00049 def getName(self): 00050 return "SLAM" 00051 00052 def saveToFile(self, parent): 00053 element = SubElement(parent, 'include', { 00054 'file': '$(find ric_board)/launch/hector_slam.launch' 00055 }) 00056 00057 SubElement(element, 'arg', { 00058 'name': 'tf_map_scanmatch_transform_frame_name', 00059 'value': self._tf_map_scanmatch_transform_frame_name 00060 }) 00061 SubElement(element, 'arg', { 00062 'name': 'base_frame', 00063 'value': self._base_frame 00064 }) 00065 SubElement(element, 'arg', { 00066 'name': 'odom_frame', 00067 'value': self._odom_frame 00068 }) 00069 SubElement(element, 'arg', { 00070 'name': 'map_frame', 00071 'value': self._map_frame 00072 }) 00073 SubElement(element, 'arg', { 00074 'name': 'pub_map_odom_transform', 00075 'default': 'true' 00076 }) 00077 SubElement(element, 'arg', { 00078 'name': 'scan_subscriber_queue_size', 00079 'default': '5' 00080 }) 00081 SubElement(element, 'arg', { 00082 'name': 'scan_topic', 00083 'value': self._scan_topic 00084 }) 00085 SubElement(element, 'arg', { 00086 'name': 'map_size', 00087 'default': '2048' 00088 }) 00089 00090 def add(self): 00091 if not self.nameIsValid(): 00092 QMessageBox.critical(self._frame, "Error", "Name already taken.") 00093 self._isValid = False 00094 return 00095 self._isValid = True 00096 self._tf_map_scanmatch_transform_frame_name = str(self.tf_map_scanmatch_transform_frame_name.text()) 00097 self._base_frame = str(self.base_frame.text()) 00098 self._odom_frame = str(self.odom_frame.text()) 00099 self._map_frame = str(self.map_frame.text()) 00100 self._scan_topic = str(self.scan_topic.text()) 00101 00102 def fromDict(self, data): 00103 self._tf_map_scanmatch_transform_frame_name = data['tf'] 00104 self._base_frame = data['base'] 00105 self._odom_frame = data['odom'] 00106 self._map_frame = data['map'] 00107 self._scan_topic = data['scan']