client_traci_OMG.c 12.6 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
/*******************************************************************************

  Eurecom OpenAirInterface
  Copyright(c) 1999 - 2011 Eurecom

  This program is free software; you can redistribute it and/or modify it
  under the terms and conditions of the GNU General Public License,
  version 2, as published by the Free Software Foundation.

  This program is distributed in the hope it will be useful, but WITHOUT
  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  more details.

  You should have received a copy of the GNU General Public License along with
  this program; if not, write to the Free Software Foundation, Inc.,
  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.

  The full GNU General Public License is included in this distribution in
  the file called "COPYING".

  Contact Information
  Openair Admin: openair_admin@eurecom.fr
  Openair Tech : openair_tech@eurecom.fr
  Forums       : http://forums.eurecom.fsr/openairinterface
  Address      : Eurecom, 2229, route des crêtes, 06560 Valbonne Sophia Antipolis, France

*******************************************************************************/

/*! \file client_traci_OMG.c
* \brief The OMG TraCI to send/receive commands from/to  SUMO via socket interfaces.
* \author  S. Uppoor, J. Harri
* \date 2012
* \version 0.1
* \company INRIA, Eurecom
* \email: sandesh.uppor@inria.fr, haerri@eurecom.fr
* \note
* \warning
*/

#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/time.h>
#include <string.h>

#include "client_traci_OMG.h"
#include "TraCIConstants.h"

50
51
#define UNUSED_VARIABLE(vARIABLE)   (void)(vARIABLE)

52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
int handshake(char *hoststr,int portno){
    
   check_endianness(); // check endianness
   int i;

   for(i = 0; i< 10; i++) {
        if ( connection_(hoststr,portno) <0 ) {
           #ifdef STANDALONE  
            printf("connection error...trying again in 3 seconds\n");
          #else
            LOG_E(OMG, " Could not connect to TraCIServer - sleeping before trying again...\n");
         #endif
            sleep(5);
        }
        else {
        #ifdef STANDALONE  
  	  printf(" SUMO now connected to OMG on host address\n");
        #else
          LOG_N(OMG, " SUMO now connected to OMG on host address %s and port %i .\n", hoststr, portno);
	#endif
 	      return 0;  
	}
   }
    #ifdef STANDALONE  
      printf(" SUMO unreachable...giving up...\n"); 
    #else
      LOG_E(OMG, " SUMO unreachable...giving up...\n");
     #endif
    return -1;
}

void init(int max_sim_time) {
  
  #ifdef STANDALONE 
    printf(" Initializing TraCI...\n");
  #else
    LOG_N(OMG, " Initializing TraCI...\n");
  #endif
  char *objID = " ";
 // size_t size = strlen(objID);

  int noSubscribedVars = 2;
  writeUnsignedByte(0);
  writeInt(1 + 4 + 1 + 4 + 4 + 4 + (int) strlen(objID) + 1 + noSubscribedVars);
  writeUnsignedByte(CMD_SUBSCRIBE_SIM_VARIABLE); // command id
  writeInt(0); // begin time
  writeInt(max_sim_time*1000); // end time
  writeString(objID); // object id
  writeUnsignedByte(noSubscribedVars); // variable number
  writeUnsignedByte(VAR_DEPARTED_VEHICLES_IDS);
  writeUnsignedByte(VAR_ARRIVED_VEHICLES_IDS);

  // send request message
  sendExact(storageLength(storageStart));
  extractCommandStatus(receiveExact(), CMD_SUBSCRIBE_SIM_VARIABLE, description);
  if (departed == NULL) {
108
    departed = (string_list*) malloc(sizeof(string_list)); // departed MUST point to HEAD
109
110
111
112
    departed->string = NULL;
    departed->next = NULL;
  }
  if (arrived == NULL) {
113
    arrived = (string_list*) malloc(sizeof(string_list));  // arrived MUST point to HEAD
114
115
116
117
118
119
120
121
122
123
124
125
    arrived->string = NULL;
    arrived->next = NULL;
  } 
    
  processSubscriptions();

  reset();
}

