基于共享内存的ros系统优化

最近导师给的科研任务，搭建大规模的无人机仿真平台。考虑到需要仿真的真实性，选了ros和gazebo作为仿真平台的搭建。但是ros有点坑，因为master和ros的通信机制是基于socket的，超过10架无人机就会出现延迟现象。。最近看了进程间通信的几种方法，就选择了共享内存作为单个无人机内部的通信方式，效果很好，但是关于进程间几种通信对比以后有空再写吧，下面先介绍一下本次实现的模型。

以一个writer和多个reader为例子介绍：

从图中我们可以看到需要通过一定的互斥完成几个进程之间的交互，但是对于加锁而言，当一个writer写的reader不能读，这样就白白浪费了时间。所以我们可以使用循环队列代替锁机制，这样保证writer reader都能访问这片内存，writer一直写，直到写满了，reader一直读，读一个就移动一下指针，注意，对于我们这种消息传递机制，每个reader只需要实时读到消息即可，不用在意历史信息。所以所有的reader只需要跟着一个读的指针读即可，读指针往前移动一次，就空出一个空位留待写者去写。共享内存的结构设计如下：

在这篇内存前四位保留存入共享信息，从array开始是共享内存的存储数据区域，注意需要比申请的存储长度保留多一位方便判断循环队列是否为满。

队列空时：

空的时候很好判断，两个指针相遇就表明循环队列是空的。稍复杂的是队列满的情况，在看队列是满的情况我们先来看一下，队列存入的情况是什么样的：

存入一个数据，写指针就后移一位，那么判断指针是否是满的情况就很清楚了：

队列满的情况：

当写指针的下一位是读指针时候就停下，等待读指针移动再写，比如我们申请的队列长度是5，在真正申请的时候我们空出一位留作判断队列是否为满，如图写指针目前是5，读指针在0的位置上，(5 + 1) % 6 == 0,所以停下写指针。
不多bibi，上代码：
shm.cpp 用于建立写者与读者的模板

#include <stdio.h>
#include <unistd.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <sys/types.h>
#include <stdlib.h>
using namespace std;

struct Cordintin{
	int x;
	int y;
	int z;
	int w;
	Cordintin(int xval, int yval, int zval, int wval) : 
		x(xval), y(yval), z(zval), w(wval) {
	}
};

template <class T>
class wriShm{
public:
	wriShm (int number, int proj_id);
	virtual ~wriShm();
	bool write(const T &i);

private:
	int wfd;
	void *ptr;
	int *hasred, *size, *pwri, *pred;
	T *array;
};

template <class T>
wriShm<T>::wriShm(int number, int proj_id) {
	key_t key = ftok("./", proj_id);
	if (key == -1) {
		perror("ftok falure:");
		exit(1);
	}
	this->wfd = shmget(key, (16+sizeof(T)*(number + 1)), IPC_CREAT|0666);
	if (wfd == -1) {
		perror("shmget falure\n");
		exit(1);
	}
	this->ptr = shmat(wfd, 0, 0);
	if(ptr == (void *)(-1)) {
		perror("shmat falure\n");
		exit(1);
	}
	this->hasred = (int *) ptr;
	*hasred = 0;
	this->pwri = hasred + 1; this->size = hasred + 2;
	this->pred = hasred + 3; this->array = (Cordintin*)(hasred + 4);
	*size = number; *pwri = 0; *pred = 0;
	printf("shm built succeed\n");
}

template <class T>
wriShm<T>::~wriShm() {
	if (shmdt(ptr) == -1) {
		perror("shmdt shm falure\n");
		exit(1);
	}
	if (shmctl(wfd, IPC_RMID, NULL) == -1) {
		perror("shmctl falure\n");
		exit(1);
	}
	printf("shm destroyed\n");
}

template <class T>
bool wriShm<T>::write(const T &i) {
	if ((*pwri + 1)% ((*size) + 1) != *pred) {
		array[*pwri] = i;
		*pwri = ((*pwri) + 1) % ((*size) + 1);
		return true;
	}
	else {
		return false;
	}
}

