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;
- }
复制代码
所有资料51hei提供下载:
ui.zip
(11.3 KB, 下载次数: 14)
|