ros下tcp通信的过程

                                                       ros框架下的tcp通信的过程

    博主在编写一个需要通过wifi传输两个整数和一个状态的问题中，通过构建ros（Ubuntu16.04)框架下的tcp(c++)通信过程解决了问题。

一、ros串口通信的实现

    首先我们需要从单片机中接收需要传输的数据，为了方便，我将状态和两个整数合成为一个七个字符的字符串传输，在需要调用的地方解析该字符串。

    ros的串口通信我们可以使用官方自带的serial通信包，主要调用了串口打开函数open()和串口读取函数read()，至于ros节点的创建与话题的发布，我在这里不再赘述，不太了解的可以参考http://wiki.ros.org/ROS。

    该ros功能包里我自定义了消息类型wifi_good.msg，虽然里面只有string一种消息类型，但是为以后程序改进留有空间

string message

#include "ros/ros.h"
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
#include <serial/serial.h>  
#include <std_msgs/String.h>
#include <wifi_nice/wifi_good.h>


serial::Serial ser; 


int main (int argc, char** argv)
{
  //初始化节点
  ros::init(argc, argv, "serial_common_node");
  //声明节点句柄
  ros::NodeHandle nh;
  //发布主题
  ros::Publisher read_pub = nh.advertise<wifi_nice::wifi_good>("read", 33);


  try
  {
  //设置串口属性，并打开串口
      ser.setPort("/dev/ttyUSB0");
      ser.setBaudrate(921600);
      serial::Timeout to = serial::Timeout::simpleTimeout(1000);
      ser.setTimeout(to);
      ser.open();
  }
  catch (serial::IOException& e)
  {
      ROS_ERROR_STREAM("Unable to open port ");
      return -1;
  }
  //检测串口是否已经打开，并给出提示信息
  if(ser.isOpen())
  {
      ROS_INFO_STREAM("Serial Port initialized");
  }
  else
  {
      return -1;
  }


  ros::Rate loop_rate(50);
  while(ros::ok())
  {

      if(ser.available()){
          ROS_INFO_STREAM("Reading from serial port\n");
          wifi_nice::wifi_good result;
          result.message=ser.read(ser.available());
          ROS_INFO_STREAM("Read: " << result.message);
          read_pub.publish(result);
      }

      //处理ROS的信息，比如订阅消息,并调用回调函数
      ros::spinOnce();
      loop_rate.sleep();

  }



}

二、话题的订阅与消息的发送

    以上的代码中将消息发送到滤read话题上，我们通过订阅该话题来实现传输

    为了方便，我在回调函数中写了tcp通信的代码（需要注意的是在每次调用回调函数时，一定要记得在回调函数后面添加ros::spin()或ros::spinOnce()函数，不清楚区别的可以自行百度），通过限定回调函数调用的频率，实现tcp消息传输的循环发送。

博主需要实现一个客户端对应多个服务端，因此定义了多个IP地址，通过定义多个socket来实现该过程。

     tcp传输主要调用了write()函数，具体代码如下：

#include <ros/ros.h>
#include<iostream>
#include<cstring>
#include<string>
#include <fstream>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <wifi_nice/wifi_good.h>
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <sys/wait.h>
#include <arpa/inet.h>
#include <unistd.h>
#define SERVPORT 8080
#define MAXDATASIZE 100
#define SERVER_IP_1 "192.168.1.102"
#define SERVER_IP_2 "192.168.1.101"
#define SERVER_IP_3 "192.168.1.104"
using namespace std;

void read_callback(const wifi_nice::wifi_good msg)
{
  char DATA[MAXDATASIZE];
  istringstream stream1;
  stream1.str(msg.message);
  int number;
  stream1 >> number;
  sprintf(DATA,"%d",number);
  ROS_INFO_STREAM("RECEIVE: "<< number);


      int sockfd_1;
      struct sockaddr_in serv_addr_1;

	if ((sockfd_1 = socket(AF_INET, SOCK_STREAM, 0)) == -1) {

		perror("socket error!");
		exit(1);
	}

	bzero(&serv_addr_1, sizeof(serv_addr_1));
	serv_addr_1.sin_family = AF_INET;
	serv_addr_1.sin_port = htons(SERVPORT);
	serv_addr_1.sin_addr.s_addr = inet_addr(SERVER_IP_1);

	connect(sockfd_1, (struct sockaddr *) &serv_addr_1, sizeof(struct sockaddr));
        write(sockfd_1, DATA, sizeof(DATA));
	close(sockfd_1);

       
       //int sockfd_2;
       //struct sockaddr_in serv_addr_2;

	//if ((sockfd_2 = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
       
        //	perror("socket error!");
	//	exit(1);
	//}

	//bzero(&serv_addr_2, sizeof(serv_addr_2));
	//serv_addr_2.sin_family = AF_INET;
	//serv_addr_2.sin_port = htons(SERVPORT);
	//serv_addr_2.sin_addr.s_addr = inet_addr(SERVER_IP_2);

	//connect(sockfd_2, (struct sockaddr *) &serv_addr_2, sizeof(struct sockaddr));
	//write(sockfd_2, DATA, sizeof(DATA));
	//close(sockfd_2);

        
        //int sockfd_3;
        //struct sockaddr_in serv_addr_3;

	//if ((sockfd_3 = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
        
 		//perror("socket error!");
		//exit(1);
	//}

	//bzero(&serv_addr_3, sizeof(serv_addr_3));
	//serv_addr_3.sin_family = AF_INET;
	//serv_addr_3.sin_port = htons(SERVPORT);
	//serv_addr_3.sin_addr.s_addr = inet_addr(SERVER_IP_3);

	//connect(sockfd_3, (struct sockaddr *) &serv_addr_3, sizeof(struct sockaddr));
	//write(sockfd_3, DATA, sizeof(DATA));
	//close(sockfd_3);

}