void processSubscriptions() {
   int noSubscriptions = readInt();

126
127
   string_list* tmp_departed = departed;
   string_list* tmp_arrived = arrived;
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
   int s;
   for (s = 0; s<noSubscriptions; ++s) {
    
      int respStart = readInt();
      int respLength = readUnsignedByte();
      if (respLength == 0)
         respLength = readInt();

     
      int cmdId = readUnsignedByte();
      if (cmdId<0xe0||cmdId>0xef) {  // only responses to subscription to supported types (vehicles, TLC, polygones...) are accepted
         //LOG_W(OMG, " Invalide Subscription response: %d\n",cmdId);
         //printf(" Invalide Subscription response: %d\n",cmdId);
         return;
      }
      char *objID = readString();
      int varNo = readUnsignedByte();
145
146
147
148

      UNUSED_VARIABLE(objID);
      UNUSED_VARIABLE(respStart);

149
150
151
152
153
154
      int i;
      for (i=0; i<varNo; ++i) {
          int varID = readUnsignedByte();
          
          bool ok = readUnsignedByte()==RTYPE_OK;
          int valueDataType = readUnsignedByte();
155
          UNUSED_VARIABLE(valueDataType);
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
          if (ok&&cmdId==CMD_SUBSCRIBE_SIM_VARIABLE+0x10&&varID==VAR_DEPARTED_VEHICLES_IDS) {
               tmp_departed = readStringList(tmp_departed);
               //printf(" OMG Got departed cars\n");
               continue;
           }
           if (ok&&cmdId==CMD_SUBSCRIBE_SIM_VARIABLE+0x10&&varID==VAR_ARRIVED_VEHICLES_IDS) {
               tmp_arrived = readStringList(tmp_arrived);
  		//printf(" OMG Got arrived cars\n");
               continue;
           }
       }
    }
}

int extractCommandStatus(storage *s, unsigned char commandId, char * description)
{
	 int success=0;

         if(s == NULL) {
           //LOG_E(OMG, " client_traci_OMG::extractCommandStatus():  Tracker is NULL \n");
           //printf(" client_traci_OMG::extractCommandStatus():  Tracker is NULL \n");
           return success=0;
	}

	// validate the the message response from SUMO
	int storageLength_ = storageLength(s);

	// tracker currently points to the begining of the recieved data in the linked list        
        tracker = s;
185
//         storage *freeTracker = tracker;   // save it for calling free
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218

	int commandLength = readUnsignedByte();
        
	// CommandID needs to fit
        int rcvdCommandId = readUnsignedByte();
        if (rcvdCommandId == (int)commandId)
	{
                //printf("%d",rcvdCommandId);
                //LOG_E(OMG, " Server answered to command\n");
		//printf(" Server answered to command\n");
	}
	// Get result and description
	unsigned char result = readUnsignedByte();
	if (result != RTYPE_OK)
	{       
                
                printf(" Server returned error\n");
		return success=0;
		
	}
	
	if (result == RTYPE_OK) {
		// printf(" Server returned success\n");
		success=1;
        }

       	description = readString();
	// print description if needed 

	
        //free actual message content
	//depends on the message which is handled

219
220
221
222
223
        /*   if (commandId != CMD_GET_VEHICLE_VARIABLE)
        freeStorage(freeTracker);*/

        UNUSED_VARIABLE(commandLength);
        UNUSED_VARIABLE(storageLength_);
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242

	return success;
}


void commandSimulationStep(double time)
{	// progress the simulation in SUMO
	// reset is used to initalize the global parameters
	reset();
	// Send command
        writeUnsignedByte(1 + 1 + 4); // command length
	writeUnsignedByte(CMD_SIMSTEP2); // look up TraCIConstants.h
     //   writeInt((int)(time*1000)); // TraCI accepts time in milli seconds
         writeInt((int)(time)); // TraCI accepts time in milli seconds
	sendExact(storageLength(storageStart));

        extractCommandStatus(receiveExact(), CMD_SIMSTEP2, description);

        if (departed == NULL) {
243
    		departed = (string_list*) malloc(sizeof(string_list)); // departed MUST point to HEAD
244
245
246
247
    		departed->string = NULL;
    		departed->next = NULL;
  	}
  	if (arrived == NULL) {
248
    		arrived = (string_list*) malloc(sizeof(string_list));  // arrived MUST point to HEAD
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
    		arrived->string = NULL;
    		arrived->next = NULL;
  	}	

        /* if (departed == NULL) 
   		departed = (String_list)malloc(sizeof(String_list));  // departed MUST point to HEAD

  	if (arrived == NULL) 
    		arrived = (String_list)malloc(sizeof(String_list));  // departed MUST point to HEAD
        */

        processSubscriptions();

}  

void commandClose()
{	reset();
	// command length
    	writeUnsignedByte(0x02);
    	// command id
    	writeUnsignedByte(CMD_CLOSE);
	  	
	// send request message
         #ifdef STANDALONE 
    		printf("closing the socket... \n");
  	#else
   		LOG_N(OMG,"closing the socket... \n");
  	#endif
        sendExact(storageLength(storageStart));
        extractCommandStatus(receiveExact(), CMD_CLOSE, description);

}


