rmcd.c 8.35 KB
Newer Older
1
2
3
4
5
/* Robot Master Control Daemon
 *
 * Dan Flickinger
 *
 *
6
 * RMCD will immediately try to connect to EMCD, get configuration data,
Daniel Flickinger's avatar
   
Daniel Flickinger committed
7
8
 * and then open connections to all robots as sent by EMCD. RMCD only heeds
 * one master; in other words, it will connect to only a single EMCD.
9
10
11
12
13
14
15
16
17
18
19
20
21
22
 *
 * Once connections are open, RMCD will wait for commands from EMCD.
 *
 * After a command is received, RMCD will wait for EMCD to send the
 * current position for the robot commanded to move by EMCD.  Once the
 * current position is received, the move command is relayed to the
 * appropriate robot after converting the coordinates into the local
 * robot's reference frame.
 *
 * Upon completion of a command, RMCD will send notice to EMCD, along
 * with a position update sent from the robot and converted into the
 * global coordinate frame.
 *
 *
23
 * 2004/12/01
Daniel Flickinger's avatar
   
Daniel Flickinger committed
24
 * 2004/12/07
25
26
27
28
 */
 
#include<stdio.h>
#include<sys/types.h>
29
30
31
32
33
34
35
36

#include<unistd.h>
#include<stdlib.h>
#include<signal.h>
#include<netiten/in.h>
#include<errno.h>

#include<string.h>
37
38
#include<sys/socket.h>

39
40
#include "rmcd.h"

Daniel Flickinger's avatar
   
Daniel Flickinger committed
41
42
#define EMC_SERVERPORT 2525
#define EMC_HOSTNAME "blah.flux.utah.edu"
43
44
 
