- 工作环境
ubuntu18.02,组合导航使用网口接口,ros使用melodic - 实现过程
#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中订阅即可。)
- 个人体会
遇到什么核心已转储这种问题,可能是你的输出控制符%d,%s等等对应错误。
socket的原理可以参考什么是socket。
在matlab上好像有解析NEMA的模块,未来可以尝试,是通过模块解析还是利用循环裁剪字符串。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)