├── Common ├── RDBHandler.cc ├── RDBHandler.hh ├── Sync.cc ├── Sync.hh ├── TC_VtdFramework │ └── viRDBIcd.h ├── rdbMapper.c └── viRDBTypes.h ├── ExampleACC.cpp ├── LICENSE └── README.md /Common/RDBHandler.hh: -------------------------------------------------------------------------------- 1 | /* =================================================== 2 | * file: RDBHandler.hh 3 | * --------------------------------------------------- 4 | * purpose: collection of RDB routines 5 | * --------------------------------------------------- 6 | * first edit: 24.01.2012 by M. Dupuis @ VIRES GmbH 7 | * =================================================== 8 | */ 9 | #ifndef _FRAMEWORK_RDB_HANDLER_HH 10 | #define _FRAMEWORK_RDB_HANDLER_HH 11 | 12 | /* ====== INCLUSIONS ====== */ 13 | #include 14 | #include 15 | #include 16 | 17 | namespace Framework 18 | { 19 | class RDBHandler 20 | { 21 | public: 22 | /** 23 | * convert a package ID to the corresponding element size 24 | * @param pkgId id of the package whose size is to be determined 25 | * @param extended true if the size of the extended package is to be determined 26 | * @return size of the package 27 | */ 28 | static size_t pkgId2size( unsigned int pkgId, bool extended = false ); 29 | 30 | /** 31 | * convert a package ID to the corresponding name 32 | * @param pkgId id of the package whose name is to be determined 33 | * @return name of the package 34 | */ 35 | static std::string pkgId2string( unsigned int pkgId ); 36 | 37 | /** 38 | * convert a coord type to the corresponding name 39 | * @param type id of the coordinate type whose name is to be determined 40 | * @return name of the package 41 | */ 42 | static std::string coordType2string( unsigned int type ); 43 | 44 | /** 45 | * convert an object category into a string 46 | * @param id id of the category 47 | * @return name of the category 48 | */ 49 | static std::string objectCategory2string( unsigned int id ); 50 | 51 | /** 52 | * convert an object category string into the numeric category 53 | * @param name name of the category 54 | * @return id of the category 55 | */ 56 | static unsigned int objectString2category( const std::string & name ); 57 | 58 | /** 59 | * convert an object type into a string 60 | * @param id id of the type 61 | * @return name of the type 62 | */ 63 | static std::string objectType2string( unsigned int id ); 64 | 65 | /** 66 | * convert an object type string into the numeric type 67 | * @param name name of the type 68 | * @return id of the type 69 | */ 70 | static unsigned int objectString2type( const std::string & name ); 71 | 72 | /** 73 | * print the contents of an RDB message 74 | * @param msg pointer to the message which is to be printed; 0 for current internal message 75 | * @param details if true, print the details, not only the headers 76 | * @param binDump create a binary dump of the message 77 | * @param csv print CSV version of the message 78 | * @param csvHeader print CSV header information only 79 | */ 80 | static void printMessage( RDB_MSG_t* msg = 0, bool details = false, bool binDump = false, bool csv = false, bool csvHeader = false ); 81 | 82 | /** 83 | * print the contents of an RDB message entry 84 | * @param entryHdr pointer to the entry header whose contents are to be printed 85 | * @param details if true, print the details, not only the headers 86 | * @param csv print CSV version of the entry 87 | * @param csvHeader print CSV header information only 88 | */ 89 | static void printMessageEntry( RDB_MSG_ENTRY_HDR_t* entryHdr, bool details = false, bool csv = false, bool csvHeader = false ); 90 | 91 | /** 92 | * print the contents of a single element of an RDB message entry 93 | * @param entryHdr pointer to the entry header belonging to the element 94 | * @param dataPtr pointer to the element's content 95 | * @param ident number of identation blanks to be used for printing 96 | * @param csv print CSV version of the entry 97 | * @param csvHeader print CSV header information only 98 | * @return true if element was printed 99 | */ 100 | static bool printMessageEntryElement( RDB_MSG_ENTRY_HDR_t* entryHdr, void* dataPtr, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 101 | 102 | /** 103 | * retrieve the primary numberic ID field of an element (to be used for filtering etc.) 104 | * @param elementType numeric identifier of the element type 105 | * @param element pointer to the element itself 106 | * @param[OUT] primaryId the primary ID that has been retrieved 107 | * @return true if primary element ID could be retrieved 108 | */ 109 | static bool retrievePrimaryId( unsigned short elementType, void* element, unsigned int & primaryId ); 110 | 111 | /** 112 | * add a packet or series of packets to an RDB message 113 | * @param msg pointer to the message that is to be extended / composed (0 for new message); may be altered! 114 | * @param simTime simulation time for which to compose the package 115 | * @param simFrame simulation frame for which to compose the package 116 | * @param pkgId id of the package that is to be added to the message 117 | * @param noElements number of elements of given package ID type that are to be added 118 | * @param extended true if an extended element is to be inserted 119 | * @param trailingData size of trailing data of each element 120 | * @param isCustom if true, message is considered a custom message and size per element will be derived from argument "trailingData" 121 | * @return pointer where to start inserting the data, otherwise 0; message pointer may be altered! 122 | */ 123 | static void* addPackage( RDB_MSG_t* & msg, const double & simTime, const unsigned int & simFrame, 124 | unsigned int pkgId, unsigned int noElements, bool extended, size_t trailingData, bool isCustom = false ); 125 | 126 | /** 127 | * add a custom packet or series of packets to an RDB message 128 | * @param msg pointer to the message that is to be extended / composed (0 for new message); may be altered! 129 | * @param simTime simulation time for which to compose the package 130 | * @param simFrame simulation frame for which to compose the package 131 | * @param pkgId id of the package that is to be added to the message 132 | * @param noElements number of elements of given package ID type that are to be added 133 | * @param elementSize data size of each element 134 | * @return pointer where to start inserting the data, otherwise 0; message pointer may be altered! 135 | */ 136 | static void* addCustomPackage( RDB_MSG_t* & msg, const double & simTime, const unsigned int & simFrame, 137 | unsigned int pkgId, unsigned int noElements, size_t elementSize ); 138 | 139 | /** 140 | * retrieve the pointer to the first entry of a given type from a given message 141 | * @param msg pointer to the message that is to be searched for the entry 142 | * @param pkgId id (i.e. type) of the entry that is to be retrieved 143 | * @param noElements number of elements of given entry type that have been found (will be altered) 144 | * @param extended true if an extended element is to be retrieved 145 | * @return pointer to first element of the requested entry type or 0 if none has been found. 146 | */ 147 | static void* getFirstEntry( const RDB_MSG_t* msg, unsigned int pkgId, unsigned int & noElements, bool extended ); 148 | 149 | /** 150 | * retrieve the pointer to the first entry header of a given type from a given message 151 | * @param msg pointer to the message that is to be searched for the entry 152 | * @param pkgId id (i.e. type) of the entry that is to be retrieved 153 | * @param extended true if an extended element is to be retrieved 154 | * @return pointer to first entry header of the requested entry type or 0 if none has been found. 155 | */ 156 | static RDB_MSG_ENTRY_HDR_t* getEntryHdr( const RDB_MSG_t* msg, unsigned int pkgId, bool extended ); 157 | 158 | /** 159 | * create an ident string of given length 160 | * @param ident number of spaces to use for identing 161 | * @return pointer to ident character string 162 | */ 163 | static char* getIdentString( unsigned char ident ); 164 | 165 | /** 166 | * print a geometry info 167 | * @param info the info which is to be printed 168 | * @param ident number of spaces to use for identing 169 | * @param csv print CSV version of the data 170 | * @param csvHeader print CSV header information only 171 | */ 172 | static void print( const RDB_GEOMETRY_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 173 | 174 | /** 175 | * print a coordinate system 176 | * @param info the info which is to be printed 177 | * @param ident number of spaces to use for identing 178 | * @param csv print CSV version of the data 179 | * @param csvHeader print CSV header information only 180 | */ 181 | static void print( const RDB_COORD_SYSTEM_t & state, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 182 | 183 | /** 184 | * print a co-ordinate 185 | * @param info the info which is to be printed 186 | * @param ident number of spaces to use for identing 187 | * @param csv print CSV version of the data 188 | * @param csvHeader print CSV header information only 189 | */ 190 | static void print( const RDB_COORD_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 191 | 192 | /** 193 | * print the whole bunch of road information 194 | * @param info reference to the road information 195 | * @param ident number of spaces to use for identing 196 | * @param csv print CSV version of the data 197 | * @param csvHeader print CSV header information only 198 | */ 199 | static void print( const RDB_ROAD_POS_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 200 | 201 | /** 202 | * print information about lanes 203 | * @param info reference to the lane information 204 | * @param ident number of spaces to use for identing 205 | * @param csv print CSV version of the data 206 | * @param csvHeader print CSV header information only 207 | */ 208 | static void print( const RDB_LANE_INFO_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 209 | 210 | /** 211 | * print the whole bunch of road mark information 212 | * @param info reference to the road mark information 213 | * @param ident number of spaces to use for identing 214 | * @param csv print CSV version of the data 215 | * @param csvHeader print CSV header information only 216 | */ 217 | static void print( const RDB_ROADMARK_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 218 | 219 | /** 220 | * print an object configuration 221 | * @param info the configuration which is to be printed 222 | * @param ident number of spaces to use for identing 223 | * @param csv print CSV version of the data 224 | * @param csvHeader print CSV header information only 225 | */ 226 | static void print( const RDB_OBJECT_CFG_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 227 | 228 | /** 229 | * print an object state 230 | * @param state the state which is to be printed 231 | * @param extended true if extened object state is to be printed 232 | * @param ident number of spaces to use for identing 233 | * @param csv print CSV version of the data 234 | * @param csvHeader print CSV header information only 235 | */ 236 | static void print( const RDB_OBJECT_STATE_t & state, bool extended = false, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 237 | 238 | /** 239 | * print information about vehicle systems 240 | * @param info reference to the data object 241 | * @param ident number of spaces to use for identing 242 | * @param csv print CSV version of the data 243 | * @param csvHeader print CSV header information only 244 | */ 245 | static void print( const RDB_VEHICLE_SYSTEMS_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 246 | 247 | /** 248 | * print a vehicle's setup 249 | * @param info the info which is to be printed 250 | * @param ident number of spaces to use for identing 251 | * @param csv print CSV version of the data 252 | * @param csvHeader print CSV header information only 253 | */ 254 | static void print( const RDB_VEHICLE_SETUP_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 255 | 256 | /** 257 | * print engine information 258 | * @param info the info which is to be printed 259 | * @param extended true if extened object state is to be printed 260 | * @param ident number of spaces to use for identing 261 | * @param csv print CSV version of the data 262 | * @param csvHeader print CSV header information only 263 | */ 264 | static void print( const RDB_ENGINE_t & info, bool extended = false, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 265 | 266 | /** 267 | * print drivetrain information 268 | * @param info the info which is to be printed 269 | * @param extended true if extened object state is to be printed 270 | * @param ident number of spaces to use for identing 271 | * @param csv print CSV version of the data 272 | * @param csvHeader print CSV header information only 273 | */ 274 | static void print( const RDB_DRIVETRAIN_t & info, bool extended = false, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 275 | 276 | /** 277 | * print wheel information 278 | * @param info the info which is to be printed 279 | * @param extended true if extened object state is to be printed 280 | * @param ident number of spaces to use for identing 281 | * @param csv print CSV version of the data 282 | * @param csvHeader print CSV header information only 283 | */ 284 | static void print( const RDB_WHEEL_t & info, bool extended = false, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 285 | 286 | /** 287 | * print pedestrian animation information 288 | * @param info the info which is to be printed 289 | * @param ident number of spaces to use for identing 290 | * @param csv print CSV version of the data 291 | * @param csvHeader print CSV header information only 292 | */ 293 | static void print( const RDB_PED_ANIMATION_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 294 | 295 | /** 296 | * print information about a sensor 297 | * @param info reference to the sensor state 298 | * @param ident number of spaces to use for identing 299 | * @param csv print CSV version of the data 300 | * @param csvHeader print CSV header information only 301 | */ 302 | static void print( const RDB_SENSOR_STATE_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 303 | 304 | /** 305 | * print information about sensor objects 306 | * @param info reference to the sensor object 307 | * @param ident number of spaces to use for identing 308 | * @param csv print CSV version of the data 309 | * @param csvHeader print CSV header information only 310 | */ 311 | static void print( const RDB_SENSOR_OBJECT_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 312 | 313 | /** 314 | * print the whole bunch of camera information 315 | * @param info reference to the camera information 316 | * @param ident number of spaces to use for identing 317 | * @param csv print CSV version of the data 318 | * @param csvHeader print CSV header information only 319 | */ 320 | static void print( const RDB_CAMERA_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 321 | 322 | /** 323 | * print the whole bunch of contact point information 324 | * @param info reference to the contact point information 325 | * @param ident number of spaces to use for identing 326 | * @param csv print CSV version of the data 327 | * @param csvHeader print CSV header information only 328 | */ 329 | static void print( const RDB_CONTACT_POINT_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 330 | 331 | /** 332 | * print information about a single traffic sign 333 | * @param info reference to the traffic sign's information 334 | * @param ident number of spaces to use for identing 335 | * @param csv print CSV version of the data 336 | * @param csvHeader print CSV header information only 337 | */ 338 | static void print( const RDB_TRAFFIC_SIGN_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 339 | 340 | /** 341 | * print information about a single road state 342 | * @param info reference to the road state's information 343 | * @param ident number of spaces to use for identing 344 | * @param csv print CSV version of the data 345 | * @param csvHeader print CSV header information only 346 | */ 347 | static void print( const RDB_ROAD_STATE_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 348 | 349 | /** 350 | * print information about an image data block 351 | * @param info reference to the data object 352 | * @param ident number of spaces to use for identing 353 | * @param csv print CSV version of the data 354 | * @param csvHeader print CSV header information only 355 | */ 356 | static void print( const RDB_IMAGE_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 357 | 358 | /** 359 | * print information about an light source 360 | * @param info reference to the data object 361 | * @param ident number of spaces to use for identing 362 | * @param csv print CSV version of the data 363 | * @param csvHeader print CSV header information only 364 | */ 365 | static void print( const RDB_LIGHT_SOURCE_t & info, bool extended = false, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 366 | 367 | /** 368 | * print information about environment package 369 | * @param info reference to the information 370 | * @param ident number of spaces to use for identing 371 | * @param csv print CSV version of the data 372 | * @param csvHeader print CSV header information only 373 | */ 374 | static void print( const RDB_ENVIRONMENT_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 375 | 376 | /** 377 | * print information about trigger package 378 | * @param info reference to the information 379 | * @param ident number of spaces to use for identing 380 | * @param csv print CSV version of the data 381 | * @param csvHeader print CSV header information only 382 | */ 383 | static void print( const RDB_TRIGGER_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 384 | 385 | /** 386 | * print information about driver controls 387 | * @param info reference to the driver control information 388 | * @param ident number of spaces to use for identing 389 | * @param csv print CSV version of the data 390 | * @param csvHeader print CSV header information only 391 | */ 392 | static void print( const RDB_DRIVER_CTRL_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 393 | 394 | /** 395 | * print information about a single traffic light 396 | * @param info reference to the traffic light's information 397 | * @param extended if true, print extended state 398 | * @param ident number of spaces to use for identing 399 | * @param csv print CSV version of the data 400 | * @param csvHeader print CSV header information only 401 | */ 402 | static void print( const RDB_TRAFFIC_LIGHT_t & info, bool extended = false, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 403 | 404 | /** 405 | * print information about sync package 406 | * @param info reference to the information 407 | * @param ident number of spaces to use for identing 408 | * @param csv print CSV version of the data 409 | * @param csvHeader print CSV header information only 410 | */ 411 | static void print( const RDB_SYNC_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 412 | 413 | /** 414 | * print information about driver perception package 415 | * @param info reference to the information 416 | * @param ident number of spaces to use for identing 417 | * @param csv print CSV version of the data 418 | * @param csvHeader print CSV header information only 419 | */ 420 | static void print( const RDB_DRIVER_PERCEPTION_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 421 | 422 | /** 423 | * print information about tone mapping package 424 | * @param info reference to the information 425 | * @param ident number of spaces to use for identing 426 | * @param csv print CSV version of the data 427 | * @param csvHeader print CSV header information only 428 | */ 429 | static void print( const RDB_FUNCTION_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 430 | 431 | /** 432 | * print information about road query package 433 | * @param info reference to the information 434 | * @param ident number of spaces to use for identing 435 | * @param csv print CSV version of the data 436 | * @param csvHeader print CSV header information only 437 | */ 438 | static void print( const RDB_ROAD_QUERY_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 439 | 440 | /** 441 | * print point information 442 | * @param info reference to the information 443 | * @param ident number of spaces to use for identing 444 | * @param csv print CSV version of the data 445 | * @param csvHeader print CSV header information only 446 | */ 447 | static void print( const RDB_POINT_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 448 | 449 | /** 450 | * print a trajectory information 451 | * @param info reference to the trajectory information 452 | * @param ident number of spaces to use for identing 453 | * @param csv print CSV version of the data 454 | * @param csvHeader print CSV header information only 455 | */ 456 | static void print( const RDB_TRAJECTORY_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 457 | 458 | /** 459 | * print an information from dynamics to steering 460 | * @param info reference to the information 461 | * @param ident number of spaces to use for identing 462 | * @param csv print CSV version of the data 463 | * @param csvHeader print CSV header information only 464 | */ 465 | static void print( const RDB_DYN_2_STEER_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 466 | 467 | /** 468 | * print an information from steering to dynamics 469 | * @param info reference to the information 470 | * @param ident number of spaces to use for identing 471 | * @param csv print CSV version of the data 472 | * @param csvHeader print CSV header information only 473 | */ 474 | static void print( const RDB_STEER_2_DYN_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 475 | 476 | /** 477 | * print a proxy information 478 | * @param info reference to the information 479 | * @param ident number of spaces to use for identing 480 | * @param csv print CSV version of the data 481 | * @param csvHeader print CSV header information only 482 | */ 483 | static void print( const RDB_PROXY_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 484 | 485 | /** 486 | * print a motion system information 487 | * @param info reference to the information 488 | * @param ident number of spaces to use for identing 489 | * @param csv print CSV version of the data 490 | * @param csvHeader print CSV header information only 491 | */ 492 | static void print( const RDB_MOTION_SYSTEM_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 493 | 494 | /** 495 | * print a scoring information 496 | * @param info reference to the information 497 | * @param ident number of spaces to use for identing 498 | * @param csv print CSV version of the data 499 | * @param csvHeader print CSV header information only 500 | */ 501 | static void print( const RDB_CUSTOM_SCORING_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 502 | 503 | /** 504 | * print a track control information 505 | * @param info reference to the information 506 | * @param ident number of spaces to use for identing 507 | * @param csv print CSV version of the data 508 | * @param csvHeader print CSV header information only 509 | */ 510 | static void print( const RDB_CUSTOM_OBJECT_CTRL_TRACK_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 511 | 512 | /** 513 | * print an SCP information 514 | * @param info reference to the information 515 | * @param ident number of spaces to use for identing 516 | * @param csv print CSV version of the data 517 | * @param csvHeader print CSV header information only 518 | */ 519 | static void print( const RDB_SCP_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 520 | 521 | /** 522 | * print the a FREESPACE information 523 | * @param info reference to the information 524 | * @param ident number of spaces to use for identing 525 | * @param csv print CSV version of the data 526 | * @param csvHeader print CSV header information only 527 | */ 528 | static void print( const RDB_FREESPACE_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 529 | 530 | /** 531 | * print a DYN_EL_SWITCH information 532 | * @param info reference to the information 533 | * @param ident number of spaces to use for identing 534 | * @param csv print CSV version of the data 535 | * @param csvHeader print CSV header information only 536 | */ 537 | static void print( const RDB_DYN_EL_SWITCH_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 538 | 539 | /** 540 | * print a DYN_EL_DOF information 541 | * @param info reference to the information 542 | * @param ident number of spaces to use for identing 543 | * @param csv print CSV version of the data 544 | * @param csvHeader print CSV header information only 545 | */ 546 | static void print( const RDB_DYN_EL_DOF_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 547 | 548 | /** 549 | * print an IG_FRAME information 550 | * @param info reference to the information 551 | * @param ident number of spaces to use for identing 552 | * @param csv print CSV version of the data 553 | * @param csvHeader print CSV header information only 554 | */ 555 | static void print( const RDB_IG_FRAME_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 556 | 557 | /** 558 | * print the real-time performance information 559 | * @param info reference to the information 560 | * @param ident number of spaces to use for identing 561 | * @param csv print CSV version of the data 562 | * @param csvHeader print CSV header information only 563 | */ 564 | static void print( const RDB_RT_PERFORMANCE_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 565 | 566 | /** 567 | * print information about a matrix 568 | * @param pData pointer to the data structure 569 | * @param width width of the matrix 570 | * @param height height of the matrix 571 | * @param csv print CSV version of the data 572 | * @param csvHeader print CSV header information only 573 | */ 574 | static void printMatrix( int *pData, unsigned int width, unsigned int height, bool csv = false, bool csvHeader = false ); 575 | 576 | /** 577 | * print information about a ray-tracing ray 578 | * @param info reference to the information 579 | * @param ident number of spaces to use for identing 580 | * @param csv print CSV version of the data 581 | * @param csvHeader print CSV header information only 582 | */ 583 | static void print( const RDB_RAY_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 584 | 585 | /** 586 | * print information about a symbol state 587 | * @param info reference to the information 588 | * @param ident number of spaces to use for identing 589 | * @param csv print CSV version of the data 590 | * @param csvHeader print CSV header information only 591 | */ 592 | static void print( const RDB_SYMBOL_STATE_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 593 | 594 | /** 595 | * print information about a radar hit point 596 | * @param info reference to the information 597 | * @param ident number of spaces to use for identing 598 | * @param csv print CSV version of the data 599 | * @param csvHeader print CSV header information only 600 | */ 601 | static void print( const RDB_RADAR_HIT_POINT_t & info, unsigned char ident = 0, bool csv = false, bool csvHeader = false ); 602 | 603 | public: 604 | /** 605 | * constructor 606 | */ 607 | explicit RDBHandler(); 608 | 609 | /** 610 | * Destroy the class. 611 | */ 612 | virtual ~RDBHandler(); 613 | 614 | /** 615 | * (re-) initialize the RDB message, i.e. internally held data 616 | */ 617 | void initMsg(); 618 | 619 | /** 620 | * add a packet or series of packets to an RDB message 621 | * @param simTime simulation time for which to compose the package 622 | * @param simFrmae simulation frame for which to compose the package 623 | * @param pkgId id of the package that is to be added to the message 624 | * @param noElements number of elements of given package ID type that are to be added 625 | * @param extended true if an extended element is to be inserted 626 | * @param trailingData size of trailing data of each element 627 | * @param isCustom if true, package is considered a custom package and size per element will be derived from argument "trailingData" 628 | * @return pointer where to start inserting the data, otherwise 0 629 | */ 630 | void* addPackage( const double & simTime, const unsigned int & simFrame, 631 | unsigned int pkgId, unsigned int noElements = 1, bool extended = false, 632 | size_t trailingData = 0, bool isCustom = false ); 633 | 634 | /** 635 | * add a custom packet or series of packets to an RDB message 636 | * @param simTime simulation time for which to compose the package 637 | * @param simFrmae simulation frame for which to compose the package 638 | * @param pkgId id of the package that is to be added to the message 639 | * @param noElements number of elements of given package ID type that are to be added 640 | * @param elementSize size of each element [byte] 641 | * @return pointer where to start inserting the data, otherwise 0 642 | */ 643 | void* addCustomPackage( const double & simTime, const unsigned int & simFrame, 644 | unsigned int pkgId, unsigned int noElements = 1, size_t elementSize = 0 ); 645 | 646 | /** 647 | * get a pointer to the message that is currently being composed 648 | * @return pointer to current RDB message 649 | */ 650 | RDB_MSG_t* getMsg(); 651 | 652 | /** 653 | * get a pointer to the message header that is currently being composed 654 | * @return pointer to current RDB message header 655 | */ 656 | RDB_MSG_HDR_t* getMsgHdr(); 657 | 658 | /** 659 | * get the total size of the current message 660 | * @return total size of the message which is currently being composed 661 | */ 662 | size_t getMsgTotalSize(); 663 | 664 | /** 665 | * retrieve the pointer to the first entry of a given type from the internally held message 666 | * @param pkgId id (i.e. type) of the entry that is to be retrieved 667 | * @param noElements number of elements of given entry type that have been found (will be altered) 668 | * @param extended true if an extended element is to be retrieved 669 | * @return pointer to first element of the requested entry type or 0 if none has been found. 670 | */ 671 | void* getFirstEntry( unsigned int pkgId, unsigned int & noElements, bool extended ); 672 | 673 | /** 674 | * retrieve the pointer to the first entry header of a given type from the internally held message 675 | * @param pkgId id (i.e. type) of the entry that is to be retrieved 676 | * @param extended true if an extended element is to be retrieved 677 | * @return pointer to first entry header of the requested entry type or 0 if none has been found. 678 | */ 679 | RDB_MSG_ENTRY_HDR_t* getEntryHdr( unsigned int pkgId, bool extended ); 680 | 681 | /** 682 | * configure a shared memory segment for the use with RDB 683 | * @param startPtr pointer to the start of the shared memory segment 684 | * @param noBuffers number of buffers which are to be placed within the segment 685 | * @param totalSize total size of SHM segment 686 | * @return true if successful 687 | */ 688 | bool shmConfigure( void *startPtr, unsigned int noBuffers, unsigned int totalSize = 0 ); 689 | 690 | /** 691 | * set the shared memory pointer 692 | * @param shmAddr address of the shared memory segment 693 | * @return true if successful 694 | */ 695 | bool shmSetAddress( void* shmAddr ); 696 | 697 | /** 698 | * get the pointer to the shared memory header 699 | */ 700 | RDB_SHM_HDR_t* shmGetHdr(); 701 | 702 | /** 703 | * get the pointer to a shared memory buffer's info segment 704 | * @param index index of the buffer 705 | * @return pointer to the info segment of the respective shared memory buffer 706 | */ 707 | RDB_SHM_BUFFER_INFO_t* shmBufferGetInfo( unsigned int index ); 708 | 709 | /** 710 | * get the pointer to a shared memory buffer's data segment 711 | * @param index index of the buffer 712 | * @return pointer to the data segment of the respective shared memory buffer 713 | */ 714 | void* shmBufferGetPtr( unsigned int index ); 715 | 716 | /** 717 | * update the header information of the shared memory according to buffer data 718 | */ 719 | void shmHdrUpdate(); 720 | 721 | /** 722 | * set the size of a shared memory buffer's data segment 723 | * @param index index of the buffer 724 | * @param size size of the buffer's data segment 725 | */ 726 | void shmBufferSetSize( unsigned int index, size_t size ); 727 | 728 | /** 729 | * get the size of a shared memory buffer's data segment 730 | * @param index index of the buffer 731 | */ 732 | size_t shmBufferGetSize( unsigned int index ); 733 | 734 | /** 735 | * set the id of a shared memory buffer 736 | * @param index index of the buffer 737 | * @param id id of the buffer 738 | */ 739 | void shmBufferSetId( unsigned int index, unsigned int id ); 740 | 741 | /** 742 | * set the flags of a shared memory buffer 743 | * @param index index of the buffer 744 | * @param flags flags of the buffer 745 | */ 746 | void shmBufferSetFlags( unsigned int index, unsigned int flags ); 747 | 748 | /** 749 | * add the flags of a shared memory buffer 750 | * @param index index of the buffer 751 | * @param flags flags of the buffer 752 | */ 753 | void shmBufferAddFlags( unsigned int index, unsigned int flags ); 754 | 755 | /** 756 | * release the flags of a shared memory buffer 757 | * @param index index of the buffer 758 | * @param flags flags of the buffer 759 | */ 760 | void shmBufferReleaseFlags( unsigned int index, unsigned int flags ); 761 | 762 | /** 763 | * get the flags of a shared memory buffer 764 | * @param index index of the buffer 765 | * @return flags of the buffer 766 | */ 767 | unsigned int shmBufferGetFlags( unsigned int index ); 768 | 769 | /** 770 | * check the flags of a shared memory buffer for a given mask 771 | * @param index index of the buffer 772 | * @param mask mask against which to check 773 | * @return true if mask is contained in shm flags 774 | */ 775 | bool shmBufferHasFlags( unsigned int index, unsigned int mask ); 776 | 777 | /** 778 | * copy the current message to the shared memory, replacing existing data 779 | * @param index index of the buffer to which the message shall be copied 780 | * @param relocateBuffers if true, buffer locations (i.e. offsets) will be adjusted to size of copied data; this requires buffers to be filled sequentially! 781 | * @return true if successful 782 | */ 783 | bool mapMsgToShm( unsigned int index, bool relocateBuffers = true ); 784 | 785 | /** 786 | * add a message to the shared memory, extending existing data 787 | * @param index index of the buffer to which the message shall be copied 788 | * @param msg pointer to the message that is to be transferred to SHM 789 | * @return true if successful 790 | */ 791 | bool addMsgToShm( unsigned int index, RDB_MSG_t* msg ); 792 | 793 | /** 794 | * get the usage of an SHM buffer 795 | * @param index index of the buffer which shall be queried 796 | * @return number of bytes occupied in the buffer 797 | */ 798 | unsigned int shmBufferGetUsedSize( unsigned int index ); 799 | 800 | /** 801 | * clear the given SHM buffer 802 | * @param index index of the buffer to which is to be cleared 803 | * @param force force clearing of locked buffers 804 | * @return true if successful 805 | */ 806 | bool shmBufferClear( unsigned int index, bool force = false ); 807 | 808 | /** 809 | * check if a given SHM buffer is locked 810 | * @param index index of the buffer to which is to be checked 811 | * @return true if buffer is locked 812 | */ 813 | bool shmBufferIsLocked( unsigned int index ); 814 | 815 | /** 816 | * lock a given SHM buffer 817 | * @param index index of the buffer to which is to be locked 818 | * @return true if buffer could be locked 819 | */ 820 | bool shmBufferLock( unsigned int index ); 821 | 822 | /** 823 | * release the lock of a given SHM buffer 824 | * @param index index of the buffer to which is to be released 825 | * @return true if buffer could be released 826 | */ 827 | bool shmBufferRelease( unsigned int index ); 828 | 829 | /** 830 | * get the number of buffers in the SHM segment 831 | * @return number of buffers in SHM segment 832 | */ 833 | unsigned int shmGetNoBuffers(); 834 | 835 | /** 836 | * print sizes of all structures 837 | */ 838 | void printPackageSizes(); 839 | 840 | /** 841 | * parse an RDB message 842 | * @param msg pointer to the message which is to be parsed; 843 | */ 844 | virtual void parseMessage( RDB_MSG_t* msg ); 845 | 846 | /** 847 | * parse an RDB message entry 848 | * @param entry pointer to the message entry which is to be parsed; 849 | * @param simTime simulation time of the message 850 | * @param simFrame simulation frame of the message 851 | */ 852 | virtual void parseMessageEntry( RDB_MSG_ENTRY_HDR_t* entryHdr, const double & simTime, const unsigned int & simFrame ); 853 | 854 | protected: 855 | /** 856 | * parse the information of a message entry 857 | * @param simTime simulation time of the message 858 | * @param simFrame simulation frame of the message 859 | * @param pkgId id of the package 860 | * @param flags flags of the message entry (e.g. EXTENDED message entry) 861 | * @param elemId id (index) of the current element in the vector of elements of this specific type as contained in the message 862 | * @param totalElem total number of elements in the vector of this specific type as contained in the message 863 | */ 864 | virtual void parseMessageEntryInfo( const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 865 | 866 | /** 867 | * parse routines for the frame limits 868 | * @param simTime simulation time of the message 869 | * @param simFrame simulation frame of the message 870 | */ 871 | virtual void parseStartOfFrame( const double & simTime, const unsigned int & simFrame ); 872 | virtual void parseEndOfFrame( const double & simTime, const unsigned int & simFrame ); 873 | 874 | /** 875 | * parse routines for the various message types 876 | * @param msg pointer to the message which is to be parsed; 877 | * @param data pointer to the entry data that is to be parsed 878 | * @param simTime simulation time of the message 879 | * @param simFrame simulation frame of the message 880 | * @param pkgId id of the package 881 | * @param flags flags of the message entry (e.g. EXTENDED message entry) 882 | * @param elemId id (index) of the current element in the vector of elements of this specific type as contained in the message 883 | * @param totalElem total number of elements in the vector of this specific type as contained in the message 884 | */ 885 | virtual void parseEntry( RDB_GEOMETRY_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 886 | virtual void parseEntry( RDB_COORD_SYSTEM_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 887 | virtual void parseEntry( RDB_COORD_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 888 | virtual void parseEntry( RDB_ROAD_POS_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 889 | virtual void parseEntry( RDB_LANE_INFO_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 890 | virtual void parseEntry( RDB_ROADMARK_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 891 | virtual void parseEntry( RDB_OBJECT_CFG_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 892 | virtual void parseEntry( RDB_OBJECT_STATE_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 893 | virtual void parseEntry( RDB_VEHICLE_SYSTEMS_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 894 | virtual void parseEntry( RDB_VEHICLE_SETUP_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 895 | virtual void parseEntry( RDB_ENGINE_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 896 | virtual void parseEntry( RDB_DRIVETRAIN_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 897 | virtual void parseEntry( RDB_WHEEL_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 898 | virtual void parseEntry( RDB_PED_ANIMATION_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 899 | virtual void parseEntry( RDB_SENSOR_STATE_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 900 | virtual void parseEntry( RDB_SENSOR_OBJECT_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 901 | virtual void parseEntry( RDB_CAMERA_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 902 | virtual void parseEntry( RDB_CONTACT_POINT_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 903 | virtual void parseEntry( RDB_TRAFFIC_SIGN_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 904 | virtual void parseEntry( RDB_ROAD_STATE_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 905 | virtual void parseEntry( RDB_IMAGE_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 906 | virtual void parseEntry( RDB_LIGHT_SOURCE_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 907 | virtual void parseEntry( RDB_ENVIRONMENT_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 908 | virtual void parseEntry( RDB_TRIGGER_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 909 | virtual void parseEntry( RDB_DRIVER_CTRL_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 910 | virtual void parseEntry( RDB_TRAFFIC_LIGHT_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 911 | virtual void parseEntry( RDB_SYNC_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 912 | virtual void parseEntry( RDB_DRIVER_PERCEPTION_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 913 | virtual void parseEntry( RDB_FUNCTION_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 914 | virtual void parseEntry( RDB_ROAD_QUERY_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 915 | virtual void parseEntry( RDB_POINT_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 916 | virtual void parseEntry( RDB_TRAJECTORY_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 917 | virtual void parseEntry( RDB_CUSTOM_SCORING_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 918 | virtual void parseEntry( RDB_DYN_2_STEER_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 919 | virtual void parseEntry( RDB_SCP_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 920 | virtual void parseEntry( RDB_STEER_2_DYN_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 921 | virtual void parseEntry( RDB_PROXY_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 922 | virtual void parseEntry( RDB_MOTION_SYSTEM_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 923 | virtual void parseEntry( RDB_FREESPACE_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 924 | virtual void parseEntry( RDB_DYN_EL_SWITCH_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 925 | virtual void parseEntry( RDB_DYN_EL_DOF_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 926 | virtual void parseEntry( RDB_IG_FRAME_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 927 | virtual void parseEntry( RDB_RT_PERFORMANCE_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 928 | virtual void parseEntry( RDB_CUSTOM_OBJECT_CTRL_TRACK_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 929 | virtual void parseEntry( RDB_RAY_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 930 | virtual void parseEntry( RDB_SYMBOL_STATE_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 931 | virtual void parseEntry( RDB_RADAR_HIT_POINT_t * data, const double & simTime, const unsigned int & simFrame, const unsigned short & pkgId, const unsigned short & flags, const unsigned int & elemId, const unsigned int & totalElem ); 932 | 933 | private: 934 | /** 935 | * the actual RDB message that is composed 936 | */ 937 | RDB_MSG_t* mMsg; 938 | 939 | /** 940 | * pointer to the start of the shared memory segment 941 | */ 942 | RDB_SHM_HDR_t* mShmHdr; 943 | }; 944 | } // namespace Framework 945 | #endif /* _FRAMEWORK_RDB_HANDLER_HH */ 946 | -------------------------------------------------------------------------------- /Common/Sync.cc: -------------------------------------------------------------------------------- 1 | /* =================================================== 2 | * file: Clock.cpp 3 | * --------------------------------------------------- 4 | * purpose: an clock utility class 5 | * --------------------------------------------------- 6 | * first edit: 27.04.2003 by M. Dupuis @ VIRES GmbH 7 | * last mod.: 27.04.2003 by M. Dupuis @ VIRES GmbH 8 | * =================================================== 9 | */ 10 | 11 | // ====== INCLUSIONS ====== 12 | #include 13 | #include 14 | #include 15 | #include "Sync.hh" 16 | 17 | extern "C" { 18 | static volatile sig_atomic_t sGotSignal = 0; 19 | 20 | static void SyncHandleAlarm( int aSignal ) { 21 | if ( aSignal == SIGALRM ) { 22 | ++sGotSignal; 23 | } 24 | } 25 | } 26 | 27 | namespace Util { 28 | 29 | Sync::Sync() : mFrameTime( 0.01 ), 30 | mInitLevel( 0 ), 31 | mFreeRun( true ) 32 | { 33 | } 34 | 35 | Sync::~Sync() 36 | { 37 | } 38 | 39 | void 40 | Sync::setFrameTime( const double & time ) 41 | { 42 | mFrameTime = time; 43 | } 44 | 45 | const double & 46 | Sync::getFrameTime() const 47 | { 48 | return mFrameTime; 49 | } 50 | 51 | void 52 | Sync::wait() 53 | { 54 | if( mFreeRun ) 55 | return; 56 | 57 | // wait until timer interrupt 58 | sigset_t zeromask; 59 | ::sigemptyset( &zeromask ); 60 | while ( sGotSignal == 0 ) 61 | ::sigsuspend( &zeromask ); 62 | --sGotSignal; 63 | } 64 | 65 | bool 66 | Sync::start() 67 | { 68 | sGotSignal = 0; 69 | mInitLevel = 0; 70 | 71 | if ( mFrameTime < 1.E-3 ) 72 | { 73 | std::cerr << "WARNING: Sync::start: frametime under 1ms not supported! Switching to mode ." << std::endl; 74 | stop(); 75 | return false; 76 | } 77 | 78 | // set SIGALRM handler 79 | if( mInitLevel < 1 ) 80 | { 81 | struct sigaction act; 82 | act.sa_handler = SyncHandleAlarm; 83 | ::sigemptyset( &act.sa_mask ); 84 | act.sa_flags = 0; 85 | if ( ::sigaction( SIGALRM, &act, &mOldAction ) < 0 ) 86 | { 87 | std::cerr << "Sync::start: sigaction error" << std::endl; 88 | return false; 89 | } 90 | mInitLevel = 1; 91 | } 92 | 93 | // block signal 94 | if( mInitLevel < 2 ) 95 | { 96 | sigset_t newmask; 97 | ::sigemptyset( &newmask ); 98 | ::sigaddset( &newmask, SIGALRM ); 99 | if ( ::sigprocmask( SIG_BLOCK, &newmask, &mOldSigMask ) < 0 ) 100 | { 101 | std::cerr << "Sync::start: sigprocmask error" << std::endl; 102 | return false; 103 | } 104 | mInitLevel = 2; 105 | } 106 | 107 | // set interval timer 108 | struct itimerval itval; 109 | // avoid int overflows 110 | time_t isec = static_cast( floor( mFrameTime ) ); 111 | int iusec = static_cast( floor( ( mFrameTime - isec ) * 1000000.0 + 0.5 ) ); 112 | 113 | while ( iusec >= 1000000 ) 114 | { 115 | iusec -= 1000000; 116 | isec += 1; 117 | } 118 | 119 | itval.it_interval.tv_sec = isec; 120 | itval.it_interval.tv_usec = iusec; 121 | itval.it_value = itval.it_interval; 122 | 123 | if ( ::setitimer( ITIMER_REAL, &itval, 0 ) < 0 ) 124 | { 125 | std::cerr << "Sync::start: setitimer error" << std::endl; 126 | return false; 127 | } 128 | 129 | mInitLevel = 3; 130 | 131 | // test the timer interval 132 | struct itimerval itvalSet; 133 | if( ::getitimer( ITIMER_REAL, &itvalSet ) < 0 ) 134 | { 135 | std::cerr << "Sync::start: getitimer error" << std::endl; 136 | return false; 137 | } 138 | 139 | mFreeRun = false; 140 | 141 | return true; 142 | } 143 | 144 | void 145 | Sync::stop() 146 | { 147 | if ( mInitLevel >= 3 ) 148 | { 149 | // clear interval timer 150 | struct itimerval itval; 151 | itval.it_interval.tv_sec = 0; 152 | itval.it_interval.tv_usec = 0; 153 | itval.it_value.tv_sec = 0; 154 | itval.it_value.tv_usec = 0; 155 | ::setitimer( ITIMER_REAL, &itval, 0 ); 156 | } 157 | 158 | if ( mInitLevel >= 2 ) 159 | { 160 | // reset signal mask 161 | ::sigprocmask( SIG_SETMASK, &mOldSigMask, 0 ); 162 | } 163 | 164 | if ( mInitLevel >= 1 ) 165 | { 166 | // reset alarm handler 167 | if( mOldAction.sa_handler ) 168 | ::sigaction( SIGALRM, &mOldAction, 0 ); 169 | } 170 | mInitLevel = 0; 171 | mFreeRun = true; 172 | } 173 | 174 | bool 175 | Sync::isActive() 176 | { 177 | return !mFreeRun; 178 | } 179 | 180 | }; //namespace Util 181 | -------------------------------------------------------------------------------- /Common/Sync.hh: -------------------------------------------------------------------------------- 1 | /** 2 | * =================================================== 3 | * file: Sync.hh 4 | * --------------------------------------------------- 5 | * purpose: basic class for sync signal handling 6 | * --------------------------------------------------- 7 | * first edit: 26.07.2005 by M. Dupuis @ VIRES GmbH 8 | * last mod.: 26.07.2005 by M. Dupuis @ VIRES GmbH 9 | * =================================================== 10 | */ 11 | #ifndef _UTIL_SYNC_HH 12 | #define _UTIL_SYNC_HH 13 | 14 | /* ====== INCLUSIONS ====== */ 15 | #include 16 | 17 | /* ====== CLASSES ====== */ 18 | namespace Util { 19 | 20 | class Sync { 21 | 22 | /** 23 | * private section, do not enter 24 | */ 25 | private: 26 | /** 27 | * time for synchronization in [ms] 28 | */ 29 | double mFrameTime; 30 | 31 | /** 32 | * initialization level 33 | */ 34 | int mInitLevel; 35 | 36 | /** 37 | * operating in free run? 38 | */ 39 | bool mFreeRun; 40 | 41 | struct sigaction mOldAction; 42 | sigset_t mOldSigMask; 43 | 44 | public: 45 | /** 46 | * constructor 47 | **/ 48 | explicit Sync(); 49 | 50 | /** 51 | ** Destroy the class. 52 | **/ 53 | virtual ~Sync(); 54 | 55 | /** 56 | * set the synchronization time 57 | * @param frame time in [s] 58 | */ 59 | void setFrameTime( const double & time ); 60 | 61 | /** 62 | * member access function 63 | * @return frame time in [s] 64 | */ 65 | const double & getFrameTime() const; 66 | 67 | /** 68 | * wait for the next frame 69 | */ 70 | void wait(); 71 | 72 | /** 73 | * start synchronization process 74 | */ 75 | bool start(); 76 | 77 | /** 78 | * stop synchronization process 79 | */ 80 | void stop(); 81 | 82 | /** 83 | * is the sync active? 84 | * @return true if synchronization is active 85 | */ 86 | bool isActive(); 87 | }; 88 | 89 | } // namespace Util 90 | 91 | #endif /* _UTIL_SYNC_HH */ 92 | -------------------------------------------------------------------------------- /Common/rdbMapper.c: -------------------------------------------------------------------------------- 1 | /** 2 | * this set of routines maps incoming RDB data to a data vector 3 | * of constant size (32bit word alignment) 4 | * (c)2011 by VIRES Simulationstechnologie GmbH, Germany 5 | */ 6 | 7 | /* ------------- includes ------------- */ 8 | #include 9 | #include 10 | 11 | /*#define BIG_ENDIAN_TARGET*/ 12 | #ifdef BIG_ENDIAN_TARGET 13 | #include "viRDBIcdBigEndian.h" 14 | #else 15 | #include "viRDBIcd.h" 16 | #include "viRDBaddOn.h" 17 | #endif 18 | 19 | /* ------------- local variables ------------- */ 20 | /* define a data buffer for incoming data */ 21 | static char* sDataBuffer = 0; 22 | static char* sDefaultDataBuffer = 0; 23 | static size_t sDataBufferTotalSize = 0; 24 | static size_t sDataBufferUsedSize = 0; 25 | static char sHoldOneMessageOnly = 1; 26 | static char sCompensateEndian = 0; 27 | static unsigned int sDebugValue01 = 888; 28 | 29 | typedef struct 30 | { 31 | double dist; 32 | void* obj; 33 | } ElDistStruct; 34 | 35 | /* ------------- public methods ------------- */ 36 | 37 | /* ------------- local methods ------------- */ 38 | /** 39 | * initialize the RDB mapper 40 | * @return 1 if successful, otherwise 0 41 | */ 42 | static int rdbMapperInit( void ); 43 | 44 | /** 45 | * terminate the RDB mapper 46 | * @return 1 if successful, otherwise 0 47 | */ 48 | static int rdbMapperTerminate( void ); 49 | 50 | /** 51 | * clear the message buffer 52 | * @return 1 if successful, otherwise 0 53 | */ 54 | static int rdbMapperClearBuffer( void ); 55 | 56 | /** 57 | * resync the message buffer so that there really is a message at the beginning 58 | * @return 1 if successful, otherwise 0 59 | */ 60 | static int rdbMapperReSyncBuffer( void ); 61 | 62 | /** 63 | * handle incoming data (stream) which has been read from the network 64 | * by the calling routine 65 | * @param data pointer to the incoming data 66 | * @param dataSize size of the incoming data [bytes] 67 | * @return 1 if successful, otherwise 0 68 | */ 69 | static int rdbMapperHandleIncomingData( void* data, unsigned int dataSize ); 70 | 71 | /** 72 | * get available data of dynamic objects 73 | * @param data pointer to the incoming data 74 | * @param dataSize size of the incoming data [bytes] 75 | * @param ownID numeric ID of own vehicle 76 | * @param ownX own x co-ordinate 77 | * @param ownY own y co-ordinate 78 | * @return number of valid objects 79 | */ 80 | static unsigned int rdbMapperGetDynamicObjects( void* target, size_t maxSize, unsigned int ownId, double ownX, double ownY ); 81 | static unsigned int rdbMapperGetStaticObjects( void* target, size_t maxSize, unsigned int ownId, double ownX, double ownY ); 82 | static unsigned int rdbMapperGetObjects( void* target, size_t maxSize, unsigned int ownId, double ownX, double ownY, int dynOb ); 83 | 84 | /** 85 | * terminate the current RDB frame and get ready for the next message; 86 | * this modifies the internal buffer 87 | * @return 1 if RDB frame has been processed 88 | */ 89 | static int rdbMapperTerminateFrame( void ); 90 | 91 | /** 92 | * push data into the RDB data buffer 93 | * @param data pointer to the data which is to be stored 94 | * @param dataSize size of the data that shall be pushed into the buffer 95 | * @return 1 if successful, otherwise 0 96 | */ 97 | static int rdbMapperPushData( void* data, size_t dataSize ); 98 | 99 | /** 100 | * pop data from the RDB data buffer 101 | * @param dataSize size of the data that shall be popped out of the buffer 102 | * @return 1 if successful, otherwise 0 103 | */ 104 | static int rdbMapperPopData( size_t dataSize ); 105 | 106 | /** 107 | * do I have a complete message in the buffer? 108 | * @return 1 if complete message is available, otherwise 0 109 | */ 110 | static int rdbMapperMessageIsAvailable( void ); 111 | 112 | /** 113 | * access the first element of a given type 114 | * @param dataType type of the data that is to be searched for 115 | * @param extended search for extended packages of the given data type 116 | * @param noElements pointer to a variable where to store the number of available elements 117 | * @param noElements pointer to a variable where to store the size of an element 118 | * @return pointer to the first element 119 | */ 120 | static char* rdbMapperAccessFirstElementOfType( unsigned short dataType, char extended, unsigned int *noElements, size_t *elSize ); 121 | 122 | static int rdbMapperMessageGetTimeAndFrame( double* simTime, unsigned int *simFrame ); 123 | static unsigned int rdbMapperGetBufferUsage( void ); 124 | static unsigned int rdbMapperGetMagicNo( void ); 125 | static int rdbMapperSetBuffer( void* bufPtr, unsigned int bufSize ); 126 | 127 | /** 128 | * compute the number of messages which are currently in the message buffer 129 | * @param noPkg pointer where to store the total number of packages 130 | * @return number of messages in buffer 131 | */ 132 | static int rdbMapperGetNoOfMsgInBuffer( unsigned int *noPkg ); 133 | 134 | /* ------------- implementation ------------- */ 135 | static int rdbMapperInit( void ) 136 | { 137 | if ( sDefaultDataBuffer ) 138 | free( sDefaultDataBuffer ); 139 | 140 | sDataBuffer = 0; 141 | sDataBufferTotalSize = 10000 * sizeof( int ); 142 | sDataBufferUsedSize = 0; 143 | 144 | sDefaultDataBuffer = calloc( 1, sDataBufferTotalSize ); 145 | sDataBuffer = sDefaultDataBuffer; 146 | 147 | return 1; 148 | } 149 | 150 | static int rdbMapperTerminate( void ) 151 | { 152 | if ( sDefaultDataBuffer ) 153 | free( sDefaultDataBuffer ); 154 | 155 | sDataBuffer = 0; 156 | sDataBufferTotalSize = 0; 157 | sDataBufferUsedSize = 0; 158 | 159 | return 1; 160 | } 161 | 162 | static int rdbMapperClearBuffer( void ) 163 | { 164 | sDataBufferUsedSize = 0; 165 | return 1; 166 | } 167 | 168 | static int rdbMapperSetBuffer( void* bufPtr, unsigned int bufSize ) 169 | { 170 | if ( bufPtr ) 171 | sDataBuffer = ( char* )bufPtr; 172 | 173 | sDataBufferUsedSize = bufSize; 174 | sDataBufferTotalSize = bufSize; 175 | 176 | return 1; 177 | } 178 | 179 | static int rdbMapperReSyncBuffer( void ) 180 | { 181 | char* checkPtr = sDataBuffer; 182 | int nCheckedBytes = 0; 183 | int maxCheckedBytes = sDataBufferUsedSize - sizeof( RDB_MSG_HDR_t ); 184 | 185 | // do not check more than 1000 bytes 186 | if ( maxCheckedBytes > 10000 ) 187 | maxCheckedBytes = 10000; 188 | 189 | while ( nCheckedBytes < maxCheckedBytes ) 190 | { 191 | RDB_MSG_HDR_t* hdr = ( RDB_MSG_HDR_t* )( checkPtr ); 192 | 193 | if ( hdr->magicNo == RDB_MAGIC_NO ) 194 | { 195 | // found the start of a new message, pop the "irrelevant" bytes from the buffer 196 | rdbMapperPopData( nCheckedBytes ); 197 | return 1; 198 | } 199 | nCheckedBytes++; 200 | checkPtr++; 201 | } 202 | 203 | if ( sDataBufferUsedSize > sizeof( RDB_MSG_HDR_t ) ) 204 | rdbMapperClearBuffer(); 205 | 206 | return 0; 207 | } 208 | 209 | static int rdbMapperHandleIncomingData( void* data, unsigned int dataSize ) 210 | { 211 | if ( dataSize == 0 ) 212 | return 0; 213 | 214 | if ( sHoldOneMessageOnly ) 215 | rdbMapperClearBuffer(); 216 | 217 | /* first: push the data into the data buffer */ 218 | return rdbMapperPushData( data, dataSize ); 219 | } 220 | 221 | static int rdbMapperTerminateFrame( void ) 222 | { 223 | RDB_MSG_HDR_t* msgHdr = ( RDB_MSG_HDR_t* )( sDataBuffer ); 224 | 225 | if ( !rdbMapperMessageIsAvailable() ) 226 | return 0; 227 | 228 | /* remove the message from the buffer */ 229 | return rdbMapperPopData( msgHdr->headerSize + msgHdr->dataSize ); 230 | } 231 | 232 | static int rdbMapperPushData( void* data, size_t dataSize ) 233 | { 234 | char* writePtr = NULL; 235 | 236 | /* grow the buffer if necessary */ 237 | if ( ( sDataBufferUsedSize + dataSize ) > sDataBufferTotalSize ) 238 | { 239 | return 0; 240 | 241 | //sDataBuffer = ( char* ) realloc( sDataBuffer, sDataBufferUsedSize + dataSize ); 242 | //sDataBufferTotalSize = sDataBufferUsedSize + dataSize; 243 | } 244 | 245 | if ( !sDataBuffer ) 246 | return 0; 247 | 248 | writePtr = sDataBuffer; 249 | writePtr += sDataBufferUsedSize; 250 | 251 | memcpy( writePtr, data, dataSize ); 252 | 253 | sDataBufferUsedSize += dataSize; 254 | 255 | return 1; 256 | } 257 | 258 | static int rdbMapperPopData( size_t dataSize ) 259 | { 260 | char* dataPtr = 0; 261 | 262 | if ( !dataSize ) 263 | return 0; 264 | 265 | /* do not move more data than is available */ 266 | if ( dataSize > sDataBufferUsedSize ) 267 | dataSize = sDataBufferUsedSize; 268 | 269 | /* --- move remaining contents --- */ 270 | dataPtr = sDataBuffer; 271 | dataPtr += dataSize; 272 | 273 | memmove( sDataBuffer, dataPtr, sDataBufferTotalSize - dataSize ); 274 | 275 | sDataBufferUsedSize -= dataSize; 276 | 277 | /* --- re-initialize the remaining bits of the buffer --- */ 278 | dataPtr = sDataBuffer; 279 | dataPtr += sDataBufferUsedSize; 280 | 281 | memset( dataPtr, 0, sDataBufferTotalSize - sDataBufferUsedSize ); 282 | 283 | return 1; 284 | } 285 | 286 | static int rdbMapperMessageIsAvailable( void ) 287 | { 288 | RDB_MSG_HDR_t* msgHdr = ( RDB_MSG_HDR_t* )( sDataBuffer ); 289 | 290 | if ( !sDataBuffer || ( sDataBufferUsedSize < sizeof( RDB_MSG_HDR_t ) ) ) 291 | return 0; 292 | 293 | // ok, sometimes dSPACE may overrun and skip part of messages, in this case we have 294 | // to re-sync to the message frames 295 | if ( !sHoldOneMessageOnly ) 296 | { 297 | if ( msgHdr->magicNo != RDB_MAGIC_NO ) 298 | { 299 | rdbMapperReSyncBuffer(); 300 | 301 | if ( sDataBufferUsedSize < sizeof( RDB_MSG_HDR_t ) ) 302 | return 0; 303 | } 304 | } 305 | return ( sDataBufferUsedSize >= ( msgHdr->headerSize + msgHdr->dataSize ) ); 306 | } 307 | 308 | static int rdbMapperMessageGetMagicNo( void ) 309 | { 310 | RDB_MSG_HDR_t* msgHdr = ( RDB_MSG_HDR_t* )( sDataBuffer ); 311 | unsigned short *magicNoPtr = ( unsigned short* ) sDataBuffer; 312 | 313 | if ( sDataBufferUsedSize < sizeof( RDB_MSG_HDR_t ) ) 314 | return 0; 315 | 316 | return msgHdr->magicNo; 317 | } 318 | 319 | static int rdbMapperMessageGetTimeAndFrame( double* simTime, unsigned int *simFrame ) 320 | { 321 | RDB_MSG_HDR_t* msgHdr = ( RDB_MSG_HDR_t* )( sDataBuffer ); 322 | 323 | if ( !rdbMapperMessageIsAvailable() ) 324 | return 0; 325 | 326 | if ( !simTime || !simFrame ) 327 | return 0; 328 | 329 | *simTime = msgHdr->simTime; 330 | *simFrame = msgHdr->frameNo; 331 | 332 | return 1; 333 | } 334 | 335 | static unsigned int rdbMapperGetBufferUsage( void ) 336 | { 337 | return sDataBufferUsedSize; 338 | } 339 | 340 | static char* rdbMapperAccessFirstElementOfType( unsigned short dataType, char extended, unsigned int *noElements, size_t *elSize ) 341 | { 342 | RDB_MSG_HDR_t* msgHdr = ( RDB_MSG_HDR_t* )( sDataBuffer ); 343 | RDB_MSG_ENTRY_HDR_t* entryHdr = 0; 344 | unsigned int checkedSize = 0; 345 | 346 | if ( !noElements ) 347 | return NULL; 348 | 349 | /* for sure, we do not yet have any elements */ 350 | *noElements = 0; 351 | *elSize = 0; 352 | 353 | if ( !rdbMapperMessageIsAvailable() ) 354 | return 0; 355 | 356 | /* go to the first entry header */ 357 | entryHdr = ( RDB_MSG_ENTRY_HDR_t* )( sDataBuffer + msgHdr->headerSize ); 358 | 359 | while ( checkedSize < msgHdr->dataSize ) 360 | { 361 | if ( entryHdr->pkgId == dataType ) 362 | { 363 | int entryIsExtended = ( entryHdr->flags & RDB_PKG_FLAG_EXTENDED ) != 0; 364 | 365 | if ( entryIsExtended == extended ) 366 | { 367 | if ( entryHdr->elementSize ) 368 | *noElements = entryHdr->dataSize / entryHdr->elementSize; 369 | 370 | *elSize = entryHdr->elementSize; 371 | 372 | return ( ( char* ) entryHdr ) + entryHdr->headerSize; 373 | } 374 | } 375 | 376 | /* let's go to the next entry */ 377 | checkedSize += entryHdr->headerSize + entryHdr->dataSize; 378 | entryHdr = ( RDB_MSG_ENTRY_HDR_t* )( ( ( char* ) entryHdr ) + entryHdr->headerSize + entryHdr->dataSize ); 379 | } 380 | return NULL; 381 | } 382 | 383 | static int rdbMapperGetNoOfMsgInBuffer( unsigned int *noPkg ) 384 | { 385 | RDB_MSG_HDR_t* msgHdr = ( RDB_MSG_HDR_t* )( sDataBuffer ); 386 | RDB_MSG_ENTRY_HDR_t* entryHdr = 0; 387 | unsigned int checkedBufSize = 0; 388 | unsigned int noMsg = 0; 389 | 390 | if ( !noPkg ) 391 | return 0; 392 | 393 | *noPkg = 0; 394 | 395 | if ( !rdbMapperMessageIsAvailable() ) 396 | return 0; 397 | 398 | while ( checkedBufSize < sDataBufferUsedSize ) 399 | { 400 | unsigned int checkedMsgSize = 0; 401 | noMsg++; 402 | 403 | /* go to the first entry header in current message */ 404 | entryHdr = ( RDB_MSG_ENTRY_HDR_t* )( ( ( char* ) msgHdr ) + msgHdr->headerSize ); 405 | 406 | while ( checkedMsgSize < msgHdr->dataSize ) 407 | { 408 | *noPkg += 1; 409 | 410 | /* let's go to the next entry */ 411 | checkedMsgSize += entryHdr->headerSize + entryHdr->dataSize; 412 | entryHdr = ( RDB_MSG_ENTRY_HDR_t* )( ( ( char* ) entryHdr ) + entryHdr->headerSize + entryHdr->dataSize ); 413 | } 414 | 415 | /* now go to the next message */ 416 | checkedBufSize += msgHdr->headerSize + msgHdr->dataSize; 417 | msgHdr = ( RDB_MSG_HDR_t* )( ( ( char* ) msgHdr ) + msgHdr->headerSize + msgHdr->dataSize ); 418 | } 419 | return noMsg; 420 | } 421 | 422 | static unsigned int rdbMapperGetDynamicObjects( void* target, size_t maxSize, unsigned int ownId, double ownX, double ownY ) 423 | { 424 | return rdbMapperGetObjects( target, maxSize, ownId, ownX, ownY, 1 ); 425 | } 426 | 427 | static unsigned int rdbMapperGetStaticObjects( void* target, size_t maxSize, unsigned int ownId, double ownX, double ownY ) 428 | { 429 | return rdbMapperGetObjects( target, maxSize, ownId, ownX, ownY, 0 ); 430 | } 431 | 432 | static unsigned int rdbMapperGetObjects( void* target, size_t maxSize, unsigned int ownId, double ownX, double ownY, int dynOb ) 433 | { 434 | unsigned int noElements = 0; 435 | size_t elSize = 0; 436 | size_t copiedSize = 0; 437 | size_t copiedElements = 0; 438 | char* tgtPtr = ( char* ) target; 439 | RDB_OBJECT_STATE_t* objState = 0; 440 | RDB_OBJECT_STATE_BASE_t* objStateBase = 0; 441 | ElDistStruct objList[100]; 442 | ElDistStruct tmpList; 443 | unsigned int objId; 444 | int i; 445 | int j; 446 | 447 | if ( !target ) 448 | return 0; 449 | 450 | if ( dynOb ) 451 | objState = ( RDB_OBJECT_STATE_t* ) rdbMapperAccessFirstElementOfType( RDB_PKG_ID_OBJECT_STATE, 1, &noElements, &elSize ); 452 | else 453 | objStateBase = ( RDB_OBJECT_STATE_BASE_t* ) rdbMapperAccessFirstElementOfType( RDB_PKG_ID_OBJECT_STATE, 0, &noElements, &elSize ); 454 | 455 | /* hold a list of all object states vs. distance to ownship */ 456 | if ( noElements > 100 ) 457 | noElements = 100; 458 | 459 | /* sort elements in order of distance to ownship */ 460 | for ( i = 0; i < noElements; i++ ) 461 | { 462 | RDB_OBJECT_STATE_BASE_t *basePtr = dynOb ? &( objState[i].base ) : &( objStateBase[i] ); 463 | double objX; 464 | double objY; 465 | double dx; 466 | double dy; 467 | char* dataPtr = ( char* )basePtr; 468 | 469 | if ( sCompensateEndian ) 470 | { 471 | char* xPtr = ( char* )&objX; 472 | char* yPtr = ( char* )&objY; 473 | 474 | // for this target machine you have to map the double values in pairs of 4 bytes 475 | xPtr[0] = dataPtr[68]; 476 | xPtr[1] = dataPtr[69]; 477 | xPtr[2] = dataPtr[70]; 478 | xPtr[3] = dataPtr[71]; 479 | xPtr[4] = dataPtr[64]; 480 | xPtr[5] = dataPtr[65]; 481 | xPtr[6] = dataPtr[66]; 482 | xPtr[7] = dataPtr[67]; 483 | 484 | yPtr[0] = dataPtr[76]; 485 | yPtr[1] = dataPtr[77]; 486 | yPtr[2] = dataPtr[78]; 487 | yPtr[3] = dataPtr[79]; 488 | yPtr[4] = dataPtr[72]; 489 | yPtr[5] = dataPtr[73]; 490 | yPtr[6] = dataPtr[74]; 491 | yPtr[7] = dataPtr[75]; 492 | } 493 | else 494 | { 495 | objX = basePtr->pos.x; 496 | objY = basePtr->pos.y; 497 | } 498 | 499 | dx = ownX - objX; 500 | dy = ownY - objY; 501 | 502 | objList[i].obj = basePtr; 503 | objList[i].dist = dx * dx + dy * dy; // using the square of dist is fine also and saves some time 504 | } 505 | 506 | /* bubble sort the stuff */ 507 | for ( i = 0; i < noElements; i++ ) 508 | { 509 | for ( j = i + 1; j < noElements; j++ ) 510 | { 511 | if ( objList[j].dist < objList[i].dist ) 512 | { 513 | size_t copySize = sizeof( ElDistStruct ); 514 | memcpy( &tmpList, &( objList[i] ), copySize ); 515 | memcpy( &( objList[i] ), &( objList[j] ), copySize ); 516 | memcpy( &( objList[j] ), &tmpList, copySize ); 517 | } 518 | } 519 | } 520 | 521 | for ( i = 0; ( i < noElements ) && ( ( copiedSize + elSize ) < maxSize ); i++ ) 522 | { 523 | RDB_OBJECT_STATE_BASE_t *basePtr = ( RDB_OBJECT_STATE_BASE_t* ) objList[i].obj; 524 | 525 | memcpy( &objId, basePtr, sizeof( int ) ); 526 | 527 | if ( objId != ownId ) 528 | { 529 | memcpy( tgtPtr, objList[i].obj, elSize ); 530 | 531 | copiedSize += elSize; 532 | tgtPtr += elSize; 533 | copiedElements++; 534 | } 535 | } 536 | return copiedElements; 537 | } 538 | 539 | static unsigned int rdbMapperGetElementsOfType( void* target, size_t maxSize, int elType ) 540 | { 541 | unsigned int noElements = 0; 542 | unsigned int maxElements = 0; 543 | size_t elSize = 0; 544 | char* tgtPtr = ( char* ) target; 545 | void* srcPtr = 0; 546 | 547 | if ( !target ) 548 | return 0; 549 | 550 | srcPtr = rdbMapperAccessFirstElementOfType( elType, 0, &noElements, &elSize ); 551 | 552 | if ( !srcPtr || !noElements ) 553 | return 0; 554 | 555 | // do not copy more than there is space available 556 | maxElements = maxSize / elSize; 557 | 558 | if ( noElements > maxElements ) 559 | noElements = maxElements; 560 | 561 | memcpy( tgtPtr, srcPtr, noElements * elSize ); 562 | 563 | return noElements; 564 | } 565 | 566 | static unsigned int rdbMapperGetOwnElementsOfType( unsigned int ownId, void* target, size_t maxSize, int elType ) 567 | { 568 | unsigned int noElements = 0; 569 | unsigned int maxElements = 0; 570 | unsigned int usedElements = 0; 571 | size_t elSize = 0; 572 | char* tgtPtr = ( char* ) target; 573 | void* srcPtr = 0; 574 | char* scanPtr = 0; 575 | unsigned int i; 576 | 577 | if ( !target ) 578 | return 0; 579 | 580 | srcPtr = rdbMapperAccessFirstElementOfType( elType, 0, &noElements, &elSize ); 581 | 582 | if ( !srcPtr || !noElements ) 583 | return 0; 584 | 585 | // do not copy more than there is space available 586 | maxElements = maxSize / elSize; 587 | 588 | scanPtr = ( char* ) srcPtr; 589 | 590 | for ( i = 0; ( i < noElements ) && ( maxElements ); i++ ) 591 | { 592 | char useElement = 0; 593 | 594 | switch( elType ) 595 | { 596 | case RDB_PKG_ID_CONTACT_POINT: 597 | { 598 | RDB_CONTACT_POINT_t* cp = ( RDB_CONTACT_POINT_t* ) scanPtr; 599 | 600 | useElement = ( cp->playerId == ownId ); 601 | } 602 | break; 603 | 604 | case RDB_PKG_ID_ENVIRONMENT: 605 | useElement = 1; 606 | break; 607 | 608 | case RDB_PKG_ID_DRIVER_CTRL: 609 | { 610 | RDB_DRIVER_CTRL_t* ctrl = ( RDB_DRIVER_CTRL_t* ) scanPtr; 611 | 612 | useElement = ( ctrl->playerId == ownId ); 613 | } 614 | break; 615 | } 616 | 617 | if ( useElement ) 618 | { 619 | memcpy( tgtPtr, scanPtr, elSize ); 620 | tgtPtr += elSize; 621 | maxElements--; 622 | usedElements++; 623 | } 624 | 625 | if ( maxElements ) 626 | scanPtr += elSize; 627 | } 628 | 629 | return usedElements; 630 | } 631 | -------------------------------------------------------------------------------- /Common/viRDBTypes.h: -------------------------------------------------------------------------------- 1 | /*****************************************************//** 2 | * @file 3 | * Type definitions of the Runtime Data Bus (RDB) 4 | * 5 | * (c) VIRES GmbH 6 | * @author Marius Dupuis 7 | ********************************************************/ 8 | 9 | /*****************************************************/ 10 | #ifndef VI_RDB_TYPES_H 11 | #define VI_RDB_TYPES_H 12 | 13 | typedef unsigned char uint8_t; 14 | 15 | 16 | typedef char int8_t; 17 | typedef unsigned short uint16_t; 18 | typedef short int16_t; 19 | typedef unsigned int uint32_t; 20 | typedef int int32_t; 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /ExampleACC.cpp: -------------------------------------------------------------------------------- 1 | // ExampleConsole.cpp : Defines the entry point for the console application. 2 | // 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "RDBHandler.hh" 19 | 20 | #define DEFAULT_PORT_TC 48190 /* for image port it should be 48192 */ 21 | #define DEFAULT_PORT_SENSOR 48195 22 | #define DEFAULT_BUFFER_SIZE 204800 23 | #define MAX_CONNECTIONS 2 /* maximum number of bi-directional TCP connections */ 24 | 25 | 26 | // type definition for connection handling 27 | typedef struct 28 | { 29 | int id; // unique connection ID 30 | char serverAddr[128]; // Server to connect to 31 | int port; // Port on server to connect to 32 | int desc; // client (socket) descriptor 33 | unsigned int bytesInBuffer; // used size of receive buffer 34 | size_t bufferSize; // total size of receive buffer; 35 | unsigned char *pData; // pointer to receive buffer 36 | } Connection_t; 37 | 38 | // connection instances 39 | Connection_t sConnection[MAX_CONNECTIONS]; 40 | 41 | // globally store nearest object 42 | RDB_OBJECT_STATE_t mNearestObject; 43 | RDB_OBJECT_STATE_t mOwnObject; 44 | 45 | static const unsigned int scMainConnection = 0; 46 | static const unsigned int scSensorConnection = 1; 47 | 48 | static const unsigned int scOwnId = 1; // ID of own vehicle 49 | static const double scOwnPreferredSpeed = 30.0; // default preferred speed of own vehicle 50 | double lastUpdateTimeSensorObject = 0.0; 51 | 52 | 53 | // function prototypes 54 | int openPort( int & descriptor, int portNo, const char* serverAddr ); 55 | void initConnections(); 56 | void initConnection( Connection_t & conn ); 57 | void readConnection( Connection_t & conn, bool waitForMessage, bool verbose ); 58 | 59 | void parseRDBMessage( Connection_t & conn, RDB_MSG_t* msg ); 60 | void parseRDBMessageEntry( Connection_t & conn, const double & simTime, const unsigned int & simFrame, RDB_MSG_ENTRY_HDR_t* entryHdr ); 61 | void handleRDBitem( const double & simTime, const unsigned int & simFrame, RDB_OBJECT_STATE_t & item, bool isExtended, bool isSensor, bool firstElement ); 62 | void sendDriverCtrl( int & sendSocket, const double & simTime, const unsigned int & simFrame ); 63 | 64 | 65 | // 66 | // Function: usage: 67 | // 68 | // Description: 69 | // Print usage information and exit 70 | // 71 | void usage() 72 | { 73 | printf("usage: client [-t:x] [-s:IP]\n\n"); 74 | printf(" -t:x taskControl port\n"); 75 | printf(" -s:x sensor port\n"); 76 | printf(" -T:IP taskControl IPv4 address\n"); 77 | printf(" -S:IP sensor IPv4 address\n"); 78 | exit(1); 79 | } 80 | 81 | // 82 | // Function: ValidateArgs 83 | // 84 | // Description: 85 | // Parse the command line arguments, and set some global flags 86 | // to indicate what actions to perform 87 | // 88 | void ValidateArgs(int argc, char **argv) 89 | { 90 | for(int i = 1; i < argc; i++) 91 | { 92 | if ((argv[i][0] == '-') || (argv[i][0] == '/')) 93 | { 94 | switch (argv[i][1]) 95 | { 96 | case 't': // Remote port taskControl 97 | if (strlen(argv[i]) > 3) 98 | sConnection[0].port = atoi(&argv[i][3]); 99 | break; 100 | case 's': // Remote port sensor 101 | if (strlen(argv[i]) > 3) 102 | sConnection[1].port = atoi(&argv[i][3]); 103 | break; 104 | case 'T': // TC server 105 | if (strlen(argv[i]) > 3) 106 | strcpy(sConnection[0].serverAddr, &argv[i][3]); 107 | break; 108 | case 'S': // sensor server 109 | if (strlen(argv[i]) > 3) 110 | strcpy(sConnection[1].serverAddr, &argv[i][3]); 111 | break; 112 | default: 113 | usage(); 114 | break; 115 | } 116 | } 117 | } 118 | } 119 | 120 | int main( int argc, char* argv[] ) 121 | { 122 | static bool sVerbose = false; 123 | 124 | // initialize the connections 125 | initConnections(); 126 | 127 | // Parse the command line 128 | ValidateArgs(argc, argv); 129 | 130 | // reset the information about the nearest and own object 131 | memset( &mNearestObject, 0, sizeof( RDB_OBJECT_STATE_t ) ); 132 | memset( &mOwnObject, 0, sizeof( RDB_OBJECT_STATE_t ) ); 133 | 134 | // open TC port 135 | if ( !openPort( sConnection[0].desc, sConnection[0].port, sConnection[0].serverAddr ) ) 136 | return -1; 137 | 138 | // open sensor port 139 | if ( !openPort( sConnection[1].desc, sConnection[1].port, sConnection[1].serverAddr ) ) 140 | return -1; 141 | 142 | // Send and receive data - forever! 143 | // 144 | for(;;) 145 | { 146 | readConnection( sConnection[0], false, sVerbose ); 147 | readConnection( sConnection[1], false, sVerbose ); 148 | 149 | usleep( 1 ); // do not overload the system 150 | } 151 | 152 | return 0; 153 | } 154 | 155 | void parseRDBMessage( Connection_t & conn, RDB_MSG_t* msg ) 156 | { 157 | if ( !msg ) 158 | return; 159 | 160 | if ( !msg->hdr.dataSize ) 161 | return; 162 | 163 | RDB_MSG_ENTRY_HDR_t* entry = ( RDB_MSG_ENTRY_HDR_t* ) ( ( ( char* ) msg ) + msg->hdr.headerSize ); 164 | uint32_t remainingBytes = msg->hdr.dataSize; 165 | 166 | while ( remainingBytes ) 167 | { 168 | parseRDBMessageEntry( conn, msg->hdr.simTime, msg->hdr.frameNo, entry ); 169 | 170 | remainingBytes -= ( entry->headerSize + entry->dataSize ); 171 | 172 | if ( remainingBytes ) 173 | entry = ( RDB_MSG_ENTRY_HDR_t* ) ( ( ( ( char* ) entry ) + entry->headerSize + entry->dataSize ) ); 174 | } 175 | } 176 | 177 | void parseRDBMessageEntry( Connection_t & conn, const double & simTime, const unsigned int & simFrame, RDB_MSG_ENTRY_HDR_t* entryHdr ) 178 | { 179 | if ( !entryHdr ) 180 | return; 181 | 182 | int noElements = entryHdr->elementSize ? ( entryHdr->dataSize / entryHdr->elementSize ) : 0; 183 | 184 | if ( !noElements ) // some elements require special treatment 185 | { 186 | switch ( entryHdr->pkgId ) 187 | { 188 | case RDB_PKG_ID_START_OF_FRAME: 189 | fprintf( stderr, "void parseRDBMessageEntry: connection %d: got start of frame\n", conn.id ); 190 | 191 | if ( conn.id == scSensorConnection ) // reset information about sensor object if a new sensor frame starts. 192 | memset( &mNearestObject, 0, sizeof( RDB_OBJECT_STATE_t ) );; 193 | break; 194 | 195 | case RDB_PKG_ID_END_OF_FRAME: 196 | fprintf( stderr, "void parseRDBMessageEntry: connection %d: got end of frame\n", conn.id ); 197 | // only connection to taskControl shall send driver control commands 198 | 199 | if ( conn.id == scMainConnection ) 200 | sendDriverCtrl( conn.desc, simTime, simFrame ); 201 | break; 202 | 203 | default: 204 | return; 205 | break; 206 | } 207 | return; 208 | } 209 | 210 | unsigned char ident = 6; 211 | char* dataPtr = ( char* ) entryHdr; 212 | bool firstElement = true; 213 | 214 | dataPtr += entryHdr->headerSize; 215 | 216 | while ( noElements-- ) 217 | { 218 | bool printedMsg = true; 219 | 220 | switch ( entryHdr->pkgId ) 221 | { 222 | case RDB_PKG_ID_OBJECT_STATE: 223 | // only first object state in sensor (i.e. nearest object ) is relevant 224 | if ( ( conn.id == scSensorConnection ) && ( entryHdr->flags & RDB_PKG_FLAG_EXTENDED ) ) 225 | { 226 | handleRDBitem( simTime, simFrame, *( ( RDB_OBJECT_STATE_t* ) dataPtr ), entryHdr->flags & RDB_PKG_FLAG_EXTENDED, ( conn.id == scSensorConnection ), firstElement ); 227 | firstElement = false; 228 | } 229 | break; 230 | 231 | default: 232 | printedMsg = false; 233 | break; 234 | } 235 | dataPtr += entryHdr->elementSize; 236 | } 237 | } 238 | 239 | void handleRDBitem( const double & simTime, const unsigned int & simFrame, RDB_OBJECT_STATE_t & item, bool isExtended, bool isSensor, bool firstElement ) 240 | { 241 | // own object? 242 | if ( item.base.id == scOwnId ) 243 | memcpy( &mOwnObject, &item, sizeof( RDB_OBJECT_STATE_t ) ); 244 | else if ( isSensor && firstElement ) 245 | memcpy( &mNearestObject, &item, sizeof( RDB_OBJECT_STATE_t ) ); 246 | 247 | fprintf( stderr, "handleRDBitem: handling object state\n" ); 248 | fprintf( stderr, " simTime = %.3lf, simFrame = %u\n", simTime, simFrame ); 249 | fprintf( stderr, " object = %s, id = %d\n", item.base.name, item.base.id ); 250 | fprintf( stderr, " position = %.3lf / %.3lf / %.3lf\n", item.base.pos.x, item.base.pos.y, item.base.pos.z ); 251 | 252 | if ( isExtended ) 253 | fprintf( stderr, " speed = %.3lf / %.3lf / %.3lf\n", item.ext.speed.x, item.ext.speed.y, item.ext.speed.z ); 254 | } 255 | 256 | void sendDriverCtrl( int & sendSocket, const double & simTime, const unsigned int & simFrame ) 257 | { 258 | Framework::RDBHandler myHandler; 259 | 260 | myHandler.initMsg(); 261 | 262 | RDB_DRIVER_CTRL_t *myDriver = ( RDB_DRIVER_CTRL_t* ) myHandler.addPackage( simTime, simFrame, RDB_PKG_ID_DRIVER_CTRL ); 263 | 264 | if ( !myDriver ) 265 | return; 266 | 267 | // do we have a valid nearest object? 268 | bool haveSensorObject = ( mNearestObject.base.id > 0 ); // sensor object must not be older than 1.0s 269 | 270 | double ownSpeed = sqrt( mOwnObject.ext.speed.x * mOwnObject.ext.speed.x + mOwnObject.ext.speed.y * mOwnObject.ext.speed.y ); 271 | 272 | double accelTgtDist = 0.0; 273 | double accelTgtSpeed = ( 30.0 - ownSpeed ) / 5.0; // default speed should be own preferred speed 274 | 275 | if ( haveSensorObject ) 276 | { 277 | // let's go for the same speed as preceding vehicle: 278 | if ( mNearestObject.ext.speed.x < -1.0e-3 ) 279 | accelTgtSpeed = 2.0 * mNearestObject.ext.speed.x / 5.0; 280 | else 281 | accelTgtSpeed = 0.0; 282 | 283 | // let's go for a 2s distance 284 | double tgtDist = ownSpeed * 2.0; 285 | 286 | if ( tgtDist < 10.0 ) // minimum distance to keep 287 | tgtDist = 10.0; 288 | 289 | accelTgtDist = ( mNearestObject.base.pos.x - tgtDist ) / 10.0; 290 | } 291 | 292 | fprintf( stderr, "sendDriverCtrl: accelDist = %.5lf, accelSpeed = %.5lf\n", accelTgtDist, accelTgtSpeed ); 293 | 294 | myDriver->playerId = 1; 295 | myDriver->accelTgt = accelTgtDist + accelTgtSpeed; 296 | myDriver->validityFlags = RDB_DRIVER_INPUT_VALIDITY_TGT_ACCEL | RDB_DRIVER_INPUT_VALIDITY_ADD_ON; 297 | 298 | int retVal = send( sendSocket, ( const char* ) ( myHandler.getMsg() ), myHandler.getMsgTotalSize(), 0 ); 299 | 300 | if ( !retVal ) 301 | fprintf( stderr, "sendDriverCtrl: could not send driver control\n" ); 302 | else 303 | fprintf( stderr, "sentDriverCtrl\n" ); 304 | 305 | } 306 | 307 | void initConnections() 308 | { 309 | memset( sConnection, 0, MAX_CONNECTIONS * sizeof( Connection_t ) ); 310 | 311 | // general initialization 312 | for ( int i = 0; i < MAX_CONNECTIONS; i++ ) 313 | { 314 | initConnection( sConnection[i] ); 315 | sConnection[i].id = i; 316 | } 317 | 318 | // additional individual initalization 319 | sConnection[0].port = DEFAULT_PORT_TC; 320 | sConnection[1].port = DEFAULT_PORT_SENSOR; 321 | } 322 | 323 | void initConnection( Connection_t & conn ) 324 | { 325 | strcpy( conn.serverAddr, "127.0.0.1" ); 326 | 327 | conn.desc = -1; 328 | conn.bufferSize = sizeof ( RDB_MSG_t ); 329 | conn.pData = ( unsigned char* ) calloc( 1, conn.bufferSize ); 330 | } 331 | 332 | int openPort( int & descriptor, int portNo, const char* serverAddr ) 333 | { 334 | struct sockaddr_in server; 335 | struct hostent *host = NULL; 336 | 337 | // 338 | // Create the socket, and attempt to connect to the server 339 | // 340 | descriptor = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); 341 | 342 | if ( descriptor == -1 ) 343 | { 344 | fprintf( stderr, "openPort: socket() failed: %s\n", strerror( errno ) ); 345 | return 0; 346 | } 347 | 348 | int opt = 1; 349 | setsockopt ( descriptor, IPPROTO_TCP, TCP_NODELAY, &opt, sizeof( opt ) ); 350 | 351 | server.sin_family = AF_INET; 352 | server.sin_port = htons( portNo ); 353 | server.sin_addr.s_addr = inet_addr( serverAddr ); 354 | 355 | // 356 | // If the supplied server address wasn't in the form 357 | // "aaa.bbb.ccc.ddd" it's a hostname, so try to resolve it 358 | // 359 | if ( server.sin_addr.s_addr == INADDR_NONE ) 360 | { 361 | host = gethostbyname( serverAddr ); 362 | 363 | if ( host == NULL ) 364 | { 365 | fprintf( stderr, "openPort: unable to resolve server: %s\n", serverAddr ); 366 | return 0; 367 | } 368 | memcpy( &server.sin_addr, host->h_addr_list[0], host->h_length ); 369 | } 370 | 371 | // set to non blocking 372 | opt = 1; 373 | ioctl( descriptor, FIONBIO, &opt ); 374 | 375 | // wait for connection 376 | bool bConnected = false; 377 | 378 | while ( !bConnected ) 379 | { 380 | if (connect( descriptor, (struct sockaddr *)&server, sizeof( server ) ) == -1 ) 381 | { 382 | fprintf( stderr, "connect() failed: %s\n", strerror( errno ) ); 383 | sleep( 1 ); 384 | } 385 | else 386 | bConnected = true; 387 | } 388 | 389 | fprintf( stderr, "port % d connected!\n", portNo ); 390 | 391 | return 1; 392 | } 393 | 394 | void readConnection( Connection_t & conn, bool waitForMessage, bool verbose ) 395 | { 396 | // receive buffer 397 | static char* szBuffer = ( char* ) calloc( 1, DEFAULT_BUFFER_SIZE ); 398 | int ret = -1; 399 | 400 | bool bMsgComplete = false; 401 | 402 | if ( verbose ) 403 | fprintf( stderr, "readConnection: start reading connection %d\n", conn.id ); 404 | 405 | // read a complete message 406 | do 407 | { 408 | ret = recv( conn.desc, szBuffer, DEFAULT_BUFFER_SIZE, 0 ); 409 | 410 | if ( verbose ) 411 | fprintf( stderr, "readConnection: connection %d, ret = %d\n", conn.id, ret ); 412 | 413 | if ( ret > 0 ) 414 | { 415 | // do we have to grow the buffer?? 416 | if ( ( conn.bytesInBuffer + ret ) > conn.bufferSize ) 417 | { 418 | conn.pData = ( unsigned char* ) realloc( conn.pData, conn.bytesInBuffer + ret ); 419 | conn.bufferSize = conn.bytesInBuffer + ret; 420 | } 421 | 422 | memcpy( conn.pData + conn.bytesInBuffer, szBuffer, ret ); 423 | conn.bytesInBuffer += ret; 424 | 425 | // already complete messagae? 426 | if ( conn.bytesInBuffer >= sizeof( RDB_MSG_HDR_t ) ) 427 | { 428 | RDB_MSG_HDR_t* hdr = ( RDB_MSG_HDR_t* ) conn.pData; 429 | 430 | // is this message containing the valid magic number? 431 | if ( hdr->magicNo != RDB_MAGIC_NO ) 432 | { 433 | printf( "message receiving is out of sync; discarding data" ); 434 | conn.bytesInBuffer = 0; 435 | } 436 | 437 | while ( conn.bytesInBuffer >= ( hdr->headerSize + hdr->dataSize ) ) 438 | { 439 | unsigned int msgSize = hdr->headerSize + hdr->dataSize; 440 | 441 | // print the message 442 | if ( verbose ) 443 | Framework::RDBHandler::printMessage( ( RDB_MSG_t* ) conn.pData, true ); 444 | 445 | // now parse the message 446 | parseRDBMessage( conn, ( RDB_MSG_t* ) conn.pData ); 447 | 448 | // remove message from queue 449 | memmove( conn.pData, conn.pData + msgSize, conn.bytesInBuffer - msgSize ); 450 | conn.bytesInBuffer -= msgSize; 451 | 452 | bMsgComplete = true; 453 | } 454 | } 455 | } 456 | } while ( ( ret > 0 ) && ( !waitForMessage || bMsgComplete ) ); 457 | 458 | if ( verbose ) 459 | fprintf( stderr, "readConnection: finished reading connection %d\n", conn.id ); 460 | } 461 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 huawei-octopus 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PnC Algorithm Samples with VTD Adapter 2 | Simple PnC algorithm samples for octopus simulation service users to get started with vtd adapter. 3 | 4 | ## How to use it 5 | To get started with these PnC algorithm samples, you should first of all have them compiled. 6 | ```shell 7 | g++ -o acc ./Common/RDBHandler.cc ExampleACC.cpp -I./Common 8 | ``` 9 | 10 | ## More 11 | * Issues and bugs can be raised on the [Issue tracker on GitHub](https://github.com/huawei-octopus/pnc-algo-samples-vtd/issues) 12 | * For discussion or questions please use [Huawei cloud forum for developers](https://bbs.huaweicloud.com/forum/forum-1075-1.html) 13 | --------------------------------------------------------------------------------