iCub-main
main.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia
3  * Author: Valentina Gaggero
4  * email: valentina.gaggero@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
19 /* @file main.c
20  @brief
21  @author valentina.gaggero@iit.it
22  @date 03/23/2013
23 **/
24 
25 // --------------------------------------------------------------------------------------------------------------------
26 // - external dependencies
27 // --------------------------------------------------------------------------------------------------------------------
28 
29 #include "stdint.h"
30 #include "stdlib.h"
31 #include "stdio.h"
32 #include <string>
33 #include <signal.h>
34 #include <iostream>
35 
36 using namespace std;
37 // Ace stuff
38 #include <ace/ACE.h>
39 #include "ace/SOCK_Dgram.h"
40 #include "ace/Addr.h"
41 #include "ace/Thread.h"
42 #include "ace/Logging_Strategy.h" // for logging stuff
43 
44 
45 /*#include <yarp/os/Network.h>
46 #include <yarp/os/BufferedPort.h>
47 #include <yarp/dev/DeviceDriver.h>
48 #include <yarp/dev/CanBusInterface.h>
49 #include <yarp/dev/PolyDriver.h>
50 #include <yarp/os/PeriodicThread.h>
51 */
52 
53 
54 //#include "EoCommon.h"
55 
56 #include "template_buttons.hpp"
57 
58 #include "OPCprotocolManager.h"
59 #include "OPCprotocolManager_Cfg.h"
60 
61 
62 // --------------------------------------------------------------------------------------------------------------------
63 // - declaration of external variables
64 // --------------------------------------------------------------------------------------------------------------------
65 
66 
67 // --------------------------------------------------------------------------------------------------------------------
68 // - declaration of extern public interface
69 // --------------------------------------------------------------------------------------------------------------------
70 
71 
72 // --------------------------------------------------------------------------------------------------------------------
73 // - declaration of extern hidden interface
74 // --------------------------------------------------------------------------------------------------------------------
75 
76 
77 // --------------------------------------------------------------------------------------------------------------------
78 // - #define with internal scope
79 // --------------------------------------------------------------------------------------------------------------------
80 #define DEFAULT_LOCAL_IP "10.0.1.104"
81 #define DEFAULT_PORT 4444
82 #define PAYLOAD_MAX_SIZE 128
83 
84 
85 // --------------------------------------------------------------------------------------------------------------------
86 // - definition (and initialisation) of extern variables, but better using _get(), _set()
87 // --------------------------------------------------------------------------------------------------------------------
88 OPCprotocolManager *opcMan_ptr = NULL;
89 
90 
91 
92 // --------------------------------------------------------------------------------------------------------------------
93 // - typedef with internal scope
94 // --------------------------------------------------------------------------------------------------------------------
95 typedef struct
96 {
98  ACE_INET_Addr addr;
99 } hostAddr_t;
100 
101 
102 
103 // --------------------------------------------------------------------------------------------------------------------
104 // - declaration of static functions
105 // --------------------------------------------------------------------------------------------------------------------
106 static void sighandler(int _signum);
107 void usage(void);
108 void *recvThread(void * arg);
109 
110 #ifdef __cplusplus
111 extern "C" {
112 #endif
113  extern opcprotman_cfg_t* get_OPCprotocolManager_cfg(void);
114 #ifdef __cplusplus
115 }
116 #endif
117 
118 // --------------------------------------------------------------------------------------------------------------------
119 // - definition (and initialization) of static variables
120 // --------------------------------------------------------------------------------------------------------------------
121 ACE_SOCK_Dgram *ACE_socket;
122 int keepGoingOn = 1;
123 uint8_t board = 0;
124 
125 // --------------------------------------------------------------------------------------------------------------------
126 // - definition of extern public functions
127 // --------------------------------------------------------------------------------------------------------------------
128 static void s_check_seqNum(ACE_INET_Addr src_addr, uint32_t recseqnum);
129 
130 static void sighandler(int _signum)
131 {
132  printf("\n Received signal to quit %d\n", _signum);
133  keepGoingOn = 0;
134  printf("you shoukd write <quit>\n", keepGoingOn);
135 }
136 
137 
138 int main(int argc, char *argv[])
139 {
140 
141  ACE_UINT16 port;
142  ACE_INT8 flags = 0;
143  hostAddr_t local, remote;
144  ACE_UINT32 ip1,ip2,ip3,ip4;
145 
146  uint8_t payload_buffer[PAYLOAD_MAX_SIZE];
147  uint16_t payload_size = 0;
148 
149  ACE_thread_t id_recvThread; //thread manages rx pkt
150 
151  uint32_t byte2send = 0;
152 
153 
154 
155  // Register handler to catch CTRL+C
156 // if(signal(SIGINT, sighandler) == SIG_IGN)
157 // signal(SIGINT, SIG_IGN);
158  signal (SIGQUIT, sighandler);
159  signal (SIGINT, sighandler);
160 
161  // Set default connection data
162  local.address_string=string(DEFAULT_LOCAL_IP);
163  port = DEFAULT_PORT;
164 
165 
166 
167  // parse command line input argument
168  if(argc > 1)
169  {
170  for(int i = 1; i < argc; i++)
171  {
172 
173  if(strcmp("--loc-ip", argv[i]) == 0 )
174  {
175  if(i < (argc - 1))
176  local.address_string = string(argv[++i]);
177  continue;
178  }
179 
180  if(strcmp("--port", argv[i]) == 0 )
181  {
182  if(i < (argc - 1))
183  port = atoi(argv[++i]);
184  continue;
185  }
186 
187  if((strcmp("-h", argv[i]) == 0) || (strcmp("--help", argv[i]) == 0))
188  {
189  usage();
190  return 0;
191  }
192 
193  usage();
194  return -1;
195  }
196  }
197 
198  usage();
199 
200  printf("\nConfiguration is : \n\n");
201 
202  // start the udp socket using ace / winsock / psocks
203  // Set my own address_string
204  sscanf(local.address_string.c_str(),"%d.%d.%d.%d",&ip1,&ip2,&ip3,&ip4);
205  local.addr.set(port, (ip1<<24)|(ip2<<16)|(ip3<<8)|ip4 );
206 
207  printf("local.address: %s\n", local.address_string.c_str());
208 
209  ACE_socket = new ACE_SOCK_Dgram();
210  if (-1 == ACE_socket->open(local.addr) )
211  {
212  printf("Error opening socket!!!\n");
213  return -1;
214  }
215  int n;
216  int m = sizeof(n);
217  ACE_socket->get_option(SOL_SOCKET,SO_RCVBUF,(void *)&n, &m);
218  printf("SO_RCVBUF %d\n", n);
219  ACE_socket->get_option(SOL_SOCKET,SO_SNDBUF,(void *)&n, &m);
220  printf("SO_SNDBUF %d\n", n);
221  ACE_socket->get_option(SOL_SOCKET,SO_RCVLOWAT,(void *)&n, &m);
222  printf("SO_RCVLOWAT %d\n", n);
223 
224 
225 // // Set destination address_string
226 // sscanf(remote01.address_string.c_str(),"%d.%d.%d.%d",&ip1,&ip2,&ip3,&ip4);
227 // remote01.addr.set(port, (ip1<<24)|(ip2<<16)|(ip3<<8)|ip4 );
228 // remote02.addr.set(3333, (ip1<<24)|(ip2<<16)|(ip3<<8)|ip4 );
229 // remoteAddr = eo_common_ipv4addr(ip1,ip2,ip3,ip4);
230 //
231 // printf("remote01.address: %s\n", remote01.address_string.c_str());
232 // printf("port is : %d\n\n", port);
233 // //check boardN
234 // remoteAddr = eo_common_ipv4addr(ip1,ip2,ip3,ip4); // marco (10, 255, 39, 151)
235 // eOport = port;
236 
237 
238 
239  // Start receiver thread
240  printf("Launching recvThread\n");
241  if(ACE_Thread::spawn((ACE_THR_FUNC)recvThread, NULL, THR_CANCEL_ENABLE, &id_recvThread)==-1)
242  {
243  printf(("Error in spawning recvThread\n"));
244  }
245 
246 
247  //init opc manager
248  opcMan_ptr = opcprotman_New(OPCprotocolManager_Cfg_getconfig());
249  if(NULL ==opcMan_ptr)
250  {
251  printf("Error in creating opcprotman\n!");
252  return(0);
253  }
254 
255  while(keepGoingOn)
256  {
257  //reset pkt
258  memset(&payload_buffer[0], 0x00, PAYLOAD_MAX_SIZE);
259  char in_cmd[10], aux[2];
260  int n_cmd, ret;
261  commands();
262 
263  ret = scanf("%s", in_cmd);
264 
265  if((strcmp("quit", in_cmd) == 0 ))
266  {
267  keepGoingOn = 0;
268  break;
269  }
270  else
271  {
272  n_cmd = atoi(in_cmd);
273  switch(n_cmd)
274  {
275  case 0:
276  byte2send = callback_button_0(&payload_buffer[0], PAYLOAD_MAX_SIZE, &remote.addr);
277  break;
278 
279  case 1: // send one ask rop
280  byte2send = callback_button_1(&payload_buffer[0], PAYLOAD_MAX_SIZE, &remote.addr);
281  break;
282 
283  case 2: // config/reset regular rops
284  byte2send = callback_button_2(&payload_buffer[0], PAYLOAD_MAX_SIZE, &remote.addr);
285  break;
286 
287  case 3: // send empty rop
288  byte2send = callback_button_3(&payload_buffer[0], PAYLOAD_MAX_SIZE, &remote.addr);
289  break;
290 
291  case 4: // send a set pid rop
292  byte2send = callback_button_4(&payload_buffer[0], PAYLOAD_MAX_SIZE, &remote.addr);
293  break;
294 
295  case 5: // send a status sig
296  byte2send = callback_button_5(&payload_buffer[0], PAYLOAD_MAX_SIZE, &remote.addr);
297  break;
298 
299  case 6: // send a status sig
300  byte2send = callback_button_6(&payload_buffer[0], PAYLOAD_MAX_SIZE, &remote.addr);
301  break;
302 
303  case 7: // send a status sig
304  byte2send = callback_button_7(&payload_buffer[0], PAYLOAD_MAX_SIZE, &remote.addr);
305  break;
306 
307  case 8: // send a status sig
308  byte2send = callback_button_8(&payload_buffer[0], PAYLOAD_MAX_SIZE, &remote.addr);
309  break;
310 
311  case 9: // send a status sig
312  byte2send = callback_button_9(&payload_buffer[0], PAYLOAD_MAX_SIZE, &remote.addr);
313  break;
314 
315  case 10:
316  byte2send = callback_button_10(&payload_buffer[0], PAYLOAD_MAX_SIZE, &remote.addr);
317  break;
318 
319  default:
320  byte2send = false;
321  printf("Command not known!\n");
322  break;
323  }
324 
325  if(byte2send > 0)
326  {
327  ssize_t sentBytes = ACE_socket->send(&payload_buffer[0], byte2send, remote.addr, 0/*flags*/);
328  ACE_TCHAR address[64];
329  remote.addr.addr_to_string(address, 64);
330  printf("payload of size %d is sent to %s!! (sentbytes=%ld)\n", byte2send, address, sentBytes);
331  }
332  else
333  {
334  printf("nothing to send");
335 
336  }
337 
338  }//else
339 
340  }
341 
342 
343  printf("mainThraed: exit from while..going to sleep\n");
344  sleep(2);
345 
346  printf("mainThraed: exit from sleep\n");
347  //pthread_cancel(thread);
348  ACE_Thread::cancel(id_recvThread);
349 
350 
351  return(0);
352 }
353 
354 
355 // --------------------------------------------------------------------------------------------------------------------
356 // - functions with internal scope
357 // --------------------------------------------------------------------------------------------------------------------
358 
359 void *recvThread(void * arg)
360 {
361  printf("recvThread started\n");
362 
363  uint8_t rec_payload_buffer[PAYLOAD_MAX_SIZE];
364  uint8_t replay_payload_buffer[PAYLOAD_MAX_SIZE];
365  size_t rec_payload_size = 0;
366  uint16_t replysize = 0;
367  ACE_INET_Addr src_addr;
368  ACE_TCHAR address[64];
369  opcprotman_res_t res;
370 
371 
372  sockaddr sender_addr;
373 
374  while(keepGoingOn)
375  {
376  memset(&rec_payload_buffer[0], 0, PAYLOAD_MAX_SIZE);
377  rec_payload_size = ACE_socket->recv(&rec_payload_buffer[0], PAYLOAD_MAX_SIZE, src_addr, 0/*flags*/);
378  src_addr.addr_to_string(address, 64);
379  printf("\n\nReceived pkt of size = %ld from %s\n", rec_payload_size, address);
380  s_check_seqNum(src_addr, opcprotman_getSeqNum(opcMan_ptr, (opcprotman_message_t*)&rec_payload_buffer[0]));
381  /*chiama qui funzione parser*/
382 /* for(int i =0; i<rec_payload_size; i++)
383  {
384  printf("%x", rec_payload_buffer[i]);
385  }
386 */
387 
388  res = opcprotman_Parse(opcMan_ptr, (opcprotman_message_t*)&rec_payload_buffer[0], (opcprotman_message_t*) &replay_payload_buffer[0], &replysize);
389  if(opcprotman_OK != res)
390  {
391  printf("erron in parser\n");
392  }
393 
394  }
395 
396  printf("recThraed: exit from while\n");
397 
398  pthread_exit(NULL);
399  return NULL;
400 }
401 
402 
403 
404 
405 void usage(void)
406 {
407  printf("usage: \t\t--help print this help\n");
408  printf("\t\t--loc-ip <xx> set the ip address of the local machine - needed by yarp - by default %s\n", DEFAULT_LOCAL_IP);
409  printf("\t\t--port <xx> set the socket port - by default %d\n", DEFAULT_PORT);
410 }
411 
412 
413 static uint32_t seqnumList[10] = {0};
414 static uint8_t isfirstList[10] = {0};
415 static void s_check_seqNum(ACE_INET_Addr src_addr, uint32_t recseqnum)
416 {
417  uint32_t addr = src_addr.get_ip_address();
418 
419  board = addr- 0xa000100;
420  if(board >=10)
421  {
422  printf("error in get board: num=%d, addr=%d\n", board, addr );
423  }
424 
425  if(isfirstList[board] == 0)
426  {
427  seqnumList[board] = recseqnum;
428  printf("seqnum first board %d num %d\n", board, recseqnum);
429  isfirstList[board] = 1;
430  return;
431  }
432 
433  if((seqnumList[board] +1) != recseqnum)
434  {
435  printf("error in SEQ NUM form board %d: rec %d expected = %d\n",board, recseqnum, seqnumList[board] +1);
436  seqnumList[board] = recseqnum;
437  }
438  else
439  {
440  printf("received SEQ NUM from board %d: %d\n",board, recseqnum);
441  seqnumList[board]++;
442  }
443 
444 
445 
446 }
447 
448 // --------------------------------------------------------------------------------------------------------------------
449 // - end-of-file (leave a blank line after)
450 // --------------------------------------------------------------------------------------------------------------------
int n
uint32_t callback_button_3(uint8_t *payload_ptr, uint32_t payload_size, ACE_INET_Addr *addr)
uint32_t callback_button_8(uint8_t *payload_ptr, uint32_t payload_size, ACE_INET_Addr *addr)
uint32_t callback_button_7(uint8_t *payload_ptr, uint32_t payload_size, ACE_INET_Addr *addr)
uint32_t callback_button_0(uint8_t *payload_ptr, uint32_t payload_size, ACE_INET_Addr *addr)
uint32_t callback_button_1(uint8_t *payload_ptr, uint32_t payload_size, ACE_INET_Addr *addr)
uint32_t callback_button_4(uint8_t *payload_ptr, uint32_t payload_size, ACE_INET_Addr *addr)
uint32_t callback_button_5(uint8_t *payload_ptr, uint32_t payload_size, ACE_INET_Addr *addr)
void commands(void)
uint32_t callback_button_2(uint8_t *payload_ptr, uint32_t payload_size, ACE_INET_Addr *addr)
uint32_t callback_button_10(uint8_t *payload_ptr, uint32_t payload_size, ACE_INET_Addr *addr)
uint32_t callback_button_6(uint8_t *payload_ptr, uint32_t payload_size, ACE_INET_Addr *addr)
uint32_t callback_button_9(uint8_t *payload_ptr, uint32_t payload_size, ACE_INET_Addr *addr)
int main(int argc, char *argv[])
Definition: main.cpp:31
OPCprotocolManager * opcMan_ptr
Definition: main.cpp:88
#define DEFAULT_PORT
Definition: main.cpp:81
uint8_t board
Definition: main.cpp:123
void * recvThread(void *arg)
Definition: main.cpp:359
ACE_SOCK_Dgram * ACE_socket
Definition: main.cpp:121
static void s_check_seqNum(ACE_INET_Addr src_addr, uint32_t recseqnum)
Definition: main.cpp:415
static void sighandler(int _signum)
Definition: main.cpp:130
#define DEFAULT_LOCAL_IP
Definition: main.cpp:80
#define PAYLOAD_MAX_SIZE
Definition: main.cpp:82
opcprotman_cfg_t * get_OPCprotocolManager_cfg(void)
static uint8_t isfirstList[10]
Definition: main.cpp:414
void usage(void)
Definition: main.cpp:405
static uint32_t seqnumList[10]
Definition: main.cpp:413
int keepGoingOn
Definition: main.cpp:122
ACE_INET_Addr addr
Definition: main.cpp:98
string address_string
Definition: main.cpp:97