ROS消息，经纬度采集

gnss定位时rtk数据比较精确、稳定，大场地绘制高精地图的路点或其他必要标记时，可以采用经纬度采集工具采集，然后批量进行平面的转换得到数据。可以节省时间。

简单粗暴的实现一下

python

# -*- coding: utf-8 -*-

import os,sys
#os.chdir(sys.path[0])
#old_path = os.getcwd()


from PyQt5.QtWidgets import QApplication, QMainWindow
from PyQt5.uic import loadUi
from PyQt5.QtCore import pyqtSignal
import rospy
import rosgraph
from std_msgs.msg import String
from sensor_msgs.msg import NavSatFix
import string
import json
import time

import threading
import re
import math


import yaml
from PyQt5 import QtCore, QtWidgets, QtGui
from PyQt5.QtCore import pyqtSignal
from PyQt5.QtWidgets import  QWidget, QTableView, QHBoxLayout,QHeaderView,QAbstractItemView
from PyQt5.QtGui import QStandardItemModel,QStandardItem
class GNSSRecorder(QtCore.QObject):

    signal_on_data = pyqtSignal(object)#'PyQt_PyObject'
    signal_on_shutdown = pyqtSignal(object)#'PyQt_PyObject'
    def __init__(self):
        super(QtCore.QObject, self).__init__()
        rospy.init_node('GNSSRecorder',anonymous = True)
        self.rate_hz = rospy.get_param("~rate", 2)
        #self.yaml_file_param = rospy.get_param("~yaml_file_param", "/map2d_tf_file")
        #self.yaml_file = rospy.get_param(self.yaml_file_param )
        self.yaml_file = "./test.csv"
        self.rate = rospy.Rate(self.rate_hz)
        rospy.on_shutdown(self.shutdown)        
        self.initparams()
        rospy.Subscriber('/test', String, self.recv_string)

        rospy.Subscriber('/fix', NavSatFix, self.recv_NavSatFix)
        #self.posepub = rospy.Publisher('/adjust_pose_world_map2d', Pose, queue_size=10)
        self.threadlist=[]
        self.lon=0
        self.lat=0
        self.alt=0
        self.isshutdown=0
    def recv_NavSatFix(self, msg):
        if (math.isnan(msg.latitude) or math.isnan(msg.longitude) or  math.isnan(msg.altitude)):
            print "gnss data is nan!!"
            return
        self.lat=msg.latitude
        self.lon=msg.longitude
        self.alt=msg.altitude
        self.signal_on_data.emit({'lon': str(self.lon), 'lat': str(self.lat), 'alt': str(self.alt)})
    def recv_string(self, msg):
        o=None
        if type(msg) is String:

            d=msg.data.split(",")
            print d
        if len(d)>=3:
            self.lon=d[0]
            self.lat=d[1]
            self.alt=d[2]
        self.signal_on_data.emit({'lon':self.lon, 'lat':self.lat,'alt':self.alt})
                
    def trans_save_data(self):        
        if rospy.is_shutdown() :
            return

        data={'lon':self.lon, 'lat':self.lat,'alt':self.alt}

        print data
        try:
            with open(self.yaml_file, 'w') as f:
                yaml.dump(data, f)
            print "save yaml file ok %s" % self.yaml_file
        except:
            print "save yaml file err %s" % self.yaml_file
            pass


    def th_updateparams(self):
        if len(self.threadlist)==0:
            t = threading.Thread(target=self.updateparams)
            self.threadlist.append(t)
            t.setDaemon(True)
            t.start()


   
    def initparams(self):
        pass

    def updateparams(self):
        print "update"
        
        if len(self.threadlist):
            self.threadlist.pop()

    def shutdown(self):
        self.signal_on_shutdown.emit({})
        
        while len(self.threadlist):
            th=self.threadlist.pop()
            if th.isAlive():
                ThreadStop = threading.Thread._Thread__stop
                ThreadStop(th)
        print "shutdown"
        self.trans_save_data()
        self.isshutdown=1
        pass

    def run(self):
        c=0
        while not rospy.is_shutdown():
                self.rate.sleep()
                #self.pubmarkerarray()
                c=c+1
                if c%10==0:
                    # self.updateparams()
                    #self.th_updateparams()
                    c = 0
                print c
                #self.posepub.publish(self.pose)
                if rospy.is_shutdown():
                    break


