embedded_linux_comms.c
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Paul Bouchier
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote prducts derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #ifndef ROS_EMBEDDED_LINUX_COMMS_H
36 #define ROS_EMBEDDED_LINUX_COMMS_H
37 
38 #include <stdio.h>
39 #include <stdlib.h>
40 #include <unistd.h>
41 #include <fcntl.h>
42 #include <errno.h>
43 #include <termios.h>
44 #include <string.h>
45 #include <time.h>
46 #include <stdint.h>
47 #include <sys/types.h>
48 #include <sys/socket.h>
49 #include <netinet/in.h>
50 #include <netinet/tcp.h>
51 #include <netdb.h>
52 #include <assert.h>
53 
54 #define DEFAULT_PORTNUM 11411
55 
56 void error(const char *msg)
57 {
58  perror(msg);
59  exit(0);
60 }
61 
62 void set_nonblock(int socket)
63 {
64  int flags;
65  flags = fcntl(socket, F_GETFL, 0);
66  assert(flags != -1);
67  fcntl(socket, F_SETFL, flags | O_NONBLOCK);
68 }
69 
70 int elCommInit(const char *portName, int baud)
71 {
72  struct termios options;
73  int fd;
74  int sockfd;
75  struct sockaddr_in serv_addr;
76  struct hostent *server;
77  int rv;
78 
79  if (*portName == '/') // linux serial port names always begin with /dev
80  {
81  printf("Opening serial port %s\n", portName);
82 
83  fd = open(portName, O_RDWR | O_NOCTTY | O_NDELAY);
84 
85  if (fd == -1)
86  {
87  // Could not open the port.
88  perror("init(): Unable to open serial port - ");
89  }
90  else
91  {
92  // Sets the read() function to return NOW and not wait for data to enter
93  // buffer if there isn't anything there.
94  fcntl(fd, F_SETFL, FNDELAY);
95 
96  // Configure port for 8N1 transmission, 57600 baud, SW flow control.
97  tcgetattr(fd, &options);
98  cfsetispeed(&options, B57600);
99  cfsetospeed(&options, B57600);
100  options.c_cflag |= (CLOCAL | CREAD);
101  options.c_cflag &= ~PARENB;
102  options.c_cflag &= ~CSTOPB;
103  options.c_cflag &= ~CSIZE;
104  options.c_cflag |= CS8;
105  options.c_cflag &= ~CRTSCTS;
106  options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
107  options.c_iflag &= ~(IXON | IXOFF | IXANY);
108  options.c_oflag &= ~OPOST;
109 
110  // Set the new options for the port "NOW"
111  tcsetattr(fd, TCSANOW, &options);
112  }
113  return fd;
114  }
115  else
116  {
117  // Split connection string into IP address and port.
118  const char* tcpPortNumString = strchr(portName, ':');
119  long int tcpPortNum;
120  char ip[16];
121  if (!tcpPortNumString)
122  {
123  tcpPortNum = DEFAULT_PORTNUM;
124  strncpy(ip, portName, 16);
125  }
126  else
127  {
128  tcpPortNum = strtol(tcpPortNumString + 1, NULL, 10);
129  strncpy(ip, portName, tcpPortNumString - portName);
130  }
131 
132  printf("Connecting to TCP server at %s:%ld....\n", ip, tcpPortNum);
133 
134  // Create the socket.
135  sockfd = socket(AF_INET, SOCK_STREAM, 0);
136  if (sockfd < 0)
137  {
138  error("ERROR opening socket");
139  exit(-1);
140  }
141 
142  // Disable the Nagle (TCP No Delay) algorithm.
143  int flag = 1;
144  rv = setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, (char *)&flag, sizeof(flag));
145  if (rv == -1)
146  {
147  printf("Couldn't setsockopt(TCP_NODELAY)\n");
148  exit(-1);
149  }
150 
151  // Connect to the server
152  server = gethostbyname(ip);
153  if (server == NULL)
154  {
155  fprintf(stderr, "ERROR, no such host\n");
156  exit(0);
157  }
158  bzero((char *) &serv_addr, sizeof(serv_addr));
159  serv_addr.sin_family = AF_INET;
160  bcopy((char *)server->h_addr,
161  (char *)&serv_addr.sin_addr.s_addr,
162  server->h_length);
163  serv_addr.sin_port = htons(tcpPortNum);
164  if (connect(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0)
165  error("ERROR connecting");
166  set_nonblock(sockfd);
167  printf("connected to server\n");
168  return sockfd;
169  }
170  return -1;
171 }
172 
173 int elCommRead(int fd)
174 {
175  unsigned char c;
176  unsigned int i;
177  int rv;
178  rv = read(fd, &c, 1); // read one byte
179  i = c; // convert byte to an int for return
180  if (rv > 0)
181  return i; // return the character read
182  if (rv < 0)
183  {
184  if ((errno != EAGAIN) && (errno != EWOULDBLOCK))
185  perror("elCommRead() error:");
186  }
187 
188  // return -1 or 0 either if we read nothing, or if read returned negative
189  return rv;
190 }
191 
192 int elCommWrite(int fd, uint8_t* data, int len)
193 {
194  int rv;
195  int length = len;
196  int totalsent = 0;
197  while (totalsent < length)
198  {
199  rv = write(fd, data + totalsent, length);
200  if (rv < 0)
201  perror("write(): error writing - trying again - ");
202  else
203  totalsent += rv;
204  }
205  return rv;
206 }
207 
208 #endif
#define DEFAULT_PORTNUM
void error(const char *msg)
int i
void set_nonblock(int socket)
int elCommWrite(int fd, uint8_t *data, int len)
int elCommRead(int fd)
ros::ServiceServer< Test::Request, Test::Response > server("test_srv",&svcCallback)
int elCommInit(const char *portName, int baud)


rosserial_embeddedlinux
Author(s): Paul Bouchier
autogenerated on Fri Jun 7 2019 22:02:46