Socket Programming in ROS

edit

Hi all,

I wanted to ask a simple question. I was trying to establish TCP/IP server in ROS node. Well my concept was really simple to make ROS node that will work as a server , and then I will use that server to send the values(velocities and joint values) in a string.

my code is fairly simple,

#include <iostream>     
#include <cstdlib>      
#include <unistd.h>     
#include <cerrno>       
#include <cstring>  
#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/wait.h>
#include <csignal>  

#include "ros/ros.h"
#include "nav_msgs/Odometry.h"

using namespace std;
#define PORTNUMBER 8001    // port number of connection
#define BACKLOG 2     // max number of client
#define MAXDATASIZE 100 // maximum accepted datasize at a time 



nav_msgs::Odometry lastReceivedOdometry;


//need to handle zombi processes by sigaction()
void sigchld_handler(int s)
{
        while(wait(NULL) > 0);
}

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

  ros::init(argc, argv, "socket");

  ros::NodeHandle n;

  ros::spin();

  int sockfd, new_fd, mySelect, sockoptset, bindit, listento, actionsig, numbytes,sended;  
  char buf[MAXDATASIZE];
  struct sockaddr_in my_addr;           // my address information
  struct sockaddr_in their_addr;        // address information of connected machine
  int sin_size;
  struct sigaction sa;          // need to handle "zombi" processes after ended child processes which are created by fork()
  int yes=1;                // needed by setsockopt()

  fd_set read_fd;
  struct timeval timeout;
  timeout.tv_sec = 15;
  timeout.tv_usec = 0;

  // create the socket
  sockfd = socket(AF_INET, SOCK_STREAM, 0); 
  // error check for socket()
  if (sockfd == -1) {
        cout <<"error socket"<< strerror(errno);
        exit(1);
  }

  my_addr.sin_family = AF_INET;         // host byte order , AF_INET = IPv4 Internet Protocols for Linux
  my_addr.sin_port = htons(PORTNUMBER);     // sin_port is in short-network byte order,htons()=converts PORTNUMBER to network byte order
  my_addr.sin_addr.s_addr = INADDR_ANY; // use my address automatically, use "INADDR_ANY"(0 so no need htons) or "inet_addr("192.120.13.1")
  memset(&(my_addr.sin_zero), '\0', 8); // make the rest of the structure zero

  //assign the address specified to the socket 
  bindit = bind(sockfd, (struct sockaddr *)&my_addr, sizeof(struct sockaddr));
  //error check for bind()
  if (bindit == -1) {
        cout <<"error bindit"<< strerror(errno);
            exit(1);
  }

  //listen the connection on created socket
  listento = listen(sockfd, BACKLOG);
  //error check for listen()
  if (listento == -1) {
       cout <<"error listento"<< strerror(errno);
            exit(1);
  }

  //examine and change the "zombi" processes signal actions
  sa.sa_handler = sigchld_handler;  // reap all dead processes
  sigemptyset(&sa.sa_mask);
  sa.sa_flags = SA_RESTART;
  actionsig = sigaction(SIGCHLD, &sa, NULL);
  //error check for sigaction()
  if (actionsig == -1) {
       cout <<"error actionsig"<< strerror(errno);
            exit(1);
  }
  sin_size = sizeof(struct sockaddr_in);

  // accept a connection on listened socket
  new_fd = accept(sockfd, (struct sockaddr *)&their_addr, (socklen_t*)&sin_size);
  //error check for accept()
  if (new_fd == -1) {           
         cout << strerror(errno) << "accept() has failed!" << endl ;
  }

  cout << "server: got connection from " << inet_ntoa(their_addr.sin_addr) << endl; 


  FD_ZERO(&read_fd);
  FD_SET(new_fd,&read_fd);
  mySelect = select(new_fd+1,&read_fd,NULL,NULL,&timeout);
  if(mySelect == 1 && (FD_ISSET(new_fd,&read_fd))){
            while(new_fd !=-1){
                numbytes=recv(new_fd, buf, MAXDATASIZE-1, 0);
                if(numbytes == 0){
                    cout << "Connection has terminated by " << inet_ntoa(their_addr.sin_addr)<<endl;
                    close(new_fd);
                    close(sockfd);
                    return 0;
                }
                buf[numbytes] = '\0';
                cout << numbytes << endl;
                if(strcmp ...