class UItest(QMainWindow):
    signal_on_rosok = pyqtSignal(object)  # 'PyQt_PyObject'
    def __init__(self, parent=None):
        super(UItest,self).__init__(parent)
        p=sys.path[0]+"/"
        self.ui=loadUi(p+'gnss.ui', self)
        #隐藏标签
        #self.ui.tabWidget.tabBar().hide()
        #self.ui.tabWidget.findChildren(QTabBar)[0].hide()

        # test
        #self.ui.tabWidget.setTabEnabled(0, False);
        #self.ui.tabWidget.setTabEnabled(1, False);
        #self.ui.tabWidget.setStyleSheet("QTabBar::tab:disabled {width: 0; color: transparent;}");
        
        self.ui.pushButton.clicked.connect(self.click1)
        self.ui.pushButton_2.clicked.connect(self.click2)
        self.ui.pushButton_3.clicked.connect(self.click3)
        self.ui.pushButton_record.clicked.connect(self.click_record)
        self.ui.pushButton_save.clicked.connect(self.savedata)
        self.signal_on_rosok.connect(self.createrosnode)
        self.m=None
        self.mc=0
        #self.m2=GNSSRecorder()
        #m.run()
        self.t = threading.Thread(target=self.checkros)
        self.t.setDaemon(True)
        self.t.start()
        prefilename=time.strftime("%Y_%m_%d_%H_%M_%S", time.localtime())
        print(prefilename)
        self.ui.lineEdit_filename.setText(prefilename)
        self.tableview=self.ui.tableView


        self.tablehead=['序号', '经度', '纬度','海拔']
        self.tableView_init()
    def createrosnode(self):
        self.m = GNSSRecorder()
        self.m.signal_on_data.connect(self.showdata)
        self.m.signal_on_shutdown.connect(self.savedata)
        self.mc=1
    def checkros(self):
        u = os.environ['ROS_MASTER_URI']
        #normal=rosgraph.is_master_online(u) and self.mc and self.m.isshutdown==0

        while 1:
            if not rosgraph.is_master_online(u)  :
                #self.mc = 0
                print "wait roscore...."
            else:
                if self.mc == 0:
                    print "ros node create"
                    #self.createrosnode()
                    self.signal_on_rosok.emit({})
                else :
                    if self.m.isshutdown==1:
                        #self.mc = 0
                        print "ros node shutdown.... please close this window,run it again. "
                    else:
                        print "ros node ok...."
                        pass
            time.sleep(2)

    def savedata(self, event):
        rows = self.ui.tableView.model().rowCount()
        row_str=""
        for i in range(rows):
            line_str=""
            for j in range(self.model_cols):
                #print i,j
                content = self.model.item(i, j).text()  
                if content =="":
                    content="''"
                #content = self.ui.tableView.item(row, col).text()
                if j==0:
                    line_str=line_str+ content
                else:
                    line_str=line_str+","+ content
            row_str=row_str+line_str+"\n"
        #head
        line_str = ""
        for j in range(len(self.tablehead)):
            content=self.tablehead[j]
            if j == 0:
                line_str = line_str + content
            else:
                line_str = line_str + "," + content
        line_str = line_str + "\n"
        row_str=line_str+row_str

        p="default.csv"
        try:
            p=self.ui.lineEdit_filename.text()
        except:
            pass
        if len(p)>=3:
           if not p[-3:] =="csv":
               p =p+".csv"
        else:
            p = p + ".csv"
        try:

            with open(p, 'w') as f:
                #yaml.dump(data, f)
                f.write(row_str)
            self.ui.label_desc_data.setText("save csv file ok ")
            print "save csv file ok %s" % p
        except:
            print "save csv file err %s" % p
            self.ui.label_desc_data.setText("save csv file err ")
            pass
        #print row_str

    def showdata(self, event):
        print event
        self.ui.lineEdit_lon.setText(event['lon'])
        self.ui.lineEdit_lat.setText(event['lat'])
        self.ui.lineEdit_alt.setText(event['alt'])

    def xxxx(self ):
        u = os.environ['ROS_MASTER_URI']
        if self.m:
           pass #self.statu
        if not self.rosok:
            #while self.rosmasterok 
            while not rosgraph.is_master_online(u):
                time.sleep(1)
            self.m=GNSSRecorder()
        

            print "wait rosgraph.is_master_online(m)"
    def click_record(self, event):
        print "rrrrrrrrrrrr"

        lon=self.ui.lineEdit_lon.text()
        lat=self.ui.lineEdit_lat.text()
        alt=self.ui.lineEdit_alt.text()
        flg=self.ui.lineEdit_flag.text()

        self.tableView_add(flg,lon,lat,alt)

    def click1(self, event):
        self.ui.tabWidget.setCurrentIndex(1);
    def click2(self, event):
        self.ui.tabWidget.setCurrentIndex(2);

    def click3(self, event):
        self.ui.tabWidget.setCurrentIndex(0);





    def tableView_init(self):
        self.model_cols=4
        # 创建一个 0行3列 的标准模型
        self.model = QStandardItemModel(0, self.model_cols)
        # 设置表头标签
        self.model.setHorizontalHeaderLabels(self.tablehead)
 
        # 创建 tableView 组件
        #self.tableview = QTableView()
        # 将 tableView 添加到垂直盒布局里
        #self.vboxlayout.addWidget(self.tableview)
 
        # tableView 组件 设置模型
        self.tableview.setModel(self.model)
 
        self.tableview.horizontalHeader().setSectionResizeMode(QHeaderView.Stretch)  # 所有列自动拉伸，充满界面
        self.tableview.setSelectionMode(QAbstractItemView.SingleSelection)  # 设置只能选中整行
        self.tableview.setEditTriggers(QTableView.NoEditTriggers)  # 不可编辑
        self.tableview.setSelectionBehavior(QAbstractItemView.SelectRows)  # 设置只能选中一行
 
        # self.tableView.setSelectionBehavior(QAbstractItemView.SelectRows)  # 设置只能选中整行
        # self.tableView.setSelectionMode(QAbstractItemView.ExtendedSelection)  # 设置只能选中多行
        self.ui.pushButton_del.clicked.connect(self.tableView_del_low)
 
    def tableView_add(self,flg,lon,lat,alt):

        # 写全
        item1 = QStandardItem('%s' % flg)
        item2 = QStandardItem('%s' % lon)
        item3 = QStandardItem('%s' % lat)
        item4 = QStandardItem('%s' % alt)

        self.model.appendRow([item1, item2, item3, item4])
        self.ui.label_desc.setText("记录数据："+str([flg,lon,lat,alt]))
        print('record ')
 
    def tableView_del_low(self):
        index = self.tableview.currentIndex()  # 取得当前选中行的index
        self.model.removeRow(index.row())  # 通过index的row()操作得到行数进行删除
 
    def tableView_clear(self):
        # 会全部清空，包括那个标准表头
        self.model.clear()
        # 所以重新设置标准表头 自己将一下代码注释 尝试
        self.model.setHorizontalHeaderLabels(self.tablehead)


