├── .gitignore ├── CMakeLists.txt ├── README.md ├── detectSurroundings.cpp ├── detectSurroundings.h └── serialize_sample.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | #ignore build folders 2 | **/build 3 | **/Debug 4 | **/tmp 5 | 6 | 7 | #ignore workspace files 8 | .metadata 9 | 10 | #ignore temp files 11 | *~ 12 | 13 | #logs 14 | *.log 15 | *.csv 16 | *.interLog 17 | 18 | #stuff 19 | **/.settings/language.settings.xml 20 | .directory 21 | *.db 22 | documentation/html 23 | 24 | .vscode/ 25 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 2.8) 3 | 4 | SET(PROJECT_VERSION_MAJOR "1") 5 | SET(PROJECT_VERSION_MINOR "5") 6 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -Wall -std=c++11") 7 | SET(CMAKE_VERBOSE_MAKEFILE "ON") 8 | SET(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/CMake/Modules) 9 | SET(CMAKE_INSTALL_PREFIX "/usr") 10 | 11 | set(autoware_sender_SRCS 12 | detectSurroundings.cpp 13 | ) 14 | 15 | include_directories(/opt/ros/kinetic/include) 16 | include_directories(/home/oi/Autoware/ros/install/autoware_msgs/include) 17 | link_directories(/opt/ros/kinetic/lib) 18 | add_executable (autoware_sender ${autoware_sender_SRCS}) 19 | target_link_libraries (autoware_sender roscpp rosconsole rostime roscpp_serialization proj boost_thread boost_system boost_serialization protobuf) 20 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # AutoC2X-AW 2 | 3 | ![movie](https://user-images.githubusercontent.com/23014935/76400182-39570c00-63c3-11ea-81cb-a6b84179406d.gif) 4 | 5 | AutoC2X is cooperative awareness driving software, extension for Autoware and OpenC2X. AutoC2X-AW is extension for Autoware. If you find this code useful in your research, please consider citing : 6 | 7 | @inproceedings{Tsukada2020, 8 | title = {AutoC2X: Open-source software to realize V2X cooperative perception among autonomous vehicles}, 9 | author = {Manabu Tsukada and Takaharu Oi and Akihide Ito and Mai Hirata and Hiroshi Esaki}, 10 | year = {2020}, 11 | date = {2020-10-04}, 12 | booktitle = {The 2020 IEEE 92nd Vehicular Technology Conference (VTC2020-Fall)}, 13 | address = {Victoria, B.C., Canada} 14 | } 15 | 16 | @article{Tsukada2020b, 17 | title = {Networked Roadside Perception Units for Autonomous Driving}, 18 | author = {Manabu Tsukada and Takaharu Oi and Masahiro Kitazawa and Hiroshi Esaki}, 19 | url = {https://www.mdpi.com/1424-8220/20/18/5320}, 20 | doi = {10.3390/s20185320}, 21 | issn = {1424-8220}, 22 | year = {2020}, 23 | date = {2020-09-17}, 24 | journal = {MDPI Sensors}, 25 | volume = {20}, 26 | number = {18} 27 | } 28 | 29 | 30 | 31 | ## Description 32 | 33 | [Autoware](https://gitlab.com/autowarefoundation/autoware.ai) is open-source autonomous driving software. [OpenC2X](https://www.ccs-labs.org/software/openc2x/) is open-source cooperative ITS software, able to communicate with other vehicles following ITS-G5. AutoC2X is an extension for Autoware and OpenC2X. Using this software, you can get other vehicle information run by Autoware. 34 | 35 | ## AutoC2X-OC 36 | AutoC2X-AW collaborate with [AutoC2X-OC](https://github.com/esakilab/AutoC2X-OC) at router. So you should install and run AutoC2X-OC in router. 37 | 38 | ## Requirement 39 | - autoware (ROS) 40 | - libboost 41 | - libproj 42 | - apt-get install libproj-dev 43 | 44 | ## Usage 45 | 46 | ### edit CMakeLists.txt 47 | change "include_directories" to your environment 48 | 49 | ### build 50 | ``` 51 | mkdir build 52 | cd build 53 | cmake .. 54 | make all 55 | ``` 56 | 57 | ### hardware Setup 58 | In AutoC2X, each vehicle is supposed to equip one laptop and one router. Autoware and AutoC2X-AW is running in laptop. AutoC2X-OC is running in router. 59 | 60 | ![hardware](https://user-images.githubusercontent.com/23014935/76481753-b59a2f80-6455-11ea-9134-4b5376bf75c4.png) 61 | 62 | ### network setup 63 | You should allocate IPv4 address to laptops and routers. Below image is example. 64 | 65 | ![network](https://user-images.githubusercontent.com/23014935/76482009-50930980-6456-11ea-9155-3abf0788592b.png) 66 | 67 | ### how to run 68 | 69 | You should start AutoC2X below order. 70 | 71 | 1. Start AutoC2X-AW at receiver laptop <- # ./autoware_pub -r 72 | 2. Start AutoC2X-OC at receiver router 73 | 3. Start AutoC2X-OC at sender router 74 | 4. Start AutoC2X-AW at sender laptop <- # ./autoware_pub -s 75 | 76 | ``` 77 | cd build/ 78 | ./autoware_pub [-r|-s] 79 | ``` 80 | 81 | You may have to be superuser. 82 | -------------------------------------------------------------------------------- /detectSurroundings.cpp: -------------------------------------------------------------------------------- 1 | #include "detectSurroundings.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace std; 10 | 11 | std::vector createConvexHull(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4){ 12 | std::random_device rnd; 13 | 14 | std::vector result; 15 | 16 | geometry_msgs::Point32 a1, a2, a3, a4; 17 | 18 | a1.x = x1; 19 | a1.y = y1; 20 | a1.z = 0; 21 | 22 | a2.x = x2; 23 | a2.y = y2; 24 | a2.z = 0; 25 | 26 | a3.x = x3; 27 | a3.y = y3; 28 | a3.z = 0; 29 | 30 | a4.x = x4; 31 | a4.y = y4; 32 | a4.z = 0; 33 | 34 | result.push_back(a1); 35 | result.push_back(a2); 36 | result.push_back(a3); 37 | result.push_back(a4); 38 | result.push_back(a1); 39 | 40 | return result; 41 | } 42 | 43 | sensor_msgs::ChannelFloat32 createChannel(std::string name){ 44 | sensor_msgs::ChannelFloat32 result; 45 | int N = 10; 46 | result.name = name; 47 | 48 | std::vector values; 49 | for(int i=0; i createLine(){ 58 | std::vector result; 59 | int N = 10; 60 | for(int x = 0; x < N; x++){ 61 | for(int y = 0; y < N; y++){ 62 | for(int z = 0; z < N; z++){ 63 | geometry_msgs::Point32 p; 64 | p.x = x*0.5 + 10; 65 | p.y = y*0.5; 66 | p.z = z*0.5; 67 | result.push_back(p); 68 | } 69 | } 70 | } 71 | return result; 72 | } 73 | 74 | autoware_msgs::DetectedObjectArray createObjectArray(std::vector X, std::vector Y){ 75 | 76 | std::random_device rnd; 77 | autoware_msgs::DetectedObjectArray msg; 78 | std::vector objects; 79 | 80 | msg.header.frame_id = "velodyne"; 81 | ros::Time nowTime = ros::Time::now(); 82 | for(int i=0; i points = box_line; 109 | std::vector channels; 110 | channels.push_back( channel ); 111 | sensor_msgs::PointCloud input_msg; 112 | sensor_msgs::PointCloud2 output_msg; 113 | input_msg.header.frame_id = "velodyne"; 114 | input_msg.points = points; 115 | input_msg.channels = channels; 116 | sensor_msgs::convertPointCloudToPointCloud2(input_msg, output_msg); 117 | object.pointcloud = output_msg; 118 | 119 | geometry_msgs::PolygonStamped convex_hull_msg; 120 | convex_hull_msg.header.frame_id = "velodyne"; 121 | geometry_msgs::Polygon polygon; 122 | polygon.points = createConvexHull(x1, y1, x2, y2, x3, y3, x4, y4); 123 | convex_hull_msg.polygon = polygon; 124 | object.convex_hull = convex_hull_msg; 125 | 126 | object.header.stamp = nowTime; 127 | object.pointcloud.header.stamp = nowTime; 128 | object.convex_hull.header.stamp = nowTime; 129 | objects.push_back(object); 130 | } 131 | 132 | msg.objects = objects; 133 | msg.header.stamp = nowTime; 134 | return msg; 135 | } 136 | 137 | projUV ll2xy( string lat, string lon ){ 138 | projUV ll; 139 | ll.u = dmstor(lon.c_str(),0); 140 | ll.v = dmstor(lat.c_str(),0); 141 | return pj_fwd(ll, p_proj); 142 | } 143 | 144 | void sendBackToRouter(){ 145 | if(flag != 100){ 146 | struct sockaddr_in addr; 147 | if( (sock_fd = socket( AF_INET, SOCK_STREAM, 0) ) < 0 ) perror("socket"); 148 | addr.sin_family = AF_INET; 149 | addr.sin_port = htons(23460); 150 | std::cout << "addr:" << router_addr << std::endl; 151 | addr.sin_addr.s_addr = inet_addr(router_addr.c_str()); 152 | connect(sock_fd, (struct sockaddr *)&addr, sizeof(struct sockaddr_in)); 153 | flag = 100; 154 | } 155 | 156 | std::stringstream ss; 157 | boost::archive::text_oarchive archive(ss); 158 | archive << r_message; 159 | 160 | ss.seekg(0, ios::end); 161 | if( send( sock_fd, ss.str().c_str(), ss.tellp(), 0 ) < 0 ) { 162 | perror( "send" ); 163 | } else { 164 | } 165 | } 166 | 167 | 168 | 169 | void createFolder(){ 170 | char cur_dir[1024]; 171 | getcwd(cur_dir, 1024); 172 | 173 | time_t t = time(nullptr); 174 | const tm* lt = localtime(&t); 175 | std::stringstream s; 176 | s<<"20"; 177 | s<tm_year-100; //100を引くことで20xxのxxの部分になる 178 | s<<"-"; 179 | s<tm_mon+1; //月を0からカウントしているため 180 | s<<"-"; 181 | s<tm_mday; //そのまま 182 | s<<"_"; 183 | s<tm_hour; 184 | s<<":"; 185 | s<tm_min; 186 | s<<":"; 187 | s<tm_sec; 188 | std::string timestamp = s.str(); 189 | 190 | std::string filename = std::string(cur_dir) + "/../output/delay/autoware_delay_" + timestamp + ".csv"; 191 | std::cout << "filename:" << filename << std::endl; 192 | delay_output_file.open(filename, std::ios::out); 193 | 194 | filename = std::string(cur_dir) + "/../output/1_2_delay/1_2_delay_" + timestamp + ".csv"; 195 | std::cout << "filename:" << filename << std::endl; 196 | one_two_delay_file.open(filename, std::ios::out); 197 | 198 | filename = std::string(cur_dir) + "/../output/timestamp_record/timestamp_record_" + timestamp + ".csv"; 199 | std::cout << "filename:" << filename << std::endl; 200 | timestamp_record_file.open(filename, std::ios::out); 201 | } 202 | 203 | void output_file_config(){ 204 | flag = -1; 205 | char cur_dir[1024]; 206 | getcwd(cur_dir, 1024); 207 | time_t t = time(nullptr); 208 | const tm* lt = localtime(&t); 209 | std::stringstream s; 210 | s<<"20"; 211 | s<tm_year-100; //100を引くことで20xxのxxの部分になる 212 | s<<"-"; 213 | s<tm_mon+1; //月を0からカウントしているため 214 | s<<"-"; 215 | s<tm_mday; //そのまま 216 | s<<"_"; 217 | s<tm_hour; 218 | s<<":"; 219 | s<tm_min; 220 | s<<":"; 221 | s<tm_sec; 222 | std::string timestamp = s.str(); 223 | 224 | std::string filename = std::string(cur_dir) + "/../delay/" + timestamp + ".csv"; 225 | std::cout << filename << std::endl; 226 | delay_output_file.open(filename, std::ios::out); 227 | } 228 | 229 | void createSocket(std::string router_addr){ 230 | //通信モードの時は使う 231 | struct sockaddr_in addr; 232 | if( (sockfd = socket( AF_INET, SOCK_STREAM, 0) ) < 0 ) perror( "socket" ); 233 | addr.sin_family = AF_INET; 234 | addr.sin_port = htons( 23457 ); 235 | addr.sin_addr.s_addr = inet_addr( router_addr.c_str() ); 236 | connect( sockfd, (struct sockaddr *)&addr, sizeof( struct sockaddr_in ) ); 237 | } 238 | 239 | void sendToRouter(){ 240 | // データ送信 241 | 242 | gettimeofday(&myTime, NULL); 243 | s_message.timestamp = myTime.tv_sec * 1000000 + myTime.tv_usec; 244 | auto it1 = s_message.speed.begin(); 245 | auto it2 = s_message.time.begin(); 246 | auto it3 = s_message.longitude.begin(); 247 | auto it4 = s_message.latitude.begin(); 248 | auto it5 = s_message.stationid.begin(); 249 | 250 | s_message.speed.insert(it1,speed * 100); 251 | s_message.time.insert(it2, ((generationUnixTimeSec*1000 + (int)generationUnixTimeNSec/1000000 - 1072850400000)) % 65536); 252 | s_message.longitude.insert(it3, longitude * 10000000); 253 | s_message.latitude.insert(it4, latitude * 10000000); 254 | s_message.stationid.insert(it5, 0); 255 | 256 | std::stringstream ss; 257 | { 258 | boost::archive::text_oarchive archive(ss); 259 | archive << s_message; 260 | } 261 | 262 | timestamp_record_file << s_message.timestamp << std::endl; 263 | ss.seekg(0, ios::end); 264 | if( send( sockfd, ss.str().c_str(), ss.tellp(), 0 ) < 0 ) { 265 | perror( "send" ); 266 | } else { 267 | } 268 | } 269 | 270 | std::string paramOrganize(std::string param){ //libproj setup. param is to specify which epsg_code you use. 271 | char **prm; 272 | std::string params; 273 | params=param+" no_defs"; 274 | istringstream si(params); 275 | 276 | int n_pair=1; // add one for last token "no_defs" 277 | for ( unsigned long i=0; i < params.size(); i++ ) 278 | if ( params[i] == '=' ) n_pair++; 279 | 280 | prm = new char *[n_pair]; 281 | for ( int i=0; i < n_pair; i++ ) { 282 | prm[i] = new char[256]; 283 | si >> prm[i]; 284 | } 285 | 286 | p_proj=pj_init(n_pair, prm); 287 | 288 | for ( int i=n_pair-1; i >=0; i-- ) { 289 | delete [] prm[i]; 290 | } 291 | delete [] prm; 292 | 293 | if ( !p_proj ) { 294 | cerr << "Failed to initialize the PROJ library\n"; 295 | exit(1); 296 | } 297 | return params; 298 | } 299 | 300 | void receiveFromRouter(){ 301 | std::cout << "*****receive setup at receiver" << std::endl; 302 | int sockfd; 303 | int client_sockfd; 304 | struct sockaddr_in addr; 305 | socklen_t len = sizeof( struct sockaddr_in ); 306 | struct sockaddr_in from_addr; 307 | char buf[4096]; 308 | 309 | memset( buf, 0, sizeof( buf ) ); 310 | if( ( sockfd = socket( AF_INET, SOCK_STREAM, 0 ) ) < 0 ) { 311 | perror( "socket" ); 312 | } 313 | addr.sin_family = AF_INET; 314 | addr.sin_port = htons( 23459 ); 315 | addr.sin_addr.s_addr = INADDR_ANY; 316 | if( bind( sockfd, (struct sockaddr *)&addr, sizeof( addr ) ) < 0 ) perror( "bind" ); 317 | if( listen( sockfd, SOMAXCONN ) < 0 ) perror( "listen" ); 318 | if( ( client_sockfd = accept( sockfd, (struct sockaddr *)&from_addr, &len ) ) < 0 ) perror( "accept" ); 319 | 320 | // 受信 321 | int rsize; 322 | while( 1 ) { 323 | std::stringstream ss; 324 | memset( buf, 0, sizeof( buf ) ); 325 | rsize = recv( client_sockfd, buf, sizeof( buf ), 0 ); 326 | ss << buf; 327 | 328 | boost::archive::text_iarchive archive(ss); 329 | archive >> r_message; 330 | 331 | std::cout << "receive from router" << std::endl; 332 | 333 | struct timeval myTime; // time_t構造体を定義．1970年1月1日からの秒数を格納するもの 334 | gettimeofday(&myTime, NULL); 335 | long timestamp = myTime.tv_sec * 1000000 + myTime.tv_usec; 336 | 337 | for(int i=0; i= 1000000000){ 397 | generationUnixTimeSec += 1; 398 | generationUnixTimeNSec -= 1000000000; 399 | } 400 | long delaySec = a1.sec - generationUnixTimeSec; 401 | long delayNSec = a1.nsec - generationUnixTimeNSec; 402 | if(delayNSec < 0){ 403 | delaySec -= 1; 404 | delayNSec = 1000000000 + delayNSec; 405 | } 406 | delay_output_file << std::setprecision(20) << ros::WallTime::now() << "," << delayNSec / 1000000000.0 << std::endl; 407 | } 408 | 409 | void callbackNdtPose(const geometry_msgs::PoseStamped msg){ 410 | if (isSender) { 411 | prevPose = nowPose; 412 | nowPose = msg; 413 | std::cout << "hello" << std::endl; 414 | timeCalc(); 415 | calcEgovehicleState(); 416 | sendToRouter(); 417 | } else { 418 | prevPose = nowPose; 419 | nowPose = msg; 420 | 421 | std::vector X, Y; 422 | 423 | for(int i=0; i rand(1, 99999); 457 | 458 | for(unsigned int i = 0; i < min((int)msg.objects.size(), (int)5); i++){ 459 | float sum_x = 0.0; 460 | float sum_y = 0.0; 461 | float sum_z = 0.0; 462 | for(unsigned int j = 0; j < msg.objects[i].convex_hull.polygon.points.size(); j++){ 463 | sum_x += msg.objects[i].convex_hull.polygon.points[j].x; 464 | sum_y += msg.objects[i].convex_hull.polygon.points[j].y; 465 | sum_z += msg.objects[i].convex_hull.polygon.points[j].z; 466 | } 467 | sum_x /= (float)msg.objects[i].convex_hull.polygon.points.size(); 468 | sum_y /= (float)msg.objects[i].convex_hull.polygon.points.size(); 469 | sum_z /= (float)msg.objects[i].convex_hull.polygon.points.size(); 470 | 471 | float rotated_view_x, rotated_view_y; 472 | rotated_view_x = sum_x * (std::cos(-yaw) * std::cos(-pitch)) + sum_y * (std::cos(-yaw) * std::sin(-pitch) * std::sin(-roll) - std::sin(-yaw) * std::cos(-roll)); 473 | rotated_view_y = sum_x * (std::sin(-yaw) * std::cos(-pitch)) + sum_y * (std::sin(-yaw) * std::sin(-pitch) * std::sin(-roll) + std::cos(-yaw) * std::cos(-roll)); 474 | 475 | 476 | projUV xy; 477 | xy.u = rotated_view_x + nowPose.pose.position.x; 478 | xy.v = rotated_view_y + nowPose.pose.position.y; 479 | 480 | projUV result = pj_inv(xy, p_proj); 481 | result.u /= DEG_TO_RAD; 482 | result.v /= DEG_TO_RAD; 483 | 484 | 485 | if(result.u < 150){ 486 | std::cout << "lat:" << result.u << ", lon:" << result.v << std::endl; 487 | s_message.longitude.push_back(result.u * 10000000); 488 | s_message.latitude.push_back(result.v * 10000000); 489 | s_message.speed.push_back(0); 490 | s_message.time.push_back(0); 491 | s_message.stationid.push_back(rand(mt)); 492 | } 493 | } 494 | std::cout << "detected objects:" << msg.objects.size() << std::endl; 495 | sendToRouter(); //ここで送る必要はあるか？ 496 | } 497 | 498 | void callbackTF(const tf2_msgs::TFMessage msg){ 499 | if(msg.transforms[0].header.frame_id == "/map" && msg.transforms[0].child_frame_id == "/base_link"){ 500 | tf2::Quaternion rot_q(msg.transforms[0].transform.rotation.x, msg.transforms[0].transform.rotation.y, msg.transforms[0].transform.rotation.z, msg.transforms[0].transform.rotation.w); 501 | tf2::Matrix3x3(rot_q).getRPY(roll, pitch, yaw); 502 | } 503 | } 504 | 505 | void receiveFromRouterAtSender(){ 506 | std::cout << "*****receive setup" << std::endl; 507 | int sockfd; 508 | int client_sockfd; 509 | struct sockaddr_in addr; 510 | socklen_t len = sizeof( struct sockaddr_in ); 511 | struct sockaddr_in from_addr; 512 | char buf[4096]; 513 | 514 | memset( buf, 0, sizeof( buf ) ); 515 | if( ( sockfd = socket( AF_INET, SOCK_STREAM, 0 ) ) < 0 ) { 516 | perror( "socket" ); 517 | } 518 | addr.sin_family = AF_INET; 519 | addr.sin_port = htons( 23458 ); 520 | addr.sin_addr.s_addr = INADDR_ANY; 521 | if( bind( sockfd, (struct sockaddr *)&addr, sizeof( addr ) ) < 0 ) perror( "bind" ); 522 | if( listen( sockfd, SOMAXCONN ) < 0 ) perror( "listen" ); 523 | if( ( client_sockfd = accept( sockfd, (struct sockaddr *)&from_addr, &len ) ) < 0 ) perror( "accept" ); 524 | 525 | // 受信 526 | int rsize; 527 | while( 1 ) { 528 | std::stringstream ss(std::ios::binary | std::ios::out | std::ios::in); 529 | memset( buf, 0, sizeof( buf ) ); 530 | rsize = recv( client_sockfd, buf, sizeof( buf ), 0 ); 531 | ss << buf; 532 | 533 | boost::archive::text_iarchive archive(ss); 534 | archive >> s_message; 535 | 536 | gettimeofday(&myTime, NULL); 537 | one_two_delay_file << myTime.tv_sec * 1000000 + myTime.tv_usec << "," << s_message.timestamp << std::endl; 538 | if ( rsize == 0 ) { 539 | break; 540 | } else if ( rsize == -1 ) { 541 | perror( "recv" ); 542 | } 543 | } 544 | close( client_sockfd ); 545 | close( sockfd ); 546 | 547 | } 548 | 549 | void loadOpt(int argc, char* argv[]){ 550 | isSender = true; 551 | int i, opt; 552 | opterr = 0; //getopt()のエラーメッセージを無効にする。 553 | while ((opt = getopt(argc, argv, "sr")) != -1) { 554 | //コマンドライン引数のオプションがなくなるまで繰り返す 555 | switch (opt) { 556 | case 's': 557 | break; 558 | 559 | case 'r': 560 | isSender = false; 561 | break; 562 | 563 | default: /* '?' */ 564 | //指定していないオプションが渡された場合 565 | printf("Usage: %s [-s] [-r] router_addr ...\n", argv[0]); 566 | // return -1; 567 | break; 568 | } 569 | } 570 | //オプション以外の引数を出力する 571 | for (i = optind; i < argc; i++) { 572 | router_addr = std::string(argv[i]); 573 | std::cout << router_addr << std::endl; 574 | break; 575 | } 576 | if(router_addr.length() < 4){ 577 | printf("Usage: %s [-s] [-r] router_addr ...\n", argv[0]); 578 | } 579 | } 580 | 581 | 582 | int main(int argc, char* argv[]) { 583 | 584 | loadOpt(argc, argv); 585 | paramOrganize("proj=tmerc lat_0=36 lon_0=139.8333333333333 k=0.9999 x_0=0 y_0=0 ellps=GRS80 units=m"); 586 | 587 | if(isSender){ 588 | mt = std::mt19937(rnd()); 589 | mThreadReceiveFromRouter = new boost::thread(boost::ref(receiveFromRouterAtSender)); 590 | createFolder(); 591 | createSocket(router_addr); 592 | 593 | ros::init(argc, argv, "listener"); 594 | ros::NodeHandle n,n2; 595 | ros::Subscriber sub1 = n.subscribe("ndt_pose", 1024, callbackNdtPose); 596 | ros::Subscriber sub2 = n.subscribe("detection/lidar_detector/objects", 1024, callbackDetectionObjects); 597 | ros::Subscriber sub3 = n.subscribe("tf", 1024, callbackTF); 598 | ros::spin(); 599 | } else { 600 | output_file_config(); 601 | mThreadReceive = new boost::thread(boost::ref(receiveFromRouter)); 602 | ros::init(argc, argv, "sampleCatcher"); 603 | ros::NodeHandle n; 604 | 605 | ros::Subscriber sub2 = n.subscribe("tf", 1024, callbackTF); 606 | ros::Subscriber sub3 = n.subscribe("ndt_pose", 1024, callbackNdtPose); 607 | chatter_pub = n.advertise("detection/lidar_detector/objects", 10); 608 | 609 | box_line = createLine(); 610 | channel = createChannel("rgb"); 611 | ros::spin(); 612 | } 613 | 614 | return 0; 615 | } 616 | 617 | -------------------------------------------------------------------------------- /detectSurroundings.h: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "geometry_msgs/PoseStamped.h" 3 | #include "sensor_msgs/PointCloud2.h" 4 | #include "sensor_msgs/PointCloud.h" 5 | #include "sensor_msgs/point_cloud_conversion.h" 6 | #include "autoware_msgs/DetectedObjectArray.h" 7 | #include "tf2_msgs/TFMessage.h" 8 | #include "tf2/LinearMath/Quaternion.h" 9 | #include "tf2/LinearMath/Matrix3x3.h" 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include "projects.h" 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include "visualization_msgs/MarkerArray.h" 26 | // #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | 40 | 41 | struct socket_message_sender{ 42 | long timestamp; 43 | std::vector speed; 44 | std::vector latitude; 45 | std::vector longitude; 46 | std::vector time; 47 | std::vector stationid; 48 | 49 | private: 50 | friend class boost::serialization::access; 51 | template 52 | void serialize( Archive& ar, int version){ 53 | ar & timestamp; 54 | ar & speed; 55 | ar & latitude; 56 | ar & longitude; 57 | ar & time; 58 | ar & stationid; 59 | } 60 | }; 61 | 62 | struct socket_message_receiver{ 63 | long timestamp; 64 | float lat; 65 | float lon; 66 | std::vector speed; 67 | std::vector latitude; 68 | std::vector longitude; 69 | std::vector time; 70 | 71 | private: 72 | friend class boost::serialization::access; 73 | template 74 | void serialize( Archive& ar, unsigned int ver){ 75 | ar & timestamp; 76 | ar & lat; 77 | ar & lon; 78 | ar & speed; 79 | ar & latitude; 80 | ar & longitude; 81 | ar & time; 82 | } 83 | }; 84 | 85 | 86 | 87 | 88 | /* 89 | receiver only 90 | */ 91 | std::vector createConvexHull(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4); 92 | sensor_msgs::ChannelFloat32 createChannel(std::string name); 93 | std::vector createLine(); 94 | autoware_msgs::DetectedObjectArray createObjectArray(std::vector X, std::vector Y); 95 | std::string paramOrganize(std::string param); 96 | projUV ll2xy( std::string lat, std::string lon ); 97 | void sendBackToRouter(); 98 | void output_file_config(); 99 | void receiveFromRouterAtReceiver(); 100 | 101 | ros::Publisher chatter_pub; 102 | tf2::Quaternion rotated_position; 103 | tf2::Quaternion original_position; 104 | float lat, lon; 105 | std::vector box_line; 106 | sensor_msgs::ChannelFloat32 channel; 107 | int sock_fd; 108 | int flag; 109 | socket_message_receiver r_message; 110 | boost::thread* mThreadReceive; 111 | 112 | 113 | /* 114 | sender only 115 | */ 116 | void sendToRouter(); 117 | void calcEgovehicleState(); 118 | void timeCalc(); 119 | void createSocket(std::string router_addr); 120 | void createFolder(); 121 | void callbackDetectionObjects(const autoware_msgs::DetectedObjectArray msg); 122 | void receiveFromRouterAtSender(); 123 | 124 | long generationUnixTimeSec; 125 | long generationUnixTimeNSec; 126 | int sockfd; 127 | boost::thread *mThreadReceiveFromRouter; 128 | std::ofstream one_two_delay_file; 129 | std::ofstream timestamp_record_file; 130 | socket_message_sender s_message; 131 | 132 | 133 | /* 134 | common 135 | */ 136 | void callbackNdtPose(const geometry_msgs::PoseStamped msg); 137 | void callbackTF(const tf2_msgs::TFMessage msg); 138 | void loadOpt(int argc, char* argv[]); 139 | std::string paramOrganize(std::string param); 140 | 141 | 142 | double speed; 143 | double longitude; 144 | double latitude; 145 | geometry_msgs::PoseStamped nowPose; 146 | geometry_msgs::PoseStamped prevPose; 147 | float generationUnixTime; 148 | bool isSender; 149 | std::string router_addr; 150 | PJ *p_proj; 151 | double roll, yaw, pitch; 152 | std::ofstream delay_output_file; 153 | struct timeval myTime; 154 | 155 | std::random_device rnd; 156 | std::mt19937 mt; 157 | 158 | -------------------------------------------------------------------------------- /serialize_sample.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | struct Pokemon { 12 | std::string name; 13 | int hp = 0; 14 | 15 | template 16 | void serialize(Archive & archive) 17 | { 18 | archive(hp, name); 19 | } 20 | }; 21 | 22 | struct socket_message{ 23 | long timestamp; 24 | std::vector speed; 25 | std::vector latitude; 26 | std::vector longitude; 27 | std::vector time; 28 | std::vector stationid; 29 | 30 | template 31 | void serialize( Archive& ar){ 32 | ar(timestamp, speed, latitude, longitude, time, stationid); 33 | } 34 | }; 35 | 36 | 37 | 38 | int main() 39 | { 40 | socket_message msg; 41 | msg.timestamp = 10000; 42 | msg.longitude.push_back(1); 43 | msg.latitude.push_back(1); 44 | msg.time.push_back(1); 45 | msg.stationid.push_back(1); 46 | msg.speed.push_back(1); 47 | 48 | std::stringstream ss; 49 | { 50 | cereal::BinaryOutputArchive o_archive(ss); 51 | o_archive(msg); 52 | } 53 | 54 | std::cout << ss.str() << std::endl; 55 | 56 | socket_message recv_msg; 57 | cereal::BinaryInputArchive i_archive(ss); 58 | i_archive(recv_msg); 59 | 60 | std::cout << recv_msg.timestamp << std::endl; 61 | std::cout << recv_msg.longitude[0] << std::endl; 62 | 63 | 64 | // Pokemon pokemon; 65 | // pokemon.name = "PIKACHU"; 66 | // pokemon.hp = 100; 67 | 68 | // std::stringstream ss; 69 | // { 70 | // cereal::BinaryOutputArchive o_archive(ss); 71 | // o_archive(pokemon); 72 | // } 73 | // std::cout << ss.str() << std::endl; 74 | 75 | // Pokemon pokemon_i; 76 | // cereal::BinaryInputArchive i_archive(ss); 77 | // i_archive(pokemon_i); 78 | 79 | // std::cout << pokemon_i.hp << std::endl; 80 | // std::cout << pokemon_i.name << std::endl; 81 | 82 | 83 | #ifdef _MSC_VER 84 | system("pause"); 85 | #endif 86 | return 0; 87 | } 88 | --------------------------------------------------------------------------------