组合导航在ROS中的解析（2）

1. 工作环境
   ubuntu18.02，组合导航使用网口接口，ros使用melodic
2. 实现过程

#include <ros/ros.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <fcntl.h>
#include <sys/shm.h>
#include<get_ins/ins.h>




#define    MYPORT     5017   //端口号
#define    BUF_SIZE   1000   //数据缓冲区最大长度

char* SERVER_IP = "192.168.0.200";
int result = 0;
 
using namespace std;

int main(int argc, char **argv)
{

	char recvbuf[BUF_SIZE];


	
	/*
	 *@fuc: socket()创建套节字
	 *
	 */
	int socket_cli = socket(AF_INET, SOCK_STREAM, 0);
	if(socket_cli < 0)
	{
		std::cout << "socket() error\n";
		return -1;
	}
	
	/*
	 *@fuc: 服务器端IP4地址信息,struct关键字可不写
	 *@fuc: 初始化sever地址信息   
	 */
	struct sockaddr_in sev_addr;  
	memset(&sev_addr, 0, sizeof(sev_addr));//清空之前的服务端数据
	sev_addr.sin_family      = AF_INET;
	sev_addr.sin_port        = htons(MYPORT);
	sev_addr.sin_addr.s_addr = inet_addr(SERVER_IP);
	std::cout << "connecting..." << std::endl;
	/*
	 *@fuc: 使用connect()函数来配置套节字,建立一个与TCP服务器的连接
	 */
	if(connect(socket_cli, (struct sockaddr*) &sev_addr, sizeof(sev_addr)) < 0)
	{ 
		std::cout << "connect error" << std::endl;
		return -1;
	}
	else
		std::cout << "connected successfullly!" << std::endl;

	ros::init(argc, argv, "client_node");
	ros::NodeHandle n;
    ros::Publisher pub_ins=n.advertise<get_ins::ins>("ins_date",1000);
	get_ins::ins data;
    ros::Rate rate(5);
    
	while(ros::ok)
	{
		/*
		 *@fuc: 使用recv()函数来接收服务器发送的消息
		 */
		recv(socket_cli, recvbuf, sizeof(recvbuf), 0);

		//下面是对关键信号的解析，利用分割字符串strtokk函数多次分割
		char *delim=",";
		strtok(recvbuf,delim);
		for(int i=0;i<12;i++)
		{
			strtok(NULL,delim);
		}
		data.latitude=atof(strtok(NULL,delim));
		strtok(NULL,delim);
		data.longitude=atof(strtok(NULL,delim));
		
        for(int i=0;i<12;i++)
		{
			strtok(NULL,delim);
		}
        data.velocity=atof(strtok(NULL,delim));
		
        for(int i=0;i<12;i++)
		{
			strtok(NULL,delim);
		}
        data.heading_angle=atof(strtok(NULL,delim));
		
		//ROS_INFO("server message: %s\n", strtok(NULL,delim));
        pub_ins.publish(data);

        rate.sleep();
        ros::spinOnce();
	}
	/*
	 *@fuc: 关闭连接
	 */

	close(socket_cli);
	return 0;
}

在while（ros：：ok（））之前的函数是参考ros下socket的编程的代码，由于本次方案中将组合导航的输入作为服务端，所以ros中只需要启动客户端来与之建立通信，并将得到的关键信息进行话题打包。注意！！！在打包之前要把对应的数据类型转化好。订阅方实现如下：

#include<ros/ros.h>
#include<get_ins/ins.h>

void callback(const get_ins::ins::ConstPtr& data)
{
    ROS_INFO("纬度为：%.3f,经度为：%.3f，速度为：%.2f,\
    航向角为%.2f",data->latitude,data->longitude,data->velocity,data->heading_angle);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"rec_ins");
    ros::NodeHandle nh;
    ros::Subscriber sub=nh.subscribe<get_ins::ins>("ins_data",1000,callback);
    
    ros::spin();
    return 0;
}

这样ros中即可获得消息（对于在matlab开发算法而言，这步可以不需要，直接在matlab中订阅即可。）

3. 个人体会
   遇到什么核心已转储这种问题，可能是你的输出控制符%d,%s等等对应错误。
   socket的原理可以参考什么是socket。
   在matlab上好像有解析NEMA的模块，未来可以尝试，是通过模块解析还是利用循环裁剪字符串。