GPS开发（2）GPS ROS串口采集-pubx数据解析 - MKT-porter

随笔- 1524 文章- 2 评论- 66 阅读- 127万

pubx协议数据解析

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(v1_GetGPS)
 
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
 
 
find_package(catkin REQUIRED COMPONENTS
  message_generation
  message_runtime
  roscpp
  rospy
  std_msgs
  serial #串口引用
)
 
add_message_files(
  FILES
  topic_msg.msg#消息引用-添加我们自己定义的xxx.msg文件
)
 
generate_messages(
  DEPENDENCIES
  std_msgs#消息引用-自带
)
 
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES my_topic002
#  CATKIN_DEPENDS message_generation message_runtime roscpp rospy std_msgs
#  DEPENDS system_lib
)
 
###########
## Build ##
###########
 
 
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)
 
# add_executable(publisher_gps src/publisher_gps.cpp)
# target_link_libraries(publisher_gps ${catkin_LIBRARIES})
# add_dependencies(publisher_gps ${PROJECT_NAME}_generate_messages_cpp) 
 
# add_executable(subscriber_gps src/subscriber_gps.cpp)
# target_link_libraries(subscriber_gps ${catkin_LIBRARIES})
# add_dependencies(subscriber_gps ${PROJECT_NAME}_generate_messages_cpp) 
 
 
# add_executable(serialPort src/serialPort.cpp)
# target_link_libraries(serialPort ${catkin_LIBRARIES})
# add_dependencies(serialPort ${PROJECT_NAME}_generate_messages_cpp) 
 
# add_executable(listener src/listener.cpp)
# target_link_libraries(listener ${catkin_LIBRARIES})
# add_dependencies(listener ${PROJECT_NAME}_generate_messages_cpp) 
 
 
 
add_executable(serialPort_saveTxt src/serialPort_saveTxt_v2.cpp)
target_link_libraries(serialPort_saveTxt ${catkin_LIBRARIES})
add_dependencies(serialPort_saveTxt ${PROJECT_NAME}_generate_messages_cpp) 
 
 
add_executable(GPS_save src/serialPort_saveTxt_v1.cpp)
target_link_libraries(GPS_save ${catkin_LIBRARIES})
add_dependencies(GPS_save ${PROJECT_NAME}_generate_messages_cpp) 

package.xml

<?xml version="1.0"?>
<package format="2">
  <name>v1_GetGPS</name>
  <version>0.0.0</version>
  <description>The v1_GetGPS package</description>
 
  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="dongdong@todo.todo">dongdong</maintainer>
 
 
  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>TODO</license>
 
  <buildtool_depend>catkin</buildtool_depend>
   
  <build_depend>message_generation</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>serial</build_depend>
  <build_depend>std_msgs</build_depend>
 
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>serial</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
 
  <exec_depend>message_runtime</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>serial</exec_depend>
  <exec_depend>std_msgs</exec_depend>
 
 
  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->
 
  </export>
</package>

msg/topic_msg.msg

bool bool_msg
float64 lat
float64 lon
float64 high
string string_msg
time time_msg

src/serialPort_saveTxt_v2.cpp

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

#include <string>
#include <vector>
#include <sstream>
#include <cmath>
#include <cstdlib>//string转化为double
#include <iomanip>//保留有效小数
  
#include <termio.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
 
//txt保存使用
#include <fstream>
#include <iostream>
  
//ros依赖
#include <ros/ros.h>
#include <serial/serial.h>  //ROS已经内置了的串口包
  
#include "ros/time.h"// 时间戳
  
//0 导入消息 工程名字/消息名字
#include "v1_GetGPS/topic_msg.h"
#include "std_msgs/String.h"
#include <std_msgs/Empty.h>
#include <ctime>
 
 
serial::Serial ser; //声明串口对象
using namespace std;
  
 
 
#define TTY_PATH            "/dev/tty"
#define STTY_US             "stty raw -echo -F "
#define STTY_DEF            "stty -raw echo -F "
  