if __name__ == '__main__':

    app = QApplication(sys.argv)
    w = UItest()
    w.show()
    sys.exit(app.exec_())

ui

<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
 <class>MainWindow</class>
 <widget class="QMainWindow" name="MainWindow">
  <property name="geometry">
   <rect>
    <x>0</x>
    <y>0</y>
    <width>807</width>
    <height>600</height>
   </rect>
  </property>
  <property name="windowTitle">
   <string>MainWindow</string>
  </property>
  <widget class="QWidget" name="centralwidget">
   <widget class="QTabWidget" name="tabWidget">
    <property name="geometry">
     <rect>
      <x>10</x>
      <y>10</y>
      <width>781</width>
      <height>481</height>
     </rect>
    </property>
    <property name="currentIndex">
     <number>0</number>
    </property>
    <widget class="QWidget" name="tab_3">
     <attribute name="title">
      <string>首页</string>
     </attribute>
     <widget class="QPushButton" name="pushButton_3">
      <property name="geometry">
       <rect>
        <x>0</x>
        <y>390</y>
        <width>89</width>
        <height>25</height>
       </rect>
      </property>
      <property name="text">
       <string>首页</string>
      </property>
     </widget>
     <widget class="QLabel" name="label">
      <property name="geometry">
       <rect>
        <x>230</x>
        <y>130</y>
        <width>301</width>
        <height>61</height>
       </rect>
      </property>
      <property name="font">
       <font>
        <pointsize>24</pointsize>
       </font>
      </property>
      <property name="text">
       <string>经纬度数据采集工具</string>
      </property>
     </widget>
     <widget class="QLineEdit" name="lineEdit_filename">
      <property name="geometry">
       <rect>
        <x>340</x>
        <y>250</y>
        <width>201</width>
        <height>25</height>
       </rect>
      </property>
     </widget>
     <widget class="QLabel" name="label_6">
      <property name="geometry">
       <rect>
        <x>230</x>
        <y>250</y>
        <width>111</width>
        <height>17</height>
       </rect>
      </property>
      <property name="text">
       <string>保存文件名称：</string>
      </property>
     </widget>
    </widget>
    <widget class="QWidget" name="tab_2">
     <attribute name="title">
      <string>采集</string>
     </attribute>
     <widget class="QPushButton" name="pushButton">
      <property name="geometry">
       <rect>
        <x>10</x>
        <y>330</y>
        <width>89</width>
        <height>25</height>
       </rect>
      </property>
      <property name="text">
       <string>下一页</string>
      </property>
     </widget>
     <widget class="QPushButton" name="pushButton_record">
      <property name="geometry">
       <rect>
        <x>240</x>
        <y>210</y>
        <width>89</width>
        <height>25</height>
       </rect>
      </property>
      <property name="text">
       <string>记录</string>
      </property>
     </widget>
     <widget class="QLabel" name="label_desc">
      <property name="geometry">
       <rect>
        <x>120</x>
        <y>280</y>
        <width>561</width>
        <height>111</height>
       </rect>
      </property>
      <property name="text">
       <string>描述</string>
      </property>
     </widget>
     <widget class="QLabel" name="label_2">
      <property name="geometry">
       <rect>
        <x>40</x>
        <y>30</y>
        <width>101</width>
        <height>16</height>
       </rect>
      </property>
      <property name="text">
       <string>经度：</string>
      </property>
     </widget>
     <widget class="QLabel" name="label_3">
      <property name="geometry">
       <rect>
        <x>40</x>
        <y>60</y>
        <width>101</width>
        <height>16</height>
       </rect>
      </property>
      <property name="text">
       <string>纬度：</string>
      </property>
     </widget>
     <widget class="QLabel" name="label_4">
      <property name="geometry">
       <rect>
        <x>40</x>
        <y>90</y>
        <width>101</width>
        <height>16</height>
       </rect>
      </property>
      <property name="text">
       <string>海拔：</string>
      </property>
     </widget>
     <widget class="QLineEdit" name="lineEdit_lon">
      <property name="geometry">
       <rect>
        <x>130</x>
        <y>30</y>
        <width>201</width>
        <height>25</height>
       </rect>
      </property>
      <property name="text">
       <string>0</string>
      </property>
     </widget>
     <widget class="QLineEdit" name="lineEdit_lat">
      <property name="geometry">
       <rect>
        <x>130</x>
        <y>60</y>
        <width>201</width>
        <height>25</height>
       </rect>
      </property>
      <property name="text">
       <string>0</string>
      </property>
     </widget>
     <widget class="QLineEdit" name="lineEdit_alt">
      <property name="geometry">
       <rect>
        <x>130</x>
        <y>90</y>
        <width>201</width>
        <height>25</height>
       </rect>
      </property>
      <property name="text">
       <string>0</string>
      </property>
     </widget>
     <widget class="QLineEdit" name="lineEdit_flag">
      <property name="geometry">
       <rect>
        <x>130</x>
        <y>170</y>
        <width>201</width>
        <height>25</height>
       </rect>
      </property>
      <property name="text">
       <string/>
      </property>
     </widget>
     <widget class="QLabel" name="label_5">
      <property name="geometry">
       <rect>
        <x>40</x>
        <y>170</y>
        <width>101</width>
        <height>16</height>
       </rect>
      </property>
      <property name="text">
       <string>标记：</string>
      </property>
     </widget>
    </widget>
    <widget class="QWidget" name="tab">
     <attribute name="title">
      <string>数据</string>
     </attribute>
     <widget class="QPushButton" name="pushButton_2">
      <property name="geometry">
       <rect>
        <x>0</x>
        <y>420</y>
        <width>89</width>
        <height>25</height>
       </rect>
      </property>
      <property name="text">
       <string>下一页</string>
      </property>
     </widget>
     <widget class="QTableView" name="tableView">
      <property name="geometry">
       <rect>
        <x>0</x>
        <y>10</y>
        <width>771</width>
        <height>351</height>
       </rect>
      </property>
     </widget>
     <widget class="QPushButton" name="pushButton_del">
      <property name="geometry">
       <rect>
        <x>0</x>
        <y>380</y>
        <width>89</width>
        <height>25</height>
       </rect>
      </property>
      <property name="text">
       <string>删除</string>
      </property>
     </widget>
     <widget class="QPushButton" name="pushButton_save">
      <property name="geometry">
       <rect>
        <x>530</x>
        <y>380</y>
        <width>89</width>
        <height>25</height>
       </rect>
      </property>
      <property name="text">
       <string>保存</string>
      </property>
     </widget>
     <widget class="QLabel" name="label_desc_data">
      <property name="geometry">
       <rect>
        <x>240</x>
        <y>410</y>
        <width>461</width>
        <height>31</height>
       </rect>
      </property>
      <property name="text">
       <string>描述</string>
      </property>
     </widget>
    </widget>
   </widget>
  </widget>
 </widget>
 <resources/>
 <connections/>
</ui>