int main(int argc, char **argv) {
Daniel Flickinger's avatar
   
Daniel Flickinger committed
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
  /* You are watching RMCD */

  // miscellaneous administration junk
  int quitmain = 0;     // quit main loop
  int retval = 0;       // return value
  int cpacket_rcvd = 0; // configuration packet received

  
  // file descriptors
  int FD_emc; // connection to EMC server
  int max_fd = -1;
  

  // file descriptor set and time value for select() loop
  fd_set defaultset, readset;
  struct timeval default_tv, tv;
61

62

Daniel Flickinger's avatar
   
Daniel Flickinger committed
63
64
65
66
  // set up file descriptors and select variables
  FD_ZERO(&defaultset);
  default_tv.tv_sec = 5; // 5 seconds
  default_tv.tv_usec = 0; // wah?
67
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
68
69
70

  struct sockaddr_in EMC_serv;
  struct hostent *EMC_host;
71
72
73
74
  
  struct mtp_packet *read_packet;
  
  
75
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
76
77
  
 
78
79
  // parse options
  // FIXME
Daniel Flickinger's avatar
   
Daniel Flickinger committed
80
81
82
83
84
         
  int EMC_port = EMC_SERVERPORT; // EMC server port (SET DEFAULT VALUE)
  char EMC_hostname[128]; // FIXME: what is the default hostname?

  
85
86
  
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
87
  // daemon mode -- go to hell
88
89
  // FIXME
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
90
91
92

  
         
93
94
95
96
         
  
  
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
97
98
99
  /*************************************/
  /*** Connect to EMCD and configure ***/
  /*************************************/
100
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
101
102
  // create socket for connection to EMC server
  FD_emc = socket(AF_INET, SOCK_STREAM, 0);
103
104
  if (FD_emc == -1) {
    // fuckup
Daniel Flickinger's avatar
   
Daniel Flickinger committed
105
    fprintf(stdout, "FATAL: Could not open socket to EMC: %s\n", strerror(errno));
106
107
108
    exit(1); 
  }
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
  
  // add this new socket to the default FD set
  FD_SET(FD_emc, &defaultset);
  max_fd = (FD_emc > max_fd)?FD_emc:max_fd;

  
  // client socket to connect to EMCD
  EMC_serv.sin_family = AF_INET; // TCP/IP
  EMC_serv.sin_port = htons(port); // port
  EMC_host = gethostbyname(&EMC_hostname);
  bcopy(EMC_host->h_addr, &EMC_serv, EMC_host->h_length);
  
    
  // connect socket
  if (-1 == connect(FD_emc, (struct sockaddr *)(&EMC_serv), sizeof(struct sockaddr))) {
124
    // fuckup
Daniel Flickinger's avatar
   
Daniel Flickinger committed
125
126
    fprintf(stdout, "FATAL: Could not connect to EMC: %s\n", strerror(errno));
    exit(1);
127
  }
Daniel Flickinger's avatar
   
Daniel Flickinger committed
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
  // now connected to EMCD
  
  
  
  
  // send configuration request packet to EMCD
  // FIXME: need to look at structure in the protocol
  struct mtp_config cfg;
  cfg.id = -1; // ID: FIXME
  cfg.code = -1; // code: FIXME
  cfg.msg = ""; // message: FIXME
  
  struct mtp_packet *cfg_packet = mtp_make_packet(MTP_CONFIG, MTP_ROLE_RMC, &cfg); // FIXME: is it MTP_CONFIG??
  mtp_send_packet(FD_emc, cfg_packet);
  
  
144
145
146
  
  
  while (cpacket_rcvd == 0) {
Daniel Flickinger's avatar
   
Daniel Flickinger committed
147
148
    // wait for configuration data from EMC
    
149
  
Daniel Flickinger's avatar
Daniel Flickinger committed
150
151
    // select??
    
Daniel Flickinger's avatar
   
Daniel Flickinger committed
152
153
154
155
156
157
158
159
    
    // read a packet in
    read_packet = receive_packet(FD_emc);
    if (read_packet == NULL) {
      // fuckup
      fprintf(stdout, "FATAL: Could not read config data from EMC\n");
      exit(1);
    }
160
161
  
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
162
163
    if (read_packet->opcode == MTP_CONFIG_RMC) {
      // this is a configuration packet
164
      
Daniel Flickinger's avatar
   
Daniel Flickinger committed
165
166
      // store robot list data
      // FIXME: need data structure here
167
      
Daniel Flickinger's avatar
   
Daniel Flickinger committed
168
169
170
      // FIXME: global coordinate bound box
      //cfg.box.horizontal = read_packet.box.horizontal;
      //cfg.box.vertical = read_packet.box.vertical;
171
      
Daniel Flickinger's avatar
   
Daniel Flickinger committed
172
173
174
175
      
      cpacket_rcvd = 1;
    } else {
      printf("Still missing configuration packet from EMC\n");
176
    
Daniel Flickinger's avatar
   
Daniel Flickinger committed
177
178
179
180
181
      // send notification to EMC
      // Hello? Where is CONFIG PACKET??
     
      // send the configuration request packet AGAIN
      mtp_send_packet(FD_emc, cfg);
182
    
Daniel Flickinger's avatar
   
Daniel Flickinger committed
183
    }
184
185
  }

Daniel Flickinger's avatar
   
Daniel Flickinger committed
186
187
188
189
190
  /********************************************/
  /*** Now connected to EMCD and configured ***/
  /********************************************/

  
191
192
  
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
193
194
195
  /************************/
  /*** Configure robots ***/
  /************************/
196
197
  
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
198
199
200
201
  // open a connection to each robot listener
  // FIXME: monkey
         
  // {loop}
202
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
203
204
205
206
207
208
209
210
  // create packet for this robot
  // send packet to robot
         
  // configure
         
  // add connection to default set

  // add robot to list as configured
211
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
212
  // notify that robot is configured
213
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
214
  // {endloop}
215
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
216
217
218
219
220
221
222
223
224
225
226
227
228
229
         
         
         
  /*********************************/
  /*** Robots are now configured ***/
  /*********************************/

         
         


  /**********************************************************/
  /*** Notify EMCD that RMCD is ready to receive commands ***/
  /**********************************************************/   
230
  
Daniel Flickinger's avatar
Daniel Flickinger committed
231
  // FIXME: is this right??
Daniel Flickinger's avatar
   
Daniel Flickinger committed
232
  // send packet to EMC updating ready status
Daniel Flickinger's avatar
Daniel Flickinger committed
233
234
235
236
  struct mtp_control rc_packet;
  rc_packet.id = -1; // ID: ?? FIXME
  rc_packet.code = -1; // Code: ?? FIXME
  rc_packet.msg = "RMCD ready";
237
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
238
  struct mtp_packet *rc = mtp_make_packet(MTP_CONFIG, MTP_ROLE_RMC, &rc_packet); // FIXME: not MTP_CONFIG
239
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
240
  mtp_send_packet(FD_emc, rc);
241
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
242
243
244
245


  /*** EMCD has been notified of status ***/  

246
  // set up select bitmap
Daniel Flickinger's avatar
   
Daniel Flickinger committed
247
248


249
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
250
251
252
253
254
255

  /*****************************************/  
  /*** ALL CONFIGURATION IS NOW COMPLETE ***/
  /*** Let's get down to business        ***/
  /*****************************************/

256
  
Daniel Flickinger's avatar
   
Daniel Flickinger committed
257
258
259
260
261
262
  // malloc packet structure for use in the main loop
  struct mtp_packet *packet = (struct mtp_packet *)malloc(sizeof(struct mtp_packet));
  if (packet == NULL) {
    fprintf(stdout, "FATAL: Could not malloc packet structure.\n");
    exit(1);
  }
263

Daniel Flickinger's avatar
   
Daniel Flickinger committed
264
265
266
267
  
  
  
  // main loop, whoo!
268
269
270
  while (quitmain == 0) {
   
    
Daniel Flickinger's avatar
   
Daniel Flickinger committed
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
    readset = defaultset; // select tramples defaultset otherwise
    tv = default_tv; // same here (shame!)
    
    // select() on all EMC and sub-instance file descriptors
    retval = select(max_fd + 1, &readset, NULL, NULL, &tv);
    if (retval == -1) {
      // fuckup
      fprintf(stdout, "RMCD: Error in select loop: %s\n", strerror(errno));
    } else if (retval == 0) {
      // timeout was reached
      // do nothing!
    } else {
      // check all file descriptors for activity
      
      if (FD_ISSET(FD_emc, &readset)) {
        // Uh oh, something came back from EMCD
        
        
289
290
    
    
Daniel Flickinger's avatar
   
Daniel Flickinger committed
291
    // receive packet(s) in
292
    
Daniel Flickinger's avatar
   
Daniel Flickinger committed
293
    // if packet is from EMCD:
294
    
Daniel Flickinger's avatar
   
Daniel Flickinger committed
295
296
297
298
299
300
301
302
    if (packet->role == MTP_ROLE_EMC) {
      
      
      // if goto command from EMC:
      // if this robot already moving, drop the packet
      // otherwise -- put requested position in "robot state array"
      // issue request to EMC for latest position
      // then implicitly wait to hear a position update for this robot id
303
304
305
306
    
    
    
    
Daniel Flickinger's avatar
   
Daniel Flickinger committed
307
308
309
310
311
312
      // if stop command from EMC:
      // if "stop all robots", issue commands to all subinstances saying
      // terminate all motion
      // if stop single robot:
      //   update state array saying we're trying to stop a robot.
      //   forward the stop to subinstance
313
314
315
316
    
    
    
    
Daniel Flickinger's avatar
   
Daniel Flickinger committed
317
318
319
320
      // if position update from EMC:
      // store the position in position array
      // compare intended position to current position (issue goto commands
      //    IF necessary)
321
    
Daniel Flickinger's avatar
   
Daniel Flickinger committed
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
      // transform position to robot local coordinate frame and send
      // to robot
      // ELSE: tell EMCD that this robot is at it's intended position

    
    } else if (packet->role == MTP_GOROBOT) {
      // FIXME: what is the correct role here??
      // it's from a robot
      
      // send get position update request
                 
             
      // IF robot says that it thinks goal position has been reached
      
             
      // IF robot says that it cannot get to commanded position
338
339
340
341
342
343
344
345
346
    

      
    }
    
     
  } // end main loop


Daniel Flickinger's avatar
   
Daniel Flickinger committed
347
348
349
  // have a nice day, and good bye.
  printf("RMCD exiting without complaint.\n");
  return 0; // hope to see you soon
350
}