int get_char();
  
int get_char()
{
    fd_set rfds;
    struct timeval tv;
    int ch = 0;
  
    FD_ZERO(&rfds);
    FD_SET(0, &rfds);
    tv.tv_sec = 0;
    tv.tv_usec = 10; //设置等待超时时间
  
    //检测键盘是否有输入
    if (select(1, &rfds, NULL, NULL, &tv) > 0){
        ch = getchar(); 
    }
    return ch;
}
 
 
 
//解析GPS
// 0-PUBX 1-00 2-timesharp 3-lat 4-N 5-lon 6-E 7-high 8-G3 9-Hacc 10-Vacc
void RecePro(std::string s , double& lat , double& lon, double& high,double& hacc ,double& vacc ,double& state  )
{
    //分割有效数据，存入vector中
    std::vector<std::string> v;
    std::string::size_type pos1, pos2;
    pos2 = s.find(",");
    pos1 = 0;
    while ( std::string::npos !=pos2 )
    {
        v.push_back( s.substr( pos1, pos2-pos1 ) );
        pos1 = pos2 + 1;
        pos2 = s.find(",",pos1);
    }
    if ( pos1 != s.length() )
        v.push_back( s.substr( pos1 ));
    //解析出经纬度
    //字段6：GPS状态，0=未定位，1=DeadReckonimgslolution，2=标准独立2D，3=标准独立3D，4=差分2D 5-差分3D 6-联合GPS和DeadReckonimgslolution
    if (v.max_size() >= 11  )
    {
         
        //纬度
        if (v[3] != "") lat = std::atof(v[3].c_str()) / 100;
        int ilat = (int)floor(lat) % 100;
        lat = ilat + (lat - ilat) * 100 / 60;
        //经度
        if (v[5] != "") lon = std::atof(v[5].c_str()) / 100;
        int ilon = (int)floor(lon) % 1000;
        lon = ilon + (lon - ilon) * 100 / 60;
        //海拔高度
        if (v[7] != "") high = std::atof(v[7].c_str()) ;
 
        //GPS状态  G3有数据精度还可以
        //if (v[8] == "G3" || (v[8] == "D3") {state=1;}
        //else{state=0;}
 
        state=1; //解析正常
 
        //水平误差 m
        if (v[9] != "") hacc=std::atof(v[9].c_str()) ;
        // 垂直误差 m
        if (v[10] != "") vacc=std::atof(v[10].c_str()) ;
         
         
  
    }
    else{
 
        state=0;
    }
    
}
  
  
//1 txt文件名名字 时间戳
//2 ROS发送 开关参数化
//3 加入hacc 和  vacc
  
int main(int argc, char** argv)
{  
    bool sendRos=1;
    double hacc_range=5; //水平误差阈值
    double vacc_range=5; //垂直误差阈值
 
    string save_name=argv[1];
    cout<< save_name <<endl;
    //获取当前时间保持到字符串中
    time_t t = time(NULL);
    struct tm local_tm;
    struct tm *tm = localtime_r(&t, &local_tm);
    char cNowTime[64];
    sprintf(cNowTime, "%02d-%02d-%02d-%02d-%02d-%02d",
            tm->tm_year + 1900,tm->tm_mon + 1,tm->tm_mday, 
           tm->tm_hour, tm->tm_min, tm->tm_sec);
 
    //保存TXT使用
    string  outFileName = save_name + cNowTime +".txt";
    //ofstream outfile(outFileName, ios::app); //ios::app指追加写入
    ofstream outfile(outFileName); //覆盖方式写入
    if (!outfile.is_open())
    {
        cerr << "Error: can not find file " << outFileName << endl;
        return 0;
    }
 
 
 
    //初始化节点
    ros::init(argc, argv, "serial_node");
    //声明节点句柄
    ros::NodeHandle nh;
    //注册Publisher到话题GPS
    ros::Publisher GPS_pub = nh.advertise<v1_GetGPS::topic_msg>("GPS",1000);
    try
    {
      //串口设置
      ser.setPort("/dev/ttyACM0");
      ser.setBaudrate(115200);
      serial::Timeout to = serial::Timeout::simpleTimeout(5000);
      ser.setTimeout(to);
      ser.open();
    }
    catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open Serial Port !");
        return -1;
    }
    if (ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port initialized");
    }
    else
    {
        return -1;
    }
  
    ros::Rate loop_rate(20);
 
    std::string strRece;
     
    //捕获按键
    int ch = 0;
    system(STTY_US TTY_PATH);
 
 
    while (ros::ok())
    {
         
       
         //捕获按键
        ch = get_char();
        if(ch != 0){
            printf("%d\n\r",ch);
             if(ch == 27){
                 outfile.close();// txt文件关闭
                 std::cout<< "采集程序关闭" <<std::endl;
                 break;
             }
        }
        if(ch == 3){
            system(STTY_DEF TTY_PATH);
            return 0;
        }  
 
 
        if (ser.available())
        {
  
    
  
            //1.读取串口信息：
            //ROS_INFO_STREAM("Reading from serial port\n");
            //通过ROS串口对象读取串口信息
            //std::cout << ser.available();
            //std::cout << ser.read(ser.available());
            strRece += ser.read(ser.available());
            //std::cout<< "1============"<<strRece << std::endl;
            //std::cout <<"strRece:" << strRece << "\n" ;
            //strRece = "$GNGGA,122020.70,3908.17943107,N,11715.45190423,E,1,17,1.5,19.497,M,-8.620,M,,*54\r\n";
            //2.截取数据、解析数据：
            //GPS起始标志
            std::string gstart = "$PU";
            //GPS终止标志
            std::string gend = "\r\n";
            int i = 0, start = -1, end = -1;
              
            while ( i < strRece.length() )
            {
                //找起始标志
  
                start = strRece.find(gstart);
                //如果没找到，丢弃这部分数据，但要留下最后2位,避免遗漏掉起始标志
                if ( start == -1)
                {
                    if (strRece.length() > 2)  
                        strRece = strRece.substr(strRece.length()-3);
                        break;
  
                }
                //如果找到了起始标志，开始找终止标志
                else
                {
                    //找终止标志
                    end = strRece.find(gend);
                    //如果没找到，把起始标志开始的数据留下，前面的数据丢弃，然后跳出循环
                    if (end == -1)
                    {
                        if (end != 0)
                        strRece = strRece.substr(start);
                        break;
                    }
                    //如果找到了终止标志，把这段有效的数据剪切给解析的函数，剩下的继续开始寻找
                    else
                    {
                        i = end;
  
                        //ros::Time begin = ros::Time::now();
                        double secs = ros::Time::now().toSec();
  
                        //把有效的数据给解析的函数以获取经纬度
                        double lat, lon, high,hacc,vacc,state;
                         
                        //std::cout<< "2==========="<<strRece.substr(start,end+2-start) << std::endl;
                        RecePro(strRece.substr(start,end+2-start),lat,lon,high,hacc,vacc,state);
                        //保留位数7位
                        std::cout << std::setiosflags(std::ios::fixed) <<std::setprecision(7)<< "   时间:  " << secs << "  更新状态： "<< state <<"  纬度：" << lat << " 经度："<< lon <<" 高度： "<< high << "  垂直误差: "<< hacc << "  水平误差: " <<vacc << "\n";
                        //状态1 且垂直和水平误差小于x米 保存
                        if(state==1 && hacc<hacc_range && vacc<vacc_range){
                            outfile  << std::setiosflags(std::ios::fixed) << std::setprecision(8) << secs << " "<<lat <<" "<< lon <<" " << high<<" "<< hacc << " "<< vacc<< " "<< std::endl;//写TXT文件
                            if(sendRos){
                                //发布消息到话题
                                v1_GetGPS::topic_msg GPS_data;
                                GPS_data.lat = lat;
                                GPS_data.lon = lon;
                                GPS_data.high = high;
                                GPS_pub.publish(GPS_data);
                            }
 
                        }
 
                        //如果剩下的字符大于等于4，则继续循环寻找有效数据,如果所剩字符小于等于3则跳出循环
                        if ( i+5 < strRece.length())
                            strRece = strRece.substr(end+2);
                        else
                        {   strRece = strRece.substr(end+2);
                            break;
                        }
                    }
                }
            }
        }
 
      if(sendRos){
        ros::spinOnce();
        loop_rate.sleep();
      }
    }//while
  
    outfile.close();// txt文件关闭
    std::cout<< "采集程序关闭" <<std::endl;
    return 1;
}

