//wrap SendData class static void send_sql() { //I assume that values has 4 value ex: "0 0 0 0" "1 2 3 4" //And if setting the other number of value , sendData will be failed. //create header std::string data = make_header(1, 1); data += construct_select_statement(send_data_type); data += "\n"; string db_response; int ret = sd.Sender(data, db_response); if (ret == -1) { std::cerr << "Failed: sd.Sender" << std::endl; return; } std::cout << "return data: " << db_response << std::endl; std_msgs::String msg; msg.data = db_response.c_str(); marker_publisher(msg); }
//wrap SendData class static void send_sql() { int sql_num = car_num + person_num + current_pose_position.size(); std::cout << "sqlnum : " << sql_num << std::endl; //create header std::string value = make_header(2, sql_num); std::cout << "current_num=" << current_pose_position.size() << ", car_num=" << car_num << "(" << car_positions_array.size() << ")" << ",person_num=" << person_num << "(" << person_positions_array.size() << ")" << std::endl; //get data of car and person recognizing pthread_mutex_lock(&pose_lock_); if(car_positions_array.size() > 0) { for(size_t i = 0; i < car_positions_array.size(); i++) { value += makeSendDataDetectedObj(car_positions_array[i], CAR_TOPIC_NAME); } } car_positions_array.clear(); car_num = 0; if(person_positions_array.size() > 0) { for(size_t i = 0; i < person_positions_array.size(); i++) { value += makeSendDataDetectedObj(person_positions_array[i], PERSON_TOPIC_NAME); } } person_positions_array.clear(); person_num = 0; // my_location for(size_t i = 0; i < current_pose_position.size(); i++) { std::string timestamp; timestamp = getTimeStamp(current_pose_position[i].header.stamp.sec,current_pose_position[i].header.stamp.nsec); value += pose_to_insert_statement(current_pose_position[i].pose, timestamp, OWN_TOPIC_NAME); value += "\n"; } current_pose_position.clear(); pthread_mutex_unlock(&pose_lock_); #ifdef POS_DB_VERBOSE std::cout << "val=" << value.substr(POS_DB_HEAD_LEN) << std::endl; #endif /* POS_DB_VERBOSE */ std::string res; int ret = sd.Sender(value, res, sql_num); if (ret < 0) { std::cerr << "Failed: sd.Sender" << std::endl; return; } #ifdef POS_DB_VERBOSE std::cout << "retrun message from DBserver : " << res << std::endl; #endif /* POS_DB_VERBOSE */ return; }
//wrap SendData class void* wrapSender(void *tsd){ //get values from sample_corner_point , convert latitude and longitude, //and send database server. std::vector<CARPOS> car_position_vector(global_cp_vector.size()); vector<CARPOS>::iterator cp_iterator; //thread safe process for vector pthread_mutex_lock( &mutex ); std::copy(global_cp_vector.begin(), global_cp_vector.end(), car_position_vector.begin()); pthread_mutex_unlock( &mutex ); cp_iterator = car_position_vector.begin(); while(cp_iterator != car_position_vector.end()){ double U = (cp_iterator.x1 + cp_iterator.x2)/2; double V = (cp_iterator.y1 + cp_iterator.y2)/2; //convert sl.setOriginalValue(U,V,cp_iteartor.distance); LOCATION ress = sl.cal(); //get axial rotation and movement vector from camera to velodyne. ANGLE angle; MoveVector mvvector; axiMove am; LOCATION velocoordinate = am.cal(ress,angle,mvvector); /* I got my GPS location too.it`s my_xloc,my_yloc. */ double my_xloc = 35.180188; double my_yloc = 136.906565; /* process of conversion to absolute coodinate from relative coordinates. yes, I did! next... */ LOCATION reccoord = am.cal(velecoordinate,angle); calcoordinates cc; RESULT res = cc.cal(ress.X,ress.Z,my_xloc,my_yloc); //I assume that values has 4 value ex: "0,0,0,0" "1,2,3,4" //And if setting the other number of value , sendData will be failed. char values[100]; sprintf(values,"%f,%f,0,0",res.lat,res.log); char dbn[100] = "prius_data_store"; char ct[100] = "latitude,logitude,type,id"; SendData sd; sd.setData(values); sd.setDBName(dbn); sd.setColumnType(st); sd.Sender(); cp_iterator++; } return NULL; }
//wrap SendData class void* wrapSender(void *tsd){ //get values from sample_corner_point , convert latitude and longitude, //and send database server. std::vector<CARPOS> car_position_vector(global_cp_vector.size()); std::vector<CARPOS>::iterator cp_iterator; //thread safe process for vector pthread_mutex_lock( &mutex ); std::copy(global_cp_vector.begin(), global_cp_vector.end(), car_position_vector.begin()); pthread_mutex_unlock( &mutex ); cp_iterator = car_position_vector.begin(); while(cp_iterator != car_position_vector.end()){ //middle of right-lower and left-upper double U = (cp_iterator->x1 + cp_iterator->x2)/2; double V = (cp_iterator->y1 + cp_iterator->y2)/2; //convert sl.setOriginalValue(U,V,cp_iterator->distance); LOCATION ress = sl.cal(); //get axial rotation and movement vector from camera to velodyne. ANGLE angle; angle.thiX = 0; angle.thiY = 0; angle.thiZ = 0; MoveVector mvvector; mvvector.X = 0; mvvector.Y = 0; mvvector.Z = 0; axiMove am; LOCATION velocoordinate = am.cal(ress,angle,mvvector); /* I got my GPS location too.it`s my_xloc,my_yloc. and I convert to plane rectangular coordinate from latitude and longitude. */ /* our rectangular plane is 6 in Japan. */ double lat_plane = 36;//136.906565; double lon_plane = 137.1;//35.180188; //sample Longitude and Latitude 3513.1345669,N,13658.9971525,E double my_xloc= 3513.1345669; double my_yloc= 13658.9971525; double my_zloc= 0; /* process of conversion to absolute coodinate from relative coordinates. yes, I did! next... */ geo_pos_conv geo; geo.set_plane(7); geo.set_llh_nmea_degrees(my_xloc,my_yloc,my_zloc); printf("geo : %f\t%f\n",geo.x(),geo.y()); LOCATION rescoord = am.cal(velocoordinate,angle); rescoord.X = geo.x(); rescoord.Y = 0; rescoord.Z = geo.y(); calcoordinates cc; RESULT res = cc.cal(rescoord.X,rescoord.Z,lat_plane,lon_plane); //I assume that values has 4 value ex: "0 0 0 0" "1 2 3 4" //And if setting the other number of value , sendData will be failed. char values[100]; sprintf(values,"%f %f 0 0",res.lat,res.log); char dbn[100] = "prius_data_store"; char ct[100] = "latitude logitude type id"; SendData sd; sd.setData(values); sd.setDBName(dbn); sd.setColumnType(ct); sd.Sender(); cp_iterator++; } }