ROS的一个网络通信接口源程序 -

#include "header/map_exchange.h"
net_ui::net_ui()
{
conn_id=0;
sock_srv = socket(AF_INET,SOCK_STREAM, 0);
memset(&server_addr, 0, sizeof(server_addr));
server_addr.sin_family = AF_INET;
server_addr.sin_port = htons(12307);
server_addr.sin_addr.s_addr = htonl(INADDR_ANY);
if(bind(sock_srv,(struct sockaddr *)&server_addr,sizeof(server_addr))==-1)
{
perror("Binding:");
exit(1);
}
if(listen(sock_srv,QUEUE) == -1)
{
perror("Listen");
exit(1);
}
//申明所有Publisher
map_pub = n.advertise<nav_msgs::OccupancyGrid>("env_map", 1,true);
//申明所有Subscriber
map_sub = n.subscribe<nav_msgs::OccupancyGrid>("map",1,&net_ui::map_Callback,this);
std_clk = n.subscribe<std_msgs::String>("CORE_clk",1,&net_ui::clk_Callback,this);
server_waitforclient=n.subscribe<std_msgs::Float64>("ui_deadcycle",1,& net_ui::deadcycle_Callback,this);
std_msgs::Float64 id;
ros::Publisher ui_server_pub = n.advertise<std_msgs::Float64>("ui_deadcycle", 1);
ui_server_pub.publish(id);
}
net_ui::~net_ui()
{
close(sock_srv);
}
void net_ui::Connect()
{
if(conn_id>=QUEUE) return;
conn_id++;
printf("正在等待用户连接\n");
conn[conn_id]= accept(sock_srv, (struct sockaddr*)&client_addr, &length);
printf("第%d名用户连接!!!\n",conn_id);
}
void net_ui::deadcycle_Callback(const std_msgs::Float64::ConstPtr& msg)
{
ros::Rate r(10);
while(n.ok())
{
Connect();
r.sleep();
}
}
int net_ui::Download()
{
char recvBuf[16];
int re=recv(conn[conn_id-1],recvBuf ,16,0);
if(recvBuf[0]==0x0a&&recvBuf[15]==0x0d)
{
switch(recvBuf[1])
{
case 0x01:
{
char buf[6];
buf[1]=m_map.info.height/256;buf[2]=m_map.info.height%256;
buf[3]=m_map.info.width/256;buf[4]=m_map.info.width%256;
buf[0]=0xff;buf[5]=0xff;
Upload(buf,6);
break;
}
case 0x02:
{
}
}
}
}
void net_ui::Upload(char *a,int len)
{
send(conn[conn_id-1],a,len,0);
}
void net_ui::map_Callback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
m_map.info.height=msg->info.height;
m_map.info.width=msg->info.width;
m_map.info.resolution=msg->info.resolution;
m_map.info.origin.position.x=m_map.info.width*m_map.info.resolution;
m_map.info.origin.position.y=m_map.info.height*m_map.info.resolution;
double map_time=(msg->info.map_load_time.toSec());
for(int i=0;i<m_map.info.height ;i++)
{
for(int j=0;j<m_map.info.width;j++)
{
if (map_occupy[i][j]<0) map_occupy[i][j]='X';
else map_occupy[i][j]=(msg->data[i*m_map.info.width +j])/10+'0';
}
}
Map_saver();
}
void net_ui::clk_Callback(const std_msgs::String::ConstPtr& msg)
{
}
int net_ui::Map_saver()
{
printf("收到地图长：%d,宽：%d \n",m_map.info.height,m_map.info.width);
printf("等待写入地图...\n");
FILE *fp;
fp = fopen("/home/exbot/AGV/src/cfg/maps/map.map","wb");
if(fp==NULL) {printf("连接磁盘时出现错误！\n");return 1;}
fprintf(fp,"%d %d %f\n",m_map.info.width,m_map.info.height,m_map.info.resolution);
for(int i=0;i<m_map.info.height;i++) fprintf(fp,"%s\n",map_occupy[i]);
fclose(fp);
printf("写入地图完成！\n");
return 0;
}
int net_ui::Map_loader()
{
FILE *fp;
printf("等待读取地图...\n");
fp = fopen("/home/exbot/AGV/src/cfg/maps/map.map","rb");
if(fp==NULL) {printf("连接磁盘时出现错误！\n");return 1;}
fscanf(fp,"%d %d %f\n",&m_map.info.width,&m_map.info.height,&m_map.info.resolution);
printf("磁盘内地图长：%d,宽：%d 分辨率:%f \n",m_map.info.height,m_map.info.width,m_map.info.resolution);
m_map.info.origin.position.x = 0.0;
m_map.info.origin.position.y = 0.0;
m_map.header.frame_id = "map";
m_map.info.map_load_time = ros::Time::now();
m_map.header.stamp = ros::Time::now();
for(int i=0;i<m_map.info.height;i++)
{
fscanf(fp,"%s",map_occupy[i]);
}
for(int i=0;i<m_map.info.height ;i++)
{
for(int j=0;j<m_map.info.width;j++)
{
if(map_occupy[i][j]=='X') map_occupy[i][j]=-1;
else map_occupy[i][j]=(map_occupy[i][j]-'0')*10;
}
}
//发布地图消息
m_map.data.resize((long)m_map.info.width*m_map.info.height);
for(long i=0;i<((long)m_map.info.width*m_map.info.height);i++) m_map.data[i]=(int)(map_occupy[i/m_map.info.width][i%m_map.info.width]);
fclose(fp);
map_pub.publish(m_map);
printf("地图已经发布！\n");
return 0;
}
/************************************** MAIN ************************************/
int main(int argc, char** argv)
{
ros::init(argc, argv, "map_exchanger");
ros::AsyncSpinner spinner(2);
spinner.start();
net_ui map_exchanger;
ros::Rate r(0.3);
map_exchanger.Map_loader();
while(map_exchanger.n.ok())
{
//map_exchanger.Map_loader();
r.sleep();
}
return 0;
}

复制代码