void commandGetVehicleVariable(char *vehID, int varID)// malloc for vehID and varID depends on speed or position
{	
	reset();
    	int domID = CMD_GET_VEHICLE_VARIABLE;//0xa4 specific for get vehicle variable command
	
   	// command length
    	writeUnsignedByte(1 + 1 + 1 + 4 + (int)strlen(vehID));
    	// command id
    	writeUnsignedByte(CMD_GET_VEHICLE_VARIABLE);
    	// variable id
    	writeUnsignedByte(varID);
    	// object id
    	writeString(vehID);

    	// send request message
    	sendExact(storageLength(storageStart));
    	// receive answer message
        //receiveExact();
    	if (extractCommandStatus(receiveExact(), CMD_GET_VEHICLE_VARIABLE, description)){//<---RESPONSE_GET_VEHICLE_VARIABLE
	
    	// validate result state
        if(tracker == NULL) {
            // LOG_E(OMG, " client_traci_OMG::commandGetVehicleVariable():  Tracker is NULL \n");
            //printf(" client_traci_OMG::commandGetVehicleVariable():  Tracker is NULL \n");
            return;
	}

        int length = readUnsignedByte();
        if(length ==0)
	  length = readInt();
       	int cmdId =readUnsignedByte();

        if (cmdId != (CMD_GET_VEHICLE_VARIABLE+0x10)) {
		//LOG_E(OMG, " Wrong response recieved\n");
                 //printf(" Wrong response recieved\n");
            	return;
        }
        int VariableID = readUnsignedByte();
	char* rs = readString();

        int valueDataType = readUnsignedByte();
324
325
326
327
        UNUSED_VARIABLE(VariableID);
        UNUSED_VARIABLE(rs);
        UNUSED_VARIABLE(valueDataType);
        UNUSED_VARIABLE(domID);
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
    }
}

// TODO not working for now..need to find a way to get the same info without using CMD_SCENARIO (as not implemented by SUMO)
int commandGetMaxSUMONodesVariable()
{	
	reset();

        int max_car = 0;

   	// command length
    	writeUnsignedByte(1 + 1 + 1 + 1 + 4 + 1 + 1 + 4);
        // command id
        writeUnsignedByte(CMD_SCENARIO);
        // flag
	writeUnsignedByte(0x00); // GET command for the generic environment-related values
    	// command id
    	//writeUnsignedByte(CMD_SCENARIO);
        // domain id
	writeUnsignedByte(0x01); // vehicle
	
         writeInt(0); // first vehicular domain
    	
        // variable id
    	writeUnsignedByte(DOMVAR_MAXCOUNT); // get maximum number of vehicles

	writeUnsignedByte(TYPE_INTEGER); // get maximum number of vehicles
	writeInt(max_car); // get maximum number of vehicles
    
    	// send request message
    	sendExact(storageLength(storageStart));

    	// receive answer message
    	if (extractCommandStatus(receiveExact(), CMD_SCENARIO, description)){//<---RESPONSE_GET_VEHICLE_VARIABLE
	
    	  // validate result state
          
          if(tracker == NULL) {
            //LOG_E(OMG, " client_traci_OMG::commandGetMaxSUMONodesVariable():  Tracker is NULL \n");
            return -1;
	  }
	  int res = readUnsignedByte();
	  int Length = readUnsignedByte(); // to check with Int
       	  int cmdId =readUnsignedByte();
          if (cmdId != (CMD_SCENARIO)) {
		//LOG_E(OMG, " Wrong response recieved \n");
            	return -1;
          }

 	  int flag = readUnsignedByte(); 
 	  int dom = readUnsignedByte(); // domain
          int domID = readInt(); // domain ID
          int VariableID = readUnsignedByte();

          int valueDataType = readUnsignedByte();
    
	  if (valueDataType == TYPE_INTEGER) {
        	max_car = readInt();
		//LOG_N(OMG, " max Number SUMO nodes is: %f \n", max_car);
    	  } 
      	  else {
               //LOG_W(OMG, " No Matching Data Type Value \n"); 
	  }
391
392
393
394
395
396
      UNUSED_VARIABLE(res);
      UNUSED_VARIABLE(Length);
      UNUSED_VARIABLE(flag);
      UNUSED_VARIABLE(dom);
      UNUSED_VARIABLE(domID);
      UNUSED_VARIABLE(VariableID);
397
398
399
400
401
402
403
404
	}   

    return max_car;
}




405
void GetSpeed(node_struct* node, char * sumo_id)
406
407
408
409
410
411
{
    commandGetVehicleVariable(sumo_id, VAR_SPEED);
    double speed_double = readDouble();
    node->mob->speed = speed_double;
}

412
void GetPosition(node_struct* node, char * sumo_id)
413
414
415
416
417
418
419
420
421
422
{    
    commandGetVehicleVariable(sumo_id, VAR_POSITION);
    double x_double = readDouble();
    if (x_double < 0.0)
      x_double = 0.0;

   double y_double = readDouble();
    if (y_double < 0.0)
      y_double = 0.0;

423
424
    node->x_pos = x_double;
    node->y_pos = y_double;
425
}