00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00035 #include <iostream>
00036 #include <ros/ros.h>
00037 #include <string>
00038 #include <vector>
00039
00040 #include <stdio.h>
00041 #include <time.h>
00042
00043
00044
00045
00046 #include <database_interface/db_class.h>
00047 #include <boost/shared_ptr.hpp>
00048 #include <database_interface/postgresql_database.h>
00049 #include <libpq-fe.h>
00050
00051
00052 #include <atlascar_base/ManagerStatus.h>
00053 #include <sensor_msgs/NavSatFix.h>
00054 #include <gps_common/GPSFix.h>
00055
00056 #include <boost/thread/thread.hpp>
00057
00058 using namespace std;
00059
00060
00061 int argc_g;
00062 char** argv_g;
00063
00064
00065 const string currentDateTimedate();
00066 const string currentDateTime();
00072 class Database: public database_interface::PostgresqlDatabase
00073 {
00074 public:
00075 Database(std::string server,std::string port,std::string username,std::string pass,std::string table):
00076 PostgresqlDatabase("lars.mec.ua.pt", "5432","atlas", "atlas2013", "atlas_monitoring")
00077 {
00078 touch();
00079 }
00080
00081 class PGresultAutoPtr
00082 {
00083 private:
00084 PGresult* result_;
00085 public:
00086 PGresultAutoPtr(PGresult *ptr):
00087 result_(ptr)
00088 {}
00089
00090 ~PGresultAutoPtr()
00091 {
00092 PQclear(result_);
00093 }
00094
00095 void reset(PGresult *ptr)
00096 {
00097 PQclear(result_);
00098 result_=ptr;
00099 }
00100
00101 PGresult* operator * ()
00102 {
00103 return result_;
00104 }
00105 };
00106
00107 bool cleanTable()
00108 {
00109 std::string query("DELETE FROM data_table_atlas;");
00110 PGresultAutoPtr result(PQexec(connection_, query.c_str()));
00111 return true;
00112 }
00113
00114
00115 bool copyTable()
00116 {
00117 std::string query("COPY data_table_atlas TO '/opt/atlascar/atlascar_" + currentDateTimedate()+"_"+currentDateTime()+".csv' DELIMITER ',' CSV HEADER;");
00118 PGresultAutoPtr result(PQexec(connection_, query.c_str()));
00119 return true;
00120 }
00121
00122 void touch()
00123 {
00124 tic_=ros::Time::now();
00125 }
00126
00127 ros::Time tic_;
00128 };
00134 class DatabaseStructure : public database_interface::DBClass
00135 {
00136
00137
00138
00139 public:
00140
00141
00142
00143
00144
00145 database_interface::DBField<int> id_;
00146 database_interface::DBField<int> rpm_;
00147 database_interface::DBField<double> speed_;
00148 database_interface::DBField<std::string> db_time_;
00149 database_interface::DBField<std::string> atlas_time_;
00150 database_interface::DBField<int> throttle_;
00151 database_interface::DBField<int> brake_;
00152 database_interface::DBField<int> clutch_;
00153 database_interface::DBField<int> gear_;
00154 database_interface::DBField<double> steering_;
00155 database_interface::DBField<double> throttle_press_;
00156 database_interface::DBField<double> brake_press_;
00157 database_interface::DBField<double> clutch_press_;
00158 database_interface::DBField<int> auto_throttle_;
00159 database_interface::DBField<int> auto_brake_;
00160 database_interface::DBField<int> auto_clutch_;
00161 database_interface::DBField<int> auto_steering_;
00162 database_interface::DBField<int> auto_ignition_;
00163 database_interface::DBField<int> handbrake_;
00164 database_interface::DBField<int> emergency_;
00165 database_interface::DBField<int> lights_minimum_;
00166 database_interface::DBField<int> lights_medium_;
00167 database_interface::DBField<int> lights_high_;
00168 database_interface::DBField<int> lights_left_;
00169 database_interface::DBField<int> lights_right_;
00170 database_interface::DBField<int> lights_brake_;
00171 database_interface::DBField<int> lights_reverse_;
00172 database_interface::DBField<int> lights_warning_;
00173 database_interface::DBField<int> horn_;
00174 database_interface::DBField<double> lat_gps_;
00175 database_interface::DBField<double> long_gps_;
00176 database_interface::DBField<double> alt_gps_;
00177 database_interface::DBField<double> track_gps_;
00178
00179
00180
00181 database_interface::DBField<int> empty_field3_;
00182 database_interface::DBField<int> empty_field4_;
00183
00184
00185
00186
00187
00188
00189
00190 DatabaseStructure() :
00191 id_(database_interface::DBFieldBase::TEXT,
00192 this, "id", "data_table_atlas", true),
00193 rpm_(database_interface::DBFieldBase::TEXT,
00194 this, "rpm", "data_table_atlas", true),
00195 speed_(database_interface::DBFieldBase::TEXT,
00196 this, "speed", "data_table_atlas", true),
00197 db_time_(database_interface::DBFieldBase::TEXT,
00198 this, "db_time", "data_table_atlas", true),
00199 atlas_time_(database_interface::DBFieldBase::TEXT,
00200 this, "atlas_time", "data_table_atlas", true),
00201 throttle_(database_interface::DBFieldBase::TEXT,
00202 this, "throttle", "data_table_atlas", true),
00203 brake_(database_interface::DBFieldBase::TEXT,
00204 this, "brake", "data_table_atlas", true),
00205 clutch_(database_interface::DBFieldBase::TEXT,
00206 this, "clutch", "data_table_atlas", true),
00207 gear_(database_interface::DBFieldBase::TEXT,
00208 this, "gear", "data_table_atlas", true),
00209 steering_(database_interface::DBFieldBase::TEXT,
00210 this, "steering", "data_table_atlas", true),
00211 throttle_press_(database_interface::DBFieldBase::TEXT,
00212 this, "throttle_press", "data_table_atlas", true),
00213 brake_press_(database_interface::DBFieldBase::TEXT,
00214 this, "brake_press", "data_table_atlas", true),
00215 clutch_press_(database_interface::DBFieldBase::TEXT,
00216 this, "clutch_press", "data_table_atlas", true),
00217 auto_throttle_(database_interface::DBFieldBase::TEXT,
00218 this, "auto_throttle", "data_table_atlas", true),
00219 auto_brake_(database_interface::DBFieldBase::TEXT,
00220 this, "auto_brake", "data_table_atlas", true),
00221 auto_clutch_(database_interface::DBFieldBase::TEXT,
00222 this, "auto_clutch", "data_table_atlas", true),
00223 auto_steering_(database_interface::DBFieldBase::TEXT,
00224 this, "auto_steering", "data_table_atlas", true),
00225 auto_ignition_(database_interface::DBFieldBase::TEXT,
00226 this, "auto_ignition", "data_table_atlas", true),
00227 handbrake_(database_interface::DBFieldBase::TEXT,
00228 this, "handbrake", "data_table_atlas", true),
00229 emergency_(database_interface::DBFieldBase::TEXT,
00230 this, "emergency", "data_table_atlas", true),
00231 lights_minimum_(database_interface::DBFieldBase::TEXT,
00232 this, "lights_minimum", "data_table_atlas", true),
00233 lights_medium_(database_interface::DBFieldBase::TEXT,
00234 this, "lights_medium", "data_table_atlas", true),
00235 lights_high_(database_interface::DBFieldBase::TEXT,
00236 this, "lights_high", "data_table_atlas", true),
00237 lights_left_(database_interface::DBFieldBase::TEXT,
00238 this, "lights_left", "data_table_atlas", true),
00239 lights_right_(database_interface::DBFieldBase::TEXT,
00240 this, "lights_right", "data_table_atlas", true),
00241 lights_brake_(database_interface::DBFieldBase::TEXT,
00242 this, "lights_brake", "data_table_atlas", true),
00243 lights_reverse_(database_interface::DBFieldBase::TEXT,
00244 this, "lights_reverse", "data_table_atlas", true),
00245 lights_warning_(database_interface::DBFieldBase::TEXT,
00246 this, "lights_warning", "data_table_atlas", true),
00247 horn_(database_interface::DBFieldBase::TEXT,
00248 this, "horn", "data_table_atlas", true),
00249 lat_gps_(database_interface::DBFieldBase::TEXT,
00250 this, "lat_gps", "data_table_atlas", true),
00251 long_gps_(database_interface::DBFieldBase::TEXT,
00252 this, "long_gps", "data_table_atlas", true),
00253 alt_gps_(database_interface::DBFieldBase::TEXT,
00254 this, "alt_gps", "data_table_atlas", true),
00255 track_gps_(database_interface::DBFieldBase::TEXT,
00256 this, "track_gps", "data_table_atlas", true),
00257 empty_field3_(database_interface::DBFieldBase::TEXT,
00258 this, "empty_field3", "data_table_atlas", true),
00259 empty_field4_(database_interface::DBFieldBase::TEXT,
00260 this, "empty_field4", "data_table_atlas", true)
00261
00262 {
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275 primary_key_field_ = &id_;
00276
00277 fields_.push_back(&rpm_);
00278 fields_.push_back(&speed_);
00279
00280 fields_.push_back(&db_time_);
00281 fields_.push_back(&atlas_time_);
00282 fields_.push_back(&throttle_);
00283 fields_.push_back(&brake_);
00284 fields_.push_back(&clutch_);
00285 fields_.push_back(&gear_);
00286 fields_.push_back(&steering_);
00287 fields_.push_back(&throttle_press_);
00288 fields_.push_back(&brake_press_);
00289 fields_.push_back(&clutch_press_);
00290 fields_.push_back(&auto_throttle_);
00291 fields_.push_back(&auto_brake_);
00292 fields_.push_back(&auto_clutch_);
00293 fields_.push_back(&auto_steering_);
00294 fields_.push_back(&auto_ignition_);
00295 fields_.push_back(&handbrake_);
00296 fields_.push_back(&emergency_);
00297 fields_.push_back(&lights_minimum_);
00298 fields_.push_back(&lights_medium_);
00299 fields_.push_back(&lights_high_);
00300 fields_.push_back(&lights_left_);
00301 fields_.push_back(&lights_right_);
00302 fields_.push_back(&lights_brake_);
00303 fields_.push_back(&lights_reverse_);
00304 fields_.push_back(&lights_warning_);
00305 fields_.push_back(&horn_);
00306 fields_.push_back(&lat_gps_);
00307 fields_.push_back(&long_gps_);
00308 fields_.push_back(&alt_gps_);
00309 fields_.push_back(&track_gps_);
00310
00311
00312
00313 fields_.push_back(&empty_field3_);
00314 fields_.push_back(&empty_field4_);
00315
00316 db_time_.data()=currentDateTimedate();
00317 throttle_press_.data()=0;
00318 brake_press_.data()=0;
00319 clutch_press_.data()=0;
00320 lights_left_.data()=0;
00321 lights_right_.data()=0;
00322 horn_.data()=0;
00323 lat_gps_.data()=0;
00324 long_gps_.data()=0;
00325 alt_gps_.data()=0;
00326 track_gps_.data()=0;
00327 empty_field3_.data()=0;
00328 empty_field4_.data()=0;
00329
00330 rpm_.data()=0;
00331
00332 throttle_.data()=0;
00333 brake_.data()=0;
00334 clutch_.data()=0;
00335 gear_.data()=0;
00336
00337 steering_.data()=0;
00338
00339 auto_throttle_.data()=0;
00340 auto_brake_.data()=0;
00341 auto_clutch_.data()=0;
00342 auto_steering_.data()=0;
00343 auto_ignition_.data()=0;
00344
00345 lights_minimum_.data()=0;
00346 lights_medium_.data()=0;
00347 lights_high_.data()=0;
00348 handbrake_.data()=0;
00349
00350 lights_brake_.data()=0;
00351 lights_reverse_.data()=0;
00352 lights_warning_.data()=0;
00353 emergency_.data()=0;
00354
00355
00356
00357 setAllFieldsReadFromDatabase(true);
00358
00359
00360 setAllFieldsWriteToDatabase(true);
00361
00362
00363
00364
00365 }
00366
00367
00368
00369
00370
00371 std::vector<database_interface::DBFieldBase*> getFields()
00372 {
00373 return fields_;
00374 }
00375
00376 };
00377
00378 typedef boost::shared_ptr<DatabaseStructure> DatabaseStructurePtr;
00379
00380
00381
00389 DatabaseStructurePtr getDatabaseStructureById(vector<DatabaseStructurePtr>& atlas_status,int id)
00390 {
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406 DatabaseStructurePtr null;
00407 return null;
00408 }
00414 ostream& operator<<(ostream& o, vector<DatabaseStructurePtr>& v)
00415 {
00416 o << "Atlas Status size: " << v.size() <<endl;
00417
00418 for (size_t i=0; i<v.size(); i++)
00419 {
00420 o << "Variaveis do AtlasCar: \n";
00421
00422 o << " id: " << v[i]->id_.data() << "\n";
00423 o << " rpm: " << v[i]->rpm_.data() << "\n";
00424 o << " speed: " << v[i]->speed_.data() << "\n";
00425 o << " db_time: " << v[i]->db_time_.data() << "\n";
00426 o << " atlas_time: " << v[i]->atlas_time_.data() << "\n";
00427 o << " throttle: " << v[i]->throttle_.data() << "\n";
00428 o << " brake: " << v[i]->brake_.data() << "\n";
00429 o << " clutch: " << v[i]->clutch_.data() << "\n";
00430 o << " gear: " << v[i]->gear_.data() << "\n";
00431 o << " steering: " << v[i]->steering_.data() << "\n";
00432 o << " throttle press: " << v[i]->throttle_press_.data() << "\n";
00433 o << " brake press: " << v[i]->brake_press_.data() << "\n";
00434 o << " clutch press: " << v[i]->clutch_press_.data() << "\n";
00435 o << " auto throttle: " << v[i]->auto_throttle_.data() << "\n";
00436 o << " auto brake: " << v[i]->auto_brake_.data() << "\n";
00437 o << " auto clutch: " << v[i]->auto_clutch_.data() << "\n";
00438 o << " auto steering: " << v[i]->auto_steering_.data() << "\n";
00439 o << " auto ignition: " << v[i]->auto_ignition_.data() << "\n";
00440 o << " handbrake: " << v[i]->handbrake_.data() << "\n";
00441 o << " emergency: " << v[i]->emergency_.data() << "\n";
00442 o << " lights minimum: " << v[i]->lights_minimum_.data() << "\n";
00443 o << " lights medium: " << v[i]->lights_medium_.data() << "\n";
00444 o << " lights high: " << v[i]->lights_high_.data() << "\n";
00445 o << " lights left: " << v[i]->lights_left_.data() << "\n";
00446 o << " lights right: " << v[i]->lights_right_.data() << "\n";
00447 o << " lights brake: " << v[i]->lights_brake_.data() << "\n";
00448 o << " lights reverse: " << v[i]->lights_reverse_.data() << "\n";
00449 o << " lights warning: " << v[i]->lights_warning_.data() << "\n";
00450 o << " horn: " << v[i]->horn_.data() << "\n";
00451 o << " lat gps: " << v[i]->lat_gps_.data() << "\n";
00452 o << " long gps: " << v[i]->long_gps_.data() << "\n";
00453 }
00454
00455 return o;
00456 }
00457
00458 using namespace std;
00459 DatabaseStructure atlas_insert;
00460
00466
00467 const string currentDateTime()
00468 {
00469 time_t now = time(0);
00470 struct tm tstruct;
00471 char buf[80];
00472 tstruct = *localtime(&now);
00473
00474
00475 strftime(buf, sizeof(buf), "%X", &tstruct);
00476 return buf;
00477 }
00478
00484 const string currentDateTimedate()
00485 {
00486 time_t now = time(0);
00487 struct tm tstruct;
00488 char buf[80];
00489 tstruct = *localtime(&now);
00490
00491
00492 strftime(buf, sizeof(buf), "%Y-%m-%d", &tstruct);
00493 return buf;
00494 }
00495
00501 void ManagerStatusCallback(const atlascar_base::ManagerStatusPtr& msg)
00502 {
00503 atlas_insert.db_time_.data()=currentDateTimedate();
00504
00505 atlas_insert.rpm_.data()=msg->rpm;
00506 double velocidade;
00507 velocidade=((msg->velocity)*3600)/1000;
00508
00509 atlas_insert.speed_.data()=velocidade;
00510 atlas_insert.throttle_.data()=msg->throttle;
00511 atlas_insert.brake_.data()=msg->brake;
00512 atlas_insert.clutch_.data()=msg->clutch;
00513 atlas_insert.gear_.data()=msg->gear;
00514
00515 atlas_insert.steering_.data()=msg->steering_wheel;
00516
00517 atlas_insert.auto_throttle_.data()=msg->auto_throttle;
00518 atlas_insert.auto_brake_.data()=msg->auto_brake;
00519 atlas_insert.auto_clutch_.data()=msg->auto_clutch;
00520 atlas_insert.auto_steering_.data()=msg->auto_direction;
00521 atlas_insert.auto_ignition_.data()=msg->auto_ignition;
00522
00523 atlas_insert.lights_medium_.data()=msg->lights_medium;
00524 atlas_insert.lights_high_.data()=msg->lights_high;
00525
00526
00527 atlas_insert.lights_brake_.data()=msg->lights_brake;
00528 atlas_insert.lights_reverse_.data()=msg->lights_reverse;
00529 atlas_insert.lights_warning_.data()=msg->lights_warning;
00530 atlas_insert.emergency_.data()=msg->emergency;
00531 atlas_insert.throttle_press_.data()=msg->throttle;
00532 atlas_insert.brake_press_.data()=msg->brake;
00533 atlas_insert.clutch_press_.data()=msg->clutch;
00534
00535 atlas_insert.lights_left_.data()=msg->lights_left;
00536 atlas_insert.lights_right_.data()=msg->lights_right;
00537
00538 atlas_insert.horn_.data()=msg->horn;
00539
00540
00541 atlas_insert.handbrake_.data()=0;
00542 atlas_insert.lights_minimum_.data()=0;
00543 atlas_insert.empty_field3_.data()=0;
00544 atlas_insert.empty_field4_.data()=0;
00545 }
00546
00547
00554 void NavSatFixCallback(const sensor_msgs::NavSatFixPtr& nav_sat)
00555 {
00556
00557
00558
00559 }
00560
00561 void GpsFixCallback(const gps_common::GPSFixPtr& gps)
00562 {
00563 atlas_insert.lat_gps_.data()=gps->latitude;
00564 atlas_insert.long_gps_.data()=gps->longitude;
00565 atlas_insert.alt_gps_.data()=gps->altitude;
00566 atlas_insert.track_gps_.data()=gps->track;
00567 }
00568
00569 void watchdog(Database**database)
00570 {
00571 ros::Rate r(50);
00572
00573 while(ros::ok())
00574 {
00575 r.sleep();
00576
00577 if( (ros::Time::now() - (*database)->tic_).toSec() > 5. )
00578 {
00579 cout<<"disconnected"<<endl;
00580 cout<<"restarting process"<<endl;
00581 if(execvp(argv_g[0],argv_g)<0)
00582 perror("Cannot launch new process");
00583 }
00584
00585 }
00586 }
00587
00593 int main(int argc,char**argv)
00594 {
00595 argc_g=argc;
00596 argv_g=argv;
00597
00598 ros::init(argc, argv, "listener");
00599 ros::NodeHandle n("~");
00600
00601 ros::Subscriber sub_driver = n.subscribe("manager_status", 1000, ManagerStatusCallback);
00602 ros::Subscriber sub_gps_fix = n.subscribe("gps_fix", 1000, GpsFixCallback);
00603 ros::Subscriber sub_nav_sat = n.subscribe("nav_sat_fix", 1000, NavSatFixCallback);
00604
00605
00606 Database* database = new Database("lars.mec.ua.pt", "5432","atlas", "atlas2013", "atlas_monitoring");
00607 if (!database->isConnected())
00608 {
00609 std::cerr << "Database failed to connect \n";
00610 }else
00611 {
00612 std::cerr << "Database connected successfully \n";
00613
00614 std::vector< boost::shared_ptr<DatabaseStructure> > objects;
00615
00616 if (!database->getList(objects))
00617 {
00618 ROS_ERROR("Failed to get list of test objects");
00619 }else
00620 {
00621 if(objects.size()>0)
00622 {
00623 cout<<" The database isn't empty "<<endl;
00624 ROS_INFO("Retrieved %zd objects", objects.size());
00625 database->copyTable();
00626 cout<<" Database were copied "<<endl;
00627 }
00628 }
00629
00630 database->cleanTable();
00631 cout<<"Deleted database done! "<<endl;
00632 }
00633
00634
00635 boost::thread thread(boost::bind(watchdog,&database));
00636
00637 int i=0;
00638
00639 ros::Rate r(2);
00640 while(ros::ok())
00641 {
00642 r.sleep();
00643 ros::spinOnce();
00644
00645
00646 if(!database->isConnected())
00647 {
00648 delete database;
00649
00650
00651 database = new Database("lars.mec.ua.pt", "5432","atlas", "atlas2013", "atlas_monitoring");
00652 if (!database->isConnected())
00653 {
00654 std::cerr << "Database failed to connect \n";
00655 continue;
00656 }else
00657 {
00658 std::cerr << "Database connected successfully \n";
00659
00660 std::vector< boost::shared_ptr<DatabaseStructure> > objects;
00661
00662 if (!database->getList(objects))
00663 {
00664 ROS_ERROR("Failed to get list of test objects");
00665 continue;
00666 }
00667
00668 if(objects.size()>0)
00669 {
00670 cout<<" The database isn't empty "<<endl;
00671 ROS_INFO("Retrieved %zd objects", objects.size());
00672 database->copyTable();
00673 cout<<" Database were copied "<<endl;
00674 }
00675
00676 database->cleanTable();
00677
00678 cout<<"Deleted database done! "<<endl;
00679 if (!database->getList(objects))
00680 {
00681 ROS_ERROR("Failed to get list of test objects");
00682 continue;
00683 }
00684 ROS_INFO("Retrieved %zd objects", objects.size());
00685 }
00686 }
00687
00688
00689 atlas_insert.atlas_time_.data()=currentDateTime();
00690 atlas_insert.id_.data()=i++;
00691
00692
00693 database->deleteFromDatabase((database_interface::DBClass*)&atlas_insert);
00694 database->insertIntoDatabase((database_interface::DBClass*)&atlas_insert);
00695
00696 database->touch();
00697 }
00698
00699
00700 thread.join();
00701
00702 database->copyTable();
00703 cout<<"Backup database done! "<<endl;
00704
00705 database->cleanTable();
00706 cout<<"Deleted database done! "<<endl;
00707
00708 return 0;
00709 }