From ec4b10b72336e94d41d3c2cb11327a2cc5d193fd Mon Sep 17 00:00:00 2001 From: Xuan0319 Date: Tue, 2 May 2023 00:56:54 +0800 Subject: [PATCH] add calculate data timer test --- class_model/CMakeLists.txt | 2 +- class_model/include/class_model/command.h | 13 +- .../include/class_model/convert_degree.h | 2 +- class_model/include/class_model/formation.h | 2 +- class_model/src/Leader.cpp | 4 +- class_model/src/bio_main.cpp | 23 ++-- class_model/src/bio_main2.cpp | 2 +- class_model/src/bio_main3.cpp | 2 +- class_model/src/bio_main4.cpp | 2 +- class_model/src/bio_main5.cpp | 2 +- class_model/src/command.cpp | 62 ++++++++++ class_model/src/drone2.cpp | 19 --- class_model/src/follower.cpp | 2 +- class_model/src/formation.cpp | 113 +++++++++++------- class_model/src/main.cpp | 3 +- class_model/src/mqttPubMain.py | 12 +- class_model/src/mqttPubMain2.py | 23 ++-- class_model/src/mqttPubMain3.py | 23 ++-- class_model/src/mqttPubMain4.py | 23 ++-- class_model/src/mqttPubMain5.py | 25 ++-- class_model/src/mqttSubMain.py | 3 +- class_model/src/utils/mqttConfig_PUB2.yml | 2 +- class_model/src/utils/mqttConfig_PUB3.yml | 2 +- class_model/src/utils/mqttConfig_PUB4.yml | 2 +- class_model/src/utils/mqttConfig_PUB5.yml | 2 +- .../utils/protoJson_mqtt_sub_data_from_ros.py | 3 +- class_model/src/utils/readConfig.py | 11 +- 27 files changed, 244 insertions(+), 140 deletions(-) delete mode 100755 class_model/src/drone2.cpp diff --git a/class_model/CMakeLists.txt b/class_model/CMakeLists.txt index 27a4360..84e2a33 100755 --- a/class_model/CMakeLists.txt +++ b/class_model/CMakeLists.txt @@ -232,7 +232,7 @@ add_dependencies(control_lib main_generate_messages_cpp) add_library(command_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/command.cpp) target_include_directories(command_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) -target_link_libraries(command_lib ${catkin_LIBRARIES}) +target_link_libraries(command_lib ${catkin_LIBRARIES} param_lib) add_dependencies(command_lib main_generate_messages_cpp) add_library(receiver_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/receiver.cpp) diff --git a/class_model/include/class_model/command.h b/class_model/include/class_model/command.h index e67f08a..790373a 100755 --- a/class_model/include/class_model/command.h +++ b/class_model/include/class_model/command.h @@ -1,11 +1,16 @@ /*Drone Control Command*/ #ifndef COMMAND_H #define COMMAND_H - +#define DEBUG #include #include #include #include +#include +#include +#include +#include + // #include class CommandClass { @@ -13,7 +18,13 @@ public: CommandClass(); virtual ~CommandClass(); void set_global_position(double lon,double lat,float alt,float yaw); + #ifdef DEBUG + ParamClass param_object; + void set_velocity(float x,float y,float alt,float yaw,float second,uint64_t get_t,uint64_t cal_t); + #endif + #ifndef DEBUG void set_velocity(float x,float y,float alt,float yaw,float second); + #endif void fix_velocity(float x,float y,float alt,float yaw,float second); void velocity_init(); void set_target_position(float x,float y); diff --git a/class_model/include/class_model/convert_degree.h b/class_model/include/class_model/convert_degree.h index e05ec93..271cb94 100755 --- a/class_model/include/class_model/convert_degree.h +++ b/class_model/include/class_model/convert_degree.h @@ -8,7 +8,7 @@ float ConvertDeg(float x ,float y){ degree = atan(y/x)*180/3.14 + 360; } if(x<0 && y<0){ - degree = (atan(y/x)*180/3.14) + 180; + degree = (atan(x/y)*180/3.14) + 180; } if(x==0 && y>=0){ degree = 0; diff --git a/class_model/include/class_model/formation.h b/class_model/include/class_model/formation.h index b8665de..ebfebac 100755 --- a/class_model/include/class_model/formation.h +++ b/class_model/include/class_model/formation.h @@ -1,7 +1,7 @@ /*Follow the leader in a fixed formation */ #ifndef FORMATION_H #define FORMATION_H - +#define DEBUG #include #include "class_model/sensor.h" #include "class_model/mode.h" diff --git a/class_model/src/Leader.cpp b/class_model/src/Leader.cpp index 57d9dfc..39b590b 100644 --- a/class_model/src/Leader.cpp +++ b/class_model/src/Leader.cpp @@ -100,7 +100,7 @@ void LeaderClass::go2target(float target_lon,float target_lat){ } // PubData_object.publishData(drone_speed); - command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1); + // command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1); // ROS_INFO("slope:%f,%f,%f",slope,x,y); // ROS_INFO("%f,%f",drone_speed.x,drone_speed.y); @@ -125,6 +125,6 @@ void LeaderClass::face2target(float target_heading){ error_yaw = 0; heading_status = 1; } - command_object.set_velocity(0 ,0 ,0,error_yaw,0.1); + // command_object.set_velocity(0 ,0 ,0,error_yaw,0.1); // ROS_INFO("face2target...%f",target_heading); } \ No newline at end of file diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp index 132b9dd..5787590 100644 --- a/class_model/src/bio_main.cpp +++ b/class_model/src/bio_main.cpp @@ -73,21 +73,20 @@ int main(int argc, char **argv) { M4 = request_object.get_M4_GPS(); if(checkLeader == 1){ + target.lon = config["routh1"]["x"].as()*1e7 ; + target.lat = config["routh1"]["y"].as()*1e7 + 500; leader_object.leader(target); - // for(int i=0;i<=360;i++){ - // target.lon = config["routh1"]["x"].as()*1e7 + 300*sin(i*0.2); - // target.lat = config["routh1"]["y"].as()*1e7 + 300*cos(i*0.2); - // leader_object.leader(target); - // std::cout<< "radin: " << i*3.14/180 <()*1e7 + 500; - target.lat = config["routh1"]["y"].as()*1e7 ; + + target.lon = config["routh1"]["x"].as()*1e7 + 1000; + target.lat = config["routh1"]["y"].as()*1e7 + 500; leader_object.leader(target); - target.lon = config["routh1"]["x"].as()*1e7 ; - target.lat = config["routh1"]["y"].as()*1e7 -500; + + target.lon = config["routh1"]["x"].as()*1e7 + 1000; + target.lat = config["routh1"]["y"].as()*1e7 - 500; leader_object.leader(target); - target.lon = config["routh1"]["x"].as()*1e7 -500; - target.lat = config["routh1"]["y"].as()*1e7 ; + + target.lon = config["routh1"]["x"].as()*1e7 ; + target.lat = config["routh1"]["y"].as()*1e7 - 500; leader_object.leader(target); }else{ diff --git a/class_model/src/bio_main2.cpp b/class_model/src/bio_main2.cpp index 033ca42..637a8b0 100644 --- a/class_model/src/bio_main2.cpp +++ b/class_model/src/bio_main2.cpp @@ -44,7 +44,7 @@ int main(int argc, char **argv) { mode_object.set_Mode("GUIDED"); control_object.arm(); - control_object.takeoff(2); + control_object.takeoff(3); // while(flag!=1){ // type = receiver_object.check_command(); // if(type == "go"){ diff --git a/class_model/src/bio_main3.cpp b/class_model/src/bio_main3.cpp index 5fc7873..b290cfe 100644 --- a/class_model/src/bio_main3.cpp +++ b/class_model/src/bio_main3.cpp @@ -44,7 +44,7 @@ int main(int argc, char **argv) { mode_object.set_Mode("GUIDED"); control_object.arm(); - control_object.takeoff(2); + control_object.takeoff(4); // while(flag!=1){ // type = receiver_object.check_command(); // if(type == "go"){ diff --git a/class_model/src/bio_main4.cpp b/class_model/src/bio_main4.cpp index 83f9d9c..2be79b0 100644 --- a/class_model/src/bio_main4.cpp +++ b/class_model/src/bio_main4.cpp @@ -44,7 +44,7 @@ int main(int argc, char **argv) { mode_object.set_Mode("GUIDED"); control_object.arm(); - control_object.takeoff(2); + control_object.takeoff(3); // while(flag!=1){ // type = receiver_object.check_command(); // if(type == "go"){ diff --git a/class_model/src/bio_main5.cpp b/class_model/src/bio_main5.cpp index 2f5d599..7bfc4c7 100644 --- a/class_model/src/bio_main5.cpp +++ b/class_model/src/bio_main5.cpp @@ -44,7 +44,7 @@ int main(int argc, char **argv) { mode_object.set_Mode("GUIDED"); control_object.arm(); - control_object.takeoff(2); + control_object.takeoff(4); // while(flag!=1){ // type = receiver_object.check_command(); // if(type == "go"){ diff --git a/class_model/src/command.cpp b/class_model/src/command.cpp index 0e8b9cb..0e048f8 100644 --- a/class_model/src/command.cpp +++ b/class_model/src/command.cpp @@ -153,6 +153,67 @@ int CommandClass::fix_position(global_location pre_location,float x,float y,floa return 1; } } + +#ifdef DEBUG +void CommandClass::set_velocity(float x,float y,float alt,float yaw,float second,uint64_t get_t,uint64_t cal_t){ + uint64_t fun_t = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + msg.twist.linear.x = x; + msg.twist.angular.x = x; + msg.twist.linear.y = y; + msg.twist.angular.y = y; + msg.twist.linear.z = alt; + msg.twist.angular.z = yaw; + // ROS_INFO("set_velocity: x:%f, y:%f, alt:%f, yaw:%f",x,y,alt,yaw); + // global_location pre_location=gps_object.gps_position(); + int flag = 1; + double begin = ros::Time::now().toSec(); + while(true){ + velocity_command.publish(msg); + uint64_t pub_t = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + int ID = param_object.getID(); + std::string path; + if(flag == 1){ + switch (ID) + { + case 1: + path = "/home/dodo/Desktop/TimerTest/drone1.csv"; + break; + + case 2: + path = "/home/dodo/Desktop/TimerTest/drone2.csv"; + break; + + case 3: + path = "/home/dodo/Desktop/TimerTest/drone3.csv"; + break; + } + std::fstream file(path,std::ios::app); + file << get_t << " " << cal_t << " " << fun_t <<" "<second){ + velocity_init(); + break; + } + + + // uint64_t last_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + + // while(true){ + // velocity_command.publish(msg); + // uint64_t now_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + // if((now_ms-last_ms)>second*1000){ + // // ROS_INFO("%ld",now_ms-last_ms); + // //velocity_init(); + // break; + // } + } +} +#endif + +#ifndef DEBUG void CommandClass::set_velocity(float x,float y,float alt,float yaw,float second){ msg.twist.linear.x = x; @@ -186,6 +247,7 @@ void CommandClass::set_velocity(float x,float y,float alt,float yaw,float second // } } } +#endif void CommandClass::set_target_position(float x ,float y ){ diff --git a/class_model/src/drone2.cpp b/class_model/src/drone2.cpp deleted file mode 100755 index dcc84de..0000000 --- a/class_model/src/drone2.cpp +++ /dev/null @@ -1,19 +0,0 @@ -#include "class_model/follower.h" - -int main(int argc, char **argv) { - - // Init ROS node - ros::init(argc, argv, "drone2_node"); - - // reate Assync spiner - ros::AsyncSpinner spinner(0); - spinner.start(); - - FollowerClass follower_object; - follower_object.follower(); - // RequestClass test_object; - - ros::waitForShutdown(); - - return 0; -} diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp index bd300c2..1817f2d 100755 --- a/class_model/src/follower.cpp +++ b/class_model/src/follower.cpp @@ -422,5 +422,5 @@ void FollowerClass::calculate_position(float k,float theta,int plane){ // ROS_INFO("drone%d,plane%d",self.ID,plane); - command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.1); + // command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.1); } diff --git a/class_model/src/formation.cpp b/class_model/src/formation.cpp index 9157b54..1456c8d 100644 --- a/class_model/src/formation.cpp +++ b/class_model/src/formation.cpp @@ -20,49 +20,49 @@ FormationClass::~FormationClass() { ros::shutdown(); } void FormationClass::leader(){ - int counter=0; - std::string command = "",pre_command = ""; + // int counter=0; + // std::string command = "",pre_command = ""; - while(true){ - if(flag==0){ - command_object.set_velocity(0,0,0,0.1,1); - sleep(5); - while(counter<=5){ - command_object.set_velocity(-1,1,0,0,1); - command = receiver_object.check_command(); + // while(true){ + // if(flag==0){ + // command_object.set_velocity(0,0,0,0.1,1); + // sleep(5); + // while(counter<=5){ + // command_object.set_velocity(-1,1,0,0,1); + // command = receiver_object.check_command(); - while(command =="stop" || command == "land"){ - command_object.set_velocity(0,0,0,0,1); - command = receiver_object.check_command(); + // while(command =="stop" || command == "land"){ + // command_object.set_velocity(0,0,0,0,1); + // command = receiver_object.check_command(); - if(command == "land"){ - mode_object.set_Mode("LAND"); - }else if(command != "stop"){ - break; - } + // if(command == "land"){ + // mode_object.set_Mode("LAND"); + // }else if(command != "stop"){ + // break; + // } - } + // } - counter++; - std::cout << command <=5){ - flag = 1; - } - command = receiver_object.check_command(); - // command_object.set_velocity(-1,1,0,0,10); - // // sleep(5); - // command_object.set_velocity(0,0,0,0,10); - // command_object.set_velocity(1,-1,0,0,10); - // command_object.set_velocity(1,-1,0,0,10); - // sleep(2); - } - if(command == "land"){ - mode_object.set_Mode("LAND"); - ROS_INFO("xxxx"); - } + // counter++; + // std::cout << command <=5){ + // flag = 1; + // } + // command = receiver_object.check_command(); + // // command_object.set_velocity(-1,1,0,0,10); + // // // sleep(5); + // // command_object.set_velocity(0,0,0,0,10); + // // command_object.set_velocity(1,-1,0,0,10); + // // command_object.set_velocity(1,-1,0,0,10); + // // sleep(2); + // } + // if(command == "land"){ + // mode_object.set_Mode("LAND"); + // ROS_INFO("xxxx"); + // } -} +// } } void FormationClass::leader1(float x,float y){ @@ -479,6 +479,10 @@ void FormationClass::calculate_position(float k,float theta,int direction){ theta = theta*3.14/180; leader_location=request_object.get_leader_GPS(); //get Leader/Follower 's GPS + #ifdef DEBUG + uint64_t get_data_t = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + #endif + follower_location=GPS_object.gps_position(); @@ -551,9 +555,17 @@ void FormationClass::calculate_position(float k,float theta,int direction){ // // ROS_INFO("error_degree:%f",error_degree); // // ROS_INFO("error_yaw:%f",error_yaw); // ROS_INFO("************************************"); - - // command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.3); - command_object.set_global_position(Pf[0]/1e5,Pf[1]/1e5,2,deg_phi); + #ifdef DEBUG + uint64_t cal_data_t = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + #endif + + #ifndef DEBUG + command_object.set_velocity(error_lon ,error_lat ,0,0,0.1); + #endif + #ifdef DEBUG + command_object.set_velocity(error_lon ,error_lat ,0,0,0.1,get_data_t,cal_data_t); + #endif + // command_object.set_global_position(Pf[0]/1e5,Pf[1]/1e5,2,deg_phi); // command_object.set_velocity(0,0,0,error_yaw,0.1); } @@ -636,13 +648,17 @@ void FormationClass::spherical_coordinate(float k,float theta,float psi){ ROS_INFO("Q[2]:%f ,Pf[2]:%f ,error_z:%f",k*sin(psi),Pf[2],error_z); // ROS_INFO("************************************"); - command_object.set_velocity(error_x,error_y,error_z,error_yaw,0.1); + // command_object.set_velocity(error_x,error_y,error_z,error_yaw,0.1); } void FormationClass::go2target(float x,float y){ leader_location=request_object.get_leader_GPS(); + #ifdef DEBUG + uint64_t get_data_t = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + #endif + current_location=GPS_object.gps_position(); float target_heading,error_heading; @@ -686,8 +702,17 @@ void FormationClass::go2target(float x,float y){ flag = 1; } + #ifdef DEBUG + uint64_t cal_data_t = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + #endif + PubData_object.publishData(drone_speed); + #ifndef DEBUG command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1); + #endif + #ifdef DEBUG + command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1,get_data_t,cal_data_t); + #endif // command_object.set_global_position(target_location.lon/1e5,target_location.lat/1e5,2,target_heading); // ROS_INFO("slope:%f,%f,%f",slope,x,y); @@ -713,7 +738,13 @@ void FormationClass::face2target(float target_heading){ error_yaw = 0; heading_status = 1; } + + #ifndef DEBUG command_object.set_velocity(0 ,0 ,0,error_yaw,0.1); + #endif + #ifdef DEBUG + command_object.set_velocity(0 ,0 ,0,error_yaw,0.1,0,0); + #endif ROS_INFO("face2target..."); } diff --git a/class_model/src/main.cpp b/class_model/src/main.cpp index 9aa1773..c6a1cb8 100755 --- a/class_model/src/main.cpp +++ b/class_model/src/main.cpp @@ -58,7 +58,6 @@ int main(int argc, char **argv) { // ROS_INFO("set"); } -// TODO: switch case if(type == "init"){ select_formation.goose_formation(); ROS_INFO("init formation"); @@ -69,7 +68,7 @@ int main(int argc, char **argv) { }else if(type == "row"){ select_formation.row_formation(); pre_type ="row"; - }else if(type == "v1"){ //PID error + }else if(type == "v1"){ select_formation.goose_formation(0,5); pre_type ="v1"; }else if(type == "v2"){ diff --git a/class_model/src/mqttPubMain.py b/class_model/src/mqttPubMain.py index 29ab218..d4b5b3f 100755 --- a/class_model/src/mqttPubMain.py +++ b/class_model/src/mqttPubMain.py @@ -61,7 +61,6 @@ if __name__ == '__main__': FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB.yml") cfg = utils.Read_PUB_Config(FilePath) client = utils.MQTTClient(cfg.MQTTClientNamePub) - # set log stream_log_format = "%(asctime)s - %(levelname)s - %(message)s" file_log_format = "%(message)s" @@ -72,19 +71,22 @@ if __name__ == '__main__': stream_handler.setFormatter(stream_formatter) stream_handler.setLevel(logging.DEBUG) - file_handler = logging.FileHandler(cfg.logFileName) + logpath = os.path.join(os.path.dirname(__file__),"log", cfg.logFileName) + file_handler = logging.FileHandler(logpath, mode='w') file_handler.setFormatter(file_formatter) file_handler.setLevel(logging.INFO) - logger = logging.getLogger(__name__) - logger.setLevel(logging.INFO) + + logger = logging.getLogger("__mqttPubMain__") + logger.setLevel(logging.DEBUG) logger.addHandler(file_handler) logger.addHandler(stream_handler) logger.debug(cfg) + # Mqtt client.on_connect = on_connect - client.on_publish = on_publish + # client.on_publish = on_publish client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain) client.loop_start() diff --git a/class_model/src/mqttPubMain2.py b/class_model/src/mqttPubMain2.py index ca546bc..33f1702 100755 --- a/class_model/src/mqttPubMain2.py +++ b/class_model/src/mqttPubMain2.py @@ -61,28 +61,31 @@ if __name__ == '__main__': FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB2.yml") cfg = utils.Read_PUB_Config(FilePath) client = utils.MQTTClient(cfg.MQTTClientNamePub) - # set log - log_format = "%(asctime)s - %(levelname)s - %(message)s" - formatter = logging.Formatter(log_format) + stream_log_format = "%(asctime)s - %(levelname)s - %(message)s" + file_log_format = "%(message)s" + file_formatter = logging.Formatter(file_log_format) + stream_formatter = logging.Formatter(stream_log_format) stream_handler = logging.StreamHandler() - stream_handler.setFormatter(formatter) + stream_handler.setFormatter(stream_formatter) stream_handler.setLevel(logging.DEBUG) - file_handler = logging.FileHandler(cfg.logFileName) - file_handler.setFormatter(formatter) + logpath = os.path.join(os.path.dirname(__file__),"log", cfg.logFileName) + file_handler = logging.FileHandler(logpath, mode='w') + file_handler.setFormatter(file_formatter) file_handler.setLevel(logging.INFO) - logger = logging.getLogger(__name__) - logger.setLevel(logging.INFO) + + logger = logging.getLogger("__mqttPubMain__") + logger.setLevel(logging.DEBUG) logger.addHandler(file_handler) logger.addHandler(stream_handler) - logger.info(cfg) + logger.debug(cfg) # Mqtt client.on_connect = on_connect - client.on_publish = on_publish + # client.on_publish = on_publish client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain) client.loop_start() diff --git a/class_model/src/mqttPubMain3.py b/class_model/src/mqttPubMain3.py index e7f8c33..c95f520 100755 --- a/class_model/src/mqttPubMain3.py +++ b/class_model/src/mqttPubMain3.py @@ -61,28 +61,31 @@ if __name__ == '__main__': FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB3.yml") cfg = utils.Read_PUB_Config(FilePath) client = utils.MQTTClient(cfg.MQTTClientNamePub) - # set log - log_format = "%(asctime)s - %(levelname)s - %(message)s" - formatter = logging.Formatter(log_format) + stream_log_format = "%(asctime)s - %(levelname)s - %(message)s" + file_log_format = "%(message)s" + file_formatter = logging.Formatter(file_log_format) + stream_formatter = logging.Formatter(stream_log_format) stream_handler = logging.StreamHandler() - stream_handler.setFormatter(formatter) + stream_handler.setFormatter(stream_formatter) stream_handler.setLevel(logging.DEBUG) - file_handler = logging.FileHandler(cfg.logFileName) - file_handler.setFormatter(formatter) + logpath = os.path.join(os.path.dirname(__file__),"log", cfg.logFileName) + file_handler = logging.FileHandler(logpath, mode='w') + file_handler.setFormatter(file_formatter) file_handler.setLevel(logging.INFO) - logger = logging.getLogger(__name__) - logger.setLevel(logging.INFO) + + logger = logging.getLogger("__mqttPubMain__") + logger.setLevel(logging.DEBUG) logger.addHandler(file_handler) logger.addHandler(stream_handler) - logger.info(cfg) + logger.debug(cfg) # Mqtt client.on_connect = on_connect - client.on_publish = on_publish + # client.on_publish = on_publish client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain) client.loop_start() diff --git a/class_model/src/mqttPubMain4.py b/class_model/src/mqttPubMain4.py index c46db5b..ddcbd75 100755 --- a/class_model/src/mqttPubMain4.py +++ b/class_model/src/mqttPubMain4.py @@ -61,28 +61,31 @@ if __name__ == '__main__': FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB4.yml") cfg = utils.Read_PUB_Config(FilePath) client = utils.MQTTClient(cfg.MQTTClientNamePub) - # set log - log_format = "%(asctime)s - %(levelname)s - %(message)s" - formatter = logging.Formatter(log_format) + stream_log_format = "%(asctime)s - %(levelname)s - %(message)s" + file_log_format = "%(message)s" + file_formatter = logging.Formatter(file_log_format) + stream_formatter = logging.Formatter(stream_log_format) stream_handler = logging.StreamHandler() - stream_handler.setFormatter(formatter) + stream_handler.setFormatter(stream_formatter) stream_handler.setLevel(logging.DEBUG) - file_handler = logging.FileHandler(cfg.logFileName) - file_handler.setFormatter(formatter) + logpath = os.path.join(os.path.dirname(__file__),"log", cfg.logFileName) + file_handler = logging.FileHandler(logpath, mode='w') + file_handler.setFormatter(file_formatter) file_handler.setLevel(logging.INFO) - logger = logging.getLogger(__name__) - logger.setLevel(logging.INFO) + + logger = logging.getLogger("__mqttPubMain__") + logger.setLevel(logging.DEBUG) logger.addHandler(file_handler) logger.addHandler(stream_handler) - logger.info(cfg) + logger.debug(cfg) # Mqtt client.on_connect = on_connect - client.on_publish = on_publish + # client.on_publish = on_publish client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain) client.loop_start() diff --git a/class_model/src/mqttPubMain5.py b/class_model/src/mqttPubMain5.py index 2e1e000..7191644 100755 --- a/class_model/src/mqttPubMain5.py +++ b/class_model/src/mqttPubMain5.py @@ -62,27 +62,30 @@ if __name__ == '__main__': cfg = utils.Read_PUB_Config(FilePath) client = utils.MQTTClient(cfg.MQTTClientNamePub) # set log - log_format = "%(asctime)s - %(levelname)s - %(message)s" - formatter = logging.Formatter(log_format) + stream_log_format = "%(asctime)s - %(levelname)s - %(message)s" + file_log_format = "%(message)s" + file_formatter = logging.Formatter(file_log_format) + stream_formatter = logging.Formatter(stream_log_format) stream_handler = logging.StreamHandler() - stream_handler.setFormatter(formatter) + stream_handler.setFormatter(stream_formatter) stream_handler.setLevel(logging.DEBUG) - file_handler = logging.FileHandler(cfg.logFileName) - file_handler.setFormatter(formatter) + logpath = os.path.join(os.path.dirname(__file__),"log", cfg.logFileName) + file_handler = logging.FileHandler(logpath, mode='w') + file_handler.setFormatter(file_formatter) file_handler.setLevel(logging.INFO) - logger = logging.getLogger(__name__) - logger.setLevel(logging.INFO) + + logger = logging.getLogger("__mqttPubMain__") + logger.setLevel(logging.DEBUG) logger.addHandler(file_handler) logger.addHandler(stream_handler) - logger.info(cfg) - print(cfg.msg_format) - + logger.debug(cfg) + # Mqtt client.on_connect = on_connect - client.on_publish = on_publish + # client.on_publish = on_publish client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain) client.loop_start() diff --git a/class_model/src/mqttSubMain.py b/class_model/src/mqttSubMain.py index 0bec8d5..e3418d2 100755 --- a/class_model/src/mqttSubMain.py +++ b/class_model/src/mqttSubMain.py @@ -71,7 +71,8 @@ if __name__ == '__main__': # Read Config FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_SUB.yml") cfg = utils.Read_SUB_Config(FilePath) - client = utils.MQTTClient(cfg.MQTTClientNameSub) + print(cfg) + # client = utils.MQTTClient(cfg.MQTTClientNameSub) # set log log_format = "%(asctime)s - %(levelname)s - %(message)s" diff --git a/class_model/src/utils/mqttConfig_PUB2.yml b/class_model/src/utils/mqttConfig_PUB2.yml index c2f0549..6284daf 100644 --- a/class_model/src/utils/mqttConfig_PUB2.yml +++ b/class_model/src/utils/mqttConfig_PUB2.yml @@ -19,5 +19,5 @@ ROS: ROStopicName_Flight_Information: Dron550_Flight_Information_reciver ROStopicName_Fly_Formation: Fly_Formation_reciver LOG: - logFileName: pub.log + logFileName: GPSDrone2.log UAVLINK: "None" \ No newline at end of file diff --git a/class_model/src/utils/mqttConfig_PUB3.yml b/class_model/src/utils/mqttConfig_PUB3.yml index 2bb27fe..5c5531d 100644 --- a/class_model/src/utils/mqttConfig_PUB3.yml +++ b/class_model/src/utils/mqttConfig_PUB3.yml @@ -19,5 +19,5 @@ ROS: ROStopicName_Flight_Information: Dron380_Flight_Information_reciver ROStopicName_Fly_Formation: Fly_Formation_reciver LOG: - logFileName: pub.log + logFileName: GPSDrone3.log UAVLINK: "None" \ No newline at end of file diff --git a/class_model/src/utils/mqttConfig_PUB4.yml b/class_model/src/utils/mqttConfig_PUB4.yml index 7fe4f50..47bdd4b 100644 --- a/class_model/src/utils/mqttConfig_PUB4.yml +++ b/class_model/src/utils/mqttConfig_PUB4.yml @@ -19,5 +19,5 @@ ROS: ROStopicName_Flight_Information: Drone888_Flight_Information_reciver ROStopicName_Fly_Formation: Fly_Formation_reciver LOG: - logFileName: pub.log + logFileName: GPSDrone4.log UAVLINK: "None" \ No newline at end of file diff --git a/class_model/src/utils/mqttConfig_PUB5.yml b/class_model/src/utils/mqttConfig_PUB5.yml index a2467a3..6142d75 100644 --- a/class_model/src/utils/mqttConfig_PUB5.yml +++ b/class_model/src/utils/mqttConfig_PUB5.yml @@ -19,5 +19,5 @@ ROS: ROStopicName_Flight_Information: Drone555_Flight_Information_reciver ROStopicName_Fly_Formation: Fly_Formation_reciver LOG: - logFileName: pub.log + logFileName: GPSDrone5.log UAVLINK: "None" \ No newline at end of file diff --git a/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py b/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py index 73012ca..e0b69ec 100644 --- a/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py +++ b/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py @@ -2,7 +2,7 @@ import orjson import time import logging -logger = logging.getLogger(__name__) +logger = logging.getLogger("__mqttPubMain__") class Proto_msg_from_ros: #Protobuf @@ -113,4 +113,5 @@ class Json_msg_from_ros: @classmethod def log(cls): unit = 1e7 + # print(cls.GPS_Data["lat"]) logger.info("{},{},{},{}".format(cls.GPS_Data["lat"]/1e7, cls.GPS_Data["lon"]/1e7, cls.GPS_Data["alt"]/100, cls.GPS_Data["heading"]/100)) \ No newline at end of file diff --git a/class_model/src/utils/readConfig.py b/class_model/src/utils/readConfig.py index 936ee20..6997034 100644 --- a/class_model/src/utils/readConfig.py +++ b/class_model/src/utils/readConfig.py @@ -29,7 +29,8 @@ class Config: if self.options[opts][opt][1]: raise Exception("Missing mandatory parameter {}".format(opt)) else: - setattr(self,opt,None) + pass + # setattr(self,opt,None) def __str__(self): return str(yaml.dump(self.ymlcfg, default_flow_style=False)) @@ -111,7 +112,11 @@ class Read_SUB_Config(Config): "uav_id": (str,False), "baudrate": (int,False), "ttyport": (str,False)}} + self.setAttribute() + def __str__(self): + return super().__str__() + class Read_CMD_Config(Config): def setAttribute(self): super().setAttribute() @@ -141,7 +146,7 @@ class Read_CMD_Config(Config): return super().__str__() if __name__ == "__main__": - cfg=Read_CMD_Config("uavlinkConfig_SUB.yml") - print(cfg) + cfg=Read_SUB_Config("mqttConfig_SUB.yml") + print(cfg.msg_format)