add calculate data timer test

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)

class_model/include/class_model/command.h

@@ -1,11 +1,16 @@
 /*Drone Control Command*/
 #ifndef COMMAND_H
 #define COMMAND_H
-
+#define DEBUG
 #include <ros/ros.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <mavros_msgs/GlobalPositionTarget.h>
 #include <class_model/sensor.h>
+#include <class_model/Param.h>
+#include <fstream>
+#include <iostream>
+#include <string>        
+
 // #include <geographic_msgs/GeoPoseStamped.h>
 
 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);

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;

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 <ros/ros.h>
 #include "class_model/sensor.h"
 #include "class_model/mode.h"

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);
 }

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<float>()*1e7 ;
+            target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
             leader_object.leader(target);
-            // for(int i=0;i<=360;i++){
-            //     target.lon = config["routh1"]["x"].as<float>()*1e7 + 300*sin(i*0.2);
-            //     target.lat = config["routh1"]["y"].as<float>()*1e7 + 300*cos(i*0.2);
-            //     leader_object.leader(target);
-            //     std::cout<< "radin: " << i*3.14/180 <<std::endl;
-            // }
-            target.lon = config["routh1"]["x"].as<float>()*1e7 + 500;
-            target.lat = config["routh1"]["y"].as<float>()*1e7 ;
+
+            target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
+            target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
             leader_object.leader(target);
-            target.lon = config["routh1"]["x"].as<float>()*1e7 ;
-            target.lat = config["routh1"]["y"].as<float>()*1e7 -500;
+
+            target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
+            target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
             leader_object.leader(target);
-            target.lon = config["routh1"]["x"].as<float>()*1e7 -500;
-            target.lat = config["routh1"]["y"].as<float>()*1e7 ; 
+
+            target.lon = config["routh1"]["x"].as<float>()*1e7 ;
+            target.lat = config["routh1"]["y"].as<float>()*1e7 - 500; 
             leader_object.leader(target);
 
         }else{

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"){

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"){

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"){

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"){

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::nanoseconds>(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::nanoseconds>(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 <<" "<<pub_t<< "\n";
+  			file.close();
+			flag = 0;
+		}
+		double now = ros::Time::now().toSec();
+		if((now-begin)>second){
+			velocity_init();
+			break;
+	}
+
+
+    // uint64_t last_ms = std::chrono::duration_cast<std::chrono::milliseconds>(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::milliseconds>(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 ){
 

class_model/src/drone2.cpp

@@ -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;
-}

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);
 }

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 <<std::endl;
-    }
-    if(counter>=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 <<std::endl;
+    // }
+    // if(counter>=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::nanoseconds>(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::nanoseconds>(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::nanoseconds>(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::nanoseconds>(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...");
 }
 

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"){

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()

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()

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()

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()

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()

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"

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"

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"

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"

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"

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))

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)