　　subscriber_gps.cpp

#include <cstdlib>
#include <iostream>
#include <string>
#include <cstring>
 
//0 导入ros依赖
#include <ros/ros.h>
 
//0 导入消息 工程名字/消息名字
#include "v1_GetGPS/topic_msg.h"
#include <std_msgs/String.h>
 
 
// 接收到订阅的消息后，会进入消息回调函数
void test_topic_cb(const v1_GetGPS::topic_msg::ConstPtr & msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("------------------ msg ---------------------");
    ROS_INFO("bool_msg:    [%d]", msg->bool_msg);
    ROS_INFO("string_msg:  [%s]", msg->string_msg.c_str());
    ROS_INFO("float32_msg: [%f]", msg->lat);
    ROS_INFO("float64_msg: [%f]", msg->lon);
    ROS_INFO("time_msg:    [%d.%d]", msg->time_msg.sec, msg->time_msg.nsec);
}
 
 
int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "subscriber_gps");
 
    // 创建节点句柄
    ros::NodeHandle n;
 
    // 创建一个Subscriber，订阅名为my_topic_msg的消息，注册回调函数test_topic_cb
    ros::Subscriber sub_topic = n.subscribe<v1_GetGPS::topic_msg>("my_topic_msg", 100, test_topic_cb);
 
    // 循环等待回调函数
    ros::spin();
 
    return 0;
}

ros_run_gps.sh

#!/bin/sh
 
 
#延迟1秒执行
sleep 1
   
echo "ROS——GPS 测试"
#echo "GPS 测试开始，消息记录到日志里" > /home/pi/start/test_desk1.log
 
 
echo "1 开启ros节点 roscore 等待完全开启再往后执行 "
gnome-terminal -t "1_roscore" -x bash -c "\
roscore; \
exit;exec bash;"
sleep 5
 
# /dev/ttyACM0
# /dev/ttyUSB0
echo "2 开启发送节点 serialPort "
gnome-terminal -t "2_serialPort" -x bash -c "\
echo \"dongdong\" | sudo chmod 777 /dev/ttyACM0; \ 
source /home/dongdong/v2_Project/v4_ROS/catkin_gps/devel/setup.bash; \
rosrun v1_GetGPS serialPort_saveTxt savatxtname; \
exec bash;"
sleep 1
 
echo "3 开启接收节点 listener"
 
gnome-terminal -t "3_listener" -x bash -c "\   
source /home/dongdong/v2_Project/v4_ROS/catkin_gps/devel/setup.bash; \
rosrun v1_GetGPS listener; \
exit;exec bash;"
sleep 1
 
 
#echo "执行前确保给与脚本本身执行权限 sudo chmod -R 777 xxx.sh"
#echo "查看串口： ls /dev/ttyUSB* "
#echo "临时给与一次串口权限： sudo chmod 777 /dev/ttyUSB0 "
# 开启新的命令窗口执行
# gnome-terminal -t "窗口名字" -x bash -c "要执行的命令1；命令2；exit;exec bash;"
#Shell 脚本自动输入密码 : echo "密码" | sudo 命令