int main(int argc, char **argv)
{
  
  ros::init(argc, argv, "listener");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("read", 1, read_callback);
  ros::spin(); 
  return 0;
}

三、消息的接收与发布

    消息发送后，需要在上位机创建一个功能包实现消息的接受与发布，具体代码如下：

#include <ros/ros.h>
#include <wifi_get/wifi_nice.h>
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <sys/wait.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <iostream>
#define SERVPORT 8080
#define BACKLOG 10
#define MAXSIZE 1024
using namespace std;

int main(int argc, char **argv)
{

        int sockfd, client_fd;
	struct sockaddr_in my_addr;
	struct sockaddr_in remote_addr;
	//创建套接字
if ((sockfd = socket( AF_INET, SOCK_STREAM, 0 ) )== -1 ){
		perror("socket create failed!");
		exit(1);
	}
	//绑定端口地址
	my_addr.sin_family = AF_INET;
	my_addr.sin_port = htons(SERVPORT);
	my_addr.sin_addr.s_addr = INADDR_ANY;
	bzero(&(my_addr.sin_zero), 8);
	if (bind(sockfd, (struct sockaddr*) &my_addr, sizeof(struct sockaddr))== -1) {
		perror("bind error!");
		exit(1);
	}
	//监听端口
	if (listen(sockfd, BACKLOG) == -1) {
		perror("listen error");
		exit(1);
	}


  //用于解析ROS参数，第三个参数为本节点名
  ros::init(argc, argv, "talker");
  ros::NodeHandle nh;
  wifi_get::wifi_nice msg;
  msg.message="0000000";
  ros::Publisher pub = nh.advertise<wifi_get::wifi_nice>("read", 1);
  ros::Rate loop_rate(1.0);

  while (1)
  {
    
    //int sin_size = sizeof(struct sockaddr_in);
socklen_t sin_size = sizeof(struct sockaddr_in);
if ((client_fd = accept(sockfd,(struct sockaddr*) &remote_addr,&sin_size)) == -1){
			perror("accept error!");
			continue;
		}

                

	
                        char buf[MAXSIZE];
			//接受client发送的请示信息
			int rval;
			
			if ((rval = read(client_fd, buf, MAXSIZE)) < 0) {
				perror("reading stream error!");
				continue;
			}
                

			string s(&buf[0],&buf[strlen(buf)]);  
                        msg.message=s;
                        cout<<"发布的数据为："<<s<<endl;
                        
			close(client_fd);
	
    
                        pub.publish(msg);
    //根据前面定义的频率, sleep 1s
    loop_rate.sleep();
}
  return 0;
} 

    数据在read话题上发布后，我们在需要应用到传输消息的主函数中通过回调函数，解析传输的消息，实现消息的应用，具体代码如下：

//ROS头文件
#include <ros/ros.h>
#include <string.h>
#include <wifi_get/wifi_nice.h>
#include <unistd.h>
#include <iostream>
using namespace std;


void wifi_Callback(const wifi_get::wifi_nice msg)
{  
    istringstream stream_ck;
    stream_ck.str(msg.message);
    int number;
    stream_ck>>number;
    ROS_INFO_STREAM("msg: "<<msg.message);
    int left,right,get;
    left=number%1000;
    right=(number/1000)%1000;
    get=(number/1000)/1000;
    ROS_INFO_STREAM("left: "<<left<<"     right:"<<right);
    ROS_INFO_STREAM("------------------- ");
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("read", 1, wifi_Callback);
  ros::spin(); 
  return 0;
}