template <class T>
class redShm{
public:
	redShm (int proj_id);
	virtual ~redShm();
	bool readmove(T *redret);
	bool read(T *redret);
	int hasr();
	int hasr(int i);
	int length();
private:
	int rfd;
	void *ptr;
	int *hasred, *size, *pwri, *pred;
	T *array;
};

template <class T>
redShm<T>::redShm(int proj_id) {
	key_t key = ftok("./", proj_id);
	if (key == -1) {
		perror("ftok falure:");
		exit(1);
	}
	this->rfd = shmget(key, 0, 0);
	while (this->rfd == -1) {
		perror("waiting for shm building\n");
		this->rfd = shmget(key, 0, 0);
	}
	this->ptr = shmat(rfd, 0, 0);
	if(ptr == (void *)(-1)) {
		perror("read shmat falure\n");
		exit(1);
	}
	this->hasred = (int *) ptr;
	this->pwri = hasred + 1; this->size = hasred + 2;
	this->pred = hasred + 3; this->array = (Cordintin*)(hasred + 4);
	*pwri = 0; *pred = 0;
	printf("shm built succeed\n");
}

template <class T>
redShm<T>::~redShm() {
	if (shmdt(ptr) == -1) {
		perror("read shmdt falure\n");
		exit(1);
	}
	printf("read shmdt succeed\n");
}

template <class T>
int redShm<T>::hasr() {
	return *(this->hasred);
}

template <class T>
int redShm<T>::hasr(int i) {
	*(this->hasred) = i;
}

template <class T>
int redShm<T>::length() {
	return *(this->size);
}

template <class T>
bool redShm<T>::readmove(T *redret) {
	if (*pwri != *pred % ((*size) + 1)) {
		*redret = array[*pred];
		*pred = ((*pred) + 1) % ((*size) + 1);
		return true;
	}
	else {
		return false;
	}
}

template <class T>
bool redShm<T>::read(T *redret) {
	if (*pwri != *pred) {
		*redret = array[*pred];
		return true;
	}
	else {
		return false;
	}
}

writer.cpp

#include "../include/shm.h"
#include <stdio.h>
using namespace std;


int main() {
	Cordintin *cor = new Cordintin(0, 0, 0, 0);
	wriShm<Cordintin> writer(5, 66);
	for (int i = 0; i < 20; i++) {
		cor->x = i; cor->y = i; cor->z = i; cor->w = i;
		if (writer.write(*cor)) {
			printf("write success\n");
		}
		else {
			printf("write failure\n");
		}
		sleep(0.3);
	}

	return 0;
}

reader1.cpp

#include "../include/shm.h"
#include <stdio.h>
using namespace std;

int main () {
	redShm<Cordintin> reader(66);
	Cordintin *redret = new Cordintin(0, 0, 0, 0);
	if (!(reader.hasr())) {
		reader.hasr(1);
		for (int i = 0; i < 20; i++) {
			if (reader.readmove(redret)) {
				printf("****I gona move readpointer**** \n");
				printf("read x result is:%d\n", (redret->x));
				printf("read y result is:%d\n", (redret->y));
				printf("read z result is:%d\n", (redret->z));
				printf("read w result is:%d\n", (redret->w));
			}
			else {
				printf("read failure\n");
			}
			sleep(0.3);
		}
	}
	else {
		for (int i = 0; i < 20; i++) {
			if (reader.read(redret)) {
				printf("read x result is:%d\n", (redret->x));
				printf("read y result is:%d\n", (redret->y));
				printf("read z result is:%d\n", (redret->z));
				printf("read w result is:%d\n", (redret->w));
			}
			else {
				printf("read failure\n");
			}
			sleep(0.3);
		}
	}
	reader.hasr(0);
	return 0;
}

reader2.cpp与reader1.cpp相同
需要注意的是，在同一时间只有一个reader可以挪动读指针，这在程序中可以看出来。
最后贴上Makefile

target = writer reader1 reader2

all: $(target)
.PHONY: all

%: ./obj/%.o ./obj/shm.o
	g++ $^ -o $@

./obj/%.o: ./src/%.cpp
	g++ -c $< -o $@ -l ./include/shm.h

./obj/shm.o: ./shmcpp/shm.cpp
	g++ -c $< -o $@

效果如下：
两个reader:
writer: