无人驾驶仿真（四）：基于simulink的windows与linux的UDP通讯模块搭建

最新推荐文章于 2024-06-26 21:57:52 发布

奔跑的林小川

最新推荐文章于 2024-06-26 21:57:52 发布

阅读量5.2k

点赞数 1

文章标签： ROS 无人驾驶仿真 linux windows simulink

本文链接：https://blog.csdn.net/qq_31239495/article/details/87274839

版权

1. 为什么使用UDP通讯？

在https://blog.csdn.net/qq_31239495/article/details/86609605中已经阐述了UDP的优点，其实还有一个原因是我没有实现直接使用simulink下的ROS工具箱实现不同平台的数据通讯，说来惭愧。

2. 需要准备什么？

matlab，带有ros工具箱模块的版本，windows系统电脑

linux操作系统电脑，ros1.0

数据通讯配置：https://blog.csdn.net/qq_31239495/article/details/86704679 以及 https://blog.csdn.net/qq_31239495/article/details/86704679 有详细介绍

3. simulink端口配置情况

simulink指导：* 政豪，西门子软件培训师

A：UDP send，指的是在simulink端，也就是windows下发送消息

B：UDP Recieve，指的是linux端，收到的linux端的消息

配置分别如下：

发送端：

接收端：

4. linux下配置情况：

基于ROS，如果ROS不懂的话，请查看古月居的博客http://www.guyuehome.com/，对ROS有个初步了解。

创建一个工作空间，包的名称为UDP_Test.（可以创建任何名称，我这里以我自己的代码为主），代码指提供了c++的代码，在cmakelist 等文件的配置没有详细给出，直接复制代码无法使用，请悉知。

代码贡献者：* 宇超，计算机视觉算法工程师

server端：

#include "ros/ros.h"
#include "std_msgs/String.h"

#include <sstream>

#include <cstdio>   
#include <sys/types.h>   
#include <sys/socket.h>   
#include <netinet/in.h>   
#include <unistd.h>   
#include <cerrno>   
#include <string>   
#include <cstdlib>

#define SERV_PORT   8000 //此处修改成自己的端口

int main(int argc, char *argv[])
{
	ros::init(argc, argv, "server");

	ros::NodeHandle n;

	ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);

	ros::Rate loop_rate(10);

	// udp server init
	/* sock_fd --- socket鏂囦欢鎻忚堪绗?鍒涘缓udp濂楁帴瀛?/  
  	int sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
  	if(sock_fd < 0)  
  	{  
    	perror("socket");  
   		exit(1);  
  	}
	
	/* 灏嗗鎺ュ瓧鍜孖P銆佺鍙ｇ粦瀹?*/  
	struct sockaddr_in addr_serv;  
	int len;  
	memset(&addr_serv, 0, sizeof(struct sockaddr_in));  //姣忎釜瀛楄妭閮界敤0濉厖
	addr_serv.sin_family = AF_INET;                     //浣跨敤IPV4鍦板潃
	addr_serv.sin_port = htons(SERV_PORT);              //绔彛
	/* INADDR_ANY琛ㄧず涓嶇鏄摢涓綉鍗℃帴鏀跺埌鏁版嵁锛屽彧瑕佺洰鐨勭鍙ｆ槸SERV_PORT锛屽氨浼氳璇ュ簲鐢ㄧ▼搴忔帴鏀跺埌 */  
	addr_serv.sin_addr.s_addr = htonl(INADDR_ANY);  //鑷姩鑾峰彇IP鍦板潃
	len = sizeof(addr_serv);

	/* 缁戝畾socket */  
	if(bind(sock_fd, (struct sockaddr *)&addr_serv, sizeof(addr_serv)) < 0)  
	{  
		perror("bind error:");  
		exit(1);  
	}

	int count = 0;
	int recv_num = -1;
	char recv_buf[20];
	struct sockaddr_in addr_client;

	while (ros::ok())
	{
		printf("server wait:\n");  
		
    	recv_num = recvfrom(sock_fd, recv_buf, sizeof(recv_buf), 0, (struct sockaddr*)&addr_client, (socklen_t*)&len);
		if(recv_num < 0)  
		{  
			perror("recvfrom error:");  
			exit(1);  
		} 

		recv_buf[recv_num] = '\0';

		std_msgs::String msg;

		std::stringstream ss;

		ss << recv_buf << count;
		msg.data = ss.str();

		ROS_INFO("%s", msg.data.c_str());

		chatter_pub.publish(msg);

		ros::spinOnce();

		loop_rate.sleep();
		++count;
	}

	return 0;
}

client端：

#include <cstdio>   
#include <sys/types.h>   
#include <sys/socket.h>   
#include <netinet/in.h>  
#include <arpa/inet.h> 
#include <unistd.h>   
#include <cerrno>   
#include <string>   
#include <cstdlib>

#include <ros/ros.h>
#include <std_msgs/String.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/time_synchronizer.h>

using namespace message_filters;
using namespace std;

#define DEST_PORT 9000
#define DSET_IP_ADDRESS  "127.0.0.1"

class SubAndPub {
public:
	SubAndPub() {
		cout << "SubAndPub" << endl;

		this->socket_fd = socket(AF_INET, SOCK_DGRAM, 0);
		if(this->socket_fd < 0) {
    		perror("socket");  
    		exit(1);  
  		}
		
		/* 璁剧疆address */ 
		memset(&this->addr_serv, 0, sizeof(this->addr_serv));  
		this->addr_serv.sin_family = AF_INET;  
		this->addr_serv.sin_addr.s_addr = inet_addr(DSET_IP_ADDRESS);  
		this->addr_serv.sin_port = htons(DEST_PORT);  
		this->len = sizeof(this->addr_serv);

		pub = n.advertise<std_msgs::String>("receive_pub", 1000);
	}
	
	static void callback(const SubAndPub& fusion, const std_msgs::String::ConstPtr& msg) {	
		cout << "client receive " << msg->data << endl;
		
		char send_buf[20] = "receive server"; 

		int send_num = sendto(fusion.socket_fd, send_buf, strlen(send_buf), 0, (struct sockaddr*)&fusion.addr_serv, fusion.len);

		cout << errno << endl;

    	// pub_msg.data = ss.str();  

		// fusion.pub.publish(pub_msg);
	}
	
	~SubAndPub() {
		cout << "~SubAndPub" << endl;
		// close(this->socket_fd);
	}

private:	
	ros::NodeHandle n;
	ros::Publisher pub;

	int socket_fd;
	struct sockaddr_in addr_serv;
	int len;

};

int main(int argc, char *argv[]) {
	ros::init(argc, argv, "client");

	ros::NodeHandle n;

	SubAndPub udp_client;

	message_filters::Subscriber<std_msgs::String> msg(n, "chatter", 1);
	msg.registerCallback(boost::bind(&SubAndPub::callback, udp_client, _1));

	ros::spin();

	return 0;
}