From 4b8cc0278f55e29f9d06bf2133c3e4fad0f06e54 Mon Sep 17 00:00:00 2001
From: Xuan0319 <new310049@gmail.com>
Date: Mon, 27 Mar 2023 16:03:37 +0800
Subject: [PATCH] test bio drones

---
 class_model/include/class_model/FindLeader.h  |  2 +-
 class_model/include/class_model/follower.h    |  4 +-
 class_model/src/bio_main.cpp                  | 36 +++++++++++----
 class_model/src/bio_main2.cpp                 | 46 +++++++++++++------
 class_model/src/bio_main3.cpp                 | 46 +++++++++++++------
 class_model/src/bio_main4.cpp                 | 46 +++++++++++++------
 class_model/src/bio_main5.cpp                 | 46 +++++++++++++------
 class_model/src/follower.cpp                  | 14 +++---
 class_model/src/test_lib.cpp                  |  7 ++-
 class_model/src/uavlinkSubMain.py             | 21 +++++++--
 .../utils/proto_uavlink_pub_data_to_ros.py    |  5 +-
 class_model/src/utils/readConfig.py           | 18 ++++----
 class_model/src/utils/uavlinkConfig_SUB.yml   |  5 +-
 13 files changed, 196 insertions(+), 100 deletions(-)
 mode change 100644 => 100755 class_model/src/uavlinkSubMain.py

diff --git a/class_model/include/class_model/FindLeader.h b/class_model/include/class_model/FindLeader.h
index 784e528..4e1239c 100644
--- a/class_model/include/class_model/FindLeader.h
+++ b/class_model/include/class_model/FindLeader.h
@@ -25,7 +25,7 @@ result check_leader(global_location* data, size_t index){
     check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));     //get minimum value's index
     _result.leader_ID = check_ID + 1;
     
-    std::cout <<"Drone " << _result.leader_ID << " is leader " << std::endl;
+    // std::cout <<"Drone " << _result.leader_ID << " is leader " << std::endl;
     
     if (data[1].ID == _result.leader_ID){
         _result.is_leader = 1;
diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h
index 3cf4bb3..755edbd 100755
--- a/class_model/include/class_model/follower.h
+++ b/class_model/include/class_model/follower.h
@@ -36,8 +36,8 @@ public:
     DroneStatus self;
     DroneStatus samePlane[5];
     
-    void follower(global_location* member_data, size_t index, int refID);
-    int find_ref_drone(global_location* data[], size_t index, int leaderID);
+    void follower(global_location** member_data, size_t index, int refID);
+    int find_ref_drone(global_location** data, size_t index, int leaderID);
 
     float lon[3],lat[3];
     
diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp
index 6b92554..6f0da70 100644
--- a/class_model/src/bio_main.cpp
+++ b/class_model/src/bio_main.cpp
@@ -18,11 +18,12 @@ int main(int argc, char **argv) {
     RequestClass request_object;
     FollowerClass follower_object;
     LeaderClass leader_object;
+    ReceiverClass receiver_object;
     
-    // SelectionClass selection_object;
 
     global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader;
+    int ref_ID,checkLeader,flag;
+    std::string type = "",pre_type ="x";
     
     YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
     target.lon = config["routh1"]["x"].as<float>()*1e7;
@@ -35,8 +36,7 @@ int main(int argc, char **argv) {
     M3 = request_object.get_M3_GPS();
     M4 = request_object.get_M4_GPS();
 
-    // global_location pos[] = {target,self,M1,M2};
-    // global_location member[] = {M1,M2};
+
     global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
     global_location* member[] = {&M1,&M2,&M3,&M4};
     size_t index = sizeof(pos)/sizeof(pos[0]);
@@ -45,28 +45,44 @@ int main(int argc, char **argv) {
     mode_object.set_Mode("GUIDED");
     control_object.arm();
     control_object.takeoff(2);
+    // while(flag!=1){
+    //     type = receiver_object.check_command();
+    //     if(type == "go"){
+    //         mode_object.set_Mode("GUIDED");
+    //         control_object.arm();
+    //         control_object.takeoff(2);
+    //         sleep(2);
+    //         flag = 1;
+    //         type ="";
+    //     }   
+    // }
 
     checkLeader = check_leader(*pos,index).is_leader;
     if(checkLeader != 1){
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
 
-    while(true){
+    while(ros::ok()){
 
+        // type = receiver_object.check_command();
         self = request_object.get_self_GPS();
         M1 = request_object.get_M1_GPS();
         M2 = request_object.get_M2_GPS();
         M3 = request_object.get_M3_GPS();
         M4 = request_object.get_M4_GPS();
-        // pos[] = {target,self,M1,M2,M3,M4};
-        // member[] = {M1,M2,M3,M4};
-        
+
         if(checkLeader == 1){
-            // leader_object.leader(target);
+            leader_object.leader(target);
         }else{
             ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
-            follower_object.follower(*member,m_index,ref_ID);   //follow reference drone position
+            follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
         }
+
+        // if(type == "land"){
+        //     mode_object.set_Mode("LAND");
+        // }
+
+        
     }
     ros::waitForShutdown();
 
diff --git a/class_model/src/bio_main2.cpp b/class_model/src/bio_main2.cpp
index 4d3c36a..f39b129 100644
--- a/class_model/src/bio_main2.cpp
+++ b/class_model/src/bio_main2.cpp
@@ -18,11 +18,12 @@ int main(int argc, char **argv) {
     RequestClass request_object;
     FollowerClass follower_object;
     LeaderClass leader_object;
+    ReceiverClass receiver_object;
     
-    // SelectionClass selection_object;
 
     global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader;
+    int ref_ID,checkLeader,flag;
+    std::string type = "",pre_type ="x";
     
     YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
     target.lon = config["routh1"]["x"].as<float>()*1e7;
@@ -35,8 +36,7 @@ int main(int argc, char **argv) {
     M3 = request_object.get_M3_GPS();
     M4 = request_object.get_M4_GPS();
 
-    // global_location pos[] = {target,self,M1,M2};
-    // global_location member[] = {M1,M2};
+
     global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
     global_location* member[] = {&M1,&M2,&M3,&M4};
     size_t index = sizeof(pos)/sizeof(pos[0]);
@@ -45,28 +45,44 @@ int main(int argc, char **argv) {
     mode_object.set_Mode("GUIDED");
     control_object.arm();
     control_object.takeoff(2);
+    // while(flag!=1){
+    //     type = receiver_object.check_command();
+    //     if(type == "go"){
+    //         mode_object.set_Mode("GUIDED");
+    //         control_object.arm();
+    //         control_object.takeoff(2);
+    //         sleep(2);
+    //         flag = 1;
+    //         type ="";
+    //     }   
+    // }
 
     checkLeader = check_leader(*pos,index).is_leader;
     if(checkLeader != 1){
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
 
-    while(true){
+    while(ros::ok()&& flag == 1){
+
+        // type = receiver_object.check_command();
+        self = request_object.get_self_GPS();
+        M1 = request_object.get_M1_GPS();
+        M2 = request_object.get_M2_GPS();
+        M3 = request_object.get_M3_GPS();
+        M4 = request_object.get_M4_GPS();
 
-        // self = request_object.get_self_GPS();
-        // M1 = request_object.get_M1_GPS();
-        // M2 = request_object.get_M2_GPS();
-        // M3 = request_object.get_M3_GPS();
-        // M4 = request_object.get_M4_GPS();
-        // pos[] = {target,self,M1,M2,M3,M4};
-        // member[] = {M1,M2,M3,M4};
-        
         if(checkLeader == 1){
-            // leader_object.leader(target);
+            leader_object.leader(target);
         }else{
             ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
-            follower_object.follower(*member,m_index,ref_ID);   //follow reference drone position
+            follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
         }
+
+        // if(type == "land"){
+        //     mode_object.set_Mode("LAND");
+        // }
+
+        
     }
     ros::waitForShutdown();
 
diff --git a/class_model/src/bio_main3.cpp b/class_model/src/bio_main3.cpp
index ffa56d5..6f5e3cf 100644
--- a/class_model/src/bio_main3.cpp
+++ b/class_model/src/bio_main3.cpp
@@ -18,11 +18,12 @@ int main(int argc, char **argv) {
     RequestClass request_object;
     FollowerClass follower_object;
     LeaderClass leader_object;
+    ReceiverClass receiver_object;
     
-    // SelectionClass selection_object;
 
     global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader;
+    int ref_ID,checkLeader,flag;
+    std::string type = "",pre_type ="x";
     
     YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
     target.lon = config["routh1"]["x"].as<float>()*1e7;
@@ -35,8 +36,7 @@ int main(int argc, char **argv) {
     M3 = request_object.get_M3_GPS();
     M4 = request_object.get_M4_GPS();
 
-    // global_location pos[] = {target,self,M1,M2};
-    // global_location member[] = {M1,M2};
+
     global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
     global_location* member[] = {&M1,&M2,&M3,&M4};
     size_t index = sizeof(pos)/sizeof(pos[0]);
@@ -45,28 +45,44 @@ int main(int argc, char **argv) {
     mode_object.set_Mode("GUIDED");
     control_object.arm();
     control_object.takeoff(2);
+    // while(flag!=1){
+    //     type = receiver_object.check_command();
+    //     if(type == "go"){
+    //         mode_object.set_Mode("GUIDED");
+    //         control_object.arm();
+    //         control_object.takeoff(2);
+    //         sleep(2);
+    //         flag = 1;
+    //         type ="";
+    //     }   
+    // }
 
     checkLeader = check_leader(*pos,index).is_leader;
     if(checkLeader != 1){
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
 
-    while(true){
+    while(ros::ok()&& flag == 1){
+
+        // type = receiver_object.check_command();
+        self = request_object.get_self_GPS();
+        M1 = request_object.get_M1_GPS();
+        M2 = request_object.get_M2_GPS();
+        M3 = request_object.get_M3_GPS();
+        M4 = request_object.get_M4_GPS();
 
-        // self = request_object.get_self_GPS();
-        // M1 = request_object.get_M1_GPS();
-        // M2 = request_object.get_M2_GPS();
-        // M3 = request_object.get_M3_GPS();
-        // M4 = request_object.get_M4_GPS();
-        // pos[] = {target,self,M1,M2,M3,M4};
-        // member[] = {M1,M2,M3,M4};
-        
         if(checkLeader == 1){
-            // leader_object.leader(target);
+            leader_object.leader(target);
         }else{
             ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
-            follower_object.follower(*member,m_index,ref_ID);   //follow reference drone position
+            follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
         }
+
+        // if(type == "land"){
+        //     mode_object.set_Mode("LAND");
+        // }
+
+        
     }
     ros::waitForShutdown();
 
diff --git a/class_model/src/bio_main4.cpp b/class_model/src/bio_main4.cpp
index 0b514e1..c51cfc3 100644
--- a/class_model/src/bio_main4.cpp
+++ b/class_model/src/bio_main4.cpp
@@ -18,11 +18,12 @@ int main(int argc, char **argv) {
     RequestClass request_object;
     FollowerClass follower_object;
     LeaderClass leader_object;
+    ReceiverClass receiver_object;
     
-    // SelectionClass selection_object;
 
     global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader;
+    int ref_ID,checkLeader,flag;
+    std::string type = "",pre_type ="x";
     
     YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
     target.lon = config["routh1"]["x"].as<float>()*1e7;
@@ -35,8 +36,7 @@ int main(int argc, char **argv) {
     M3 = request_object.get_M3_GPS();
     M4 = request_object.get_M4_GPS();
 
-    // global_location pos[] = {target,self,M1,M2};
-    // global_location member[] = {M1,M2};
+
     global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
     global_location* member[] = {&M1,&M2,&M3,&M4};
     size_t index = sizeof(pos)/sizeof(pos[0]);
@@ -45,28 +45,44 @@ int main(int argc, char **argv) {
     mode_object.set_Mode("GUIDED");
     control_object.arm();
     control_object.takeoff(2);
+    // while(flag!=1){
+    //     type = receiver_object.check_command();
+    //     if(type == "go"){
+    //         mode_object.set_Mode("GUIDED");
+    //         control_object.arm();
+    //         control_object.takeoff(2);
+    //         sleep(2);
+    //         flag = 1;
+    //         type ="";
+    //     }   
+    // }
 
     checkLeader = check_leader(*pos,index).is_leader;
     if(checkLeader != 1){
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
 
-    while(true){
+    while(ros::ok()&& flag == 1){
+
+        // type = receiver_object.check_command();
+        self = request_object.get_self_GPS();
+        M1 = request_object.get_M1_GPS();
+        M2 = request_object.get_M2_GPS();
+        M3 = request_object.get_M3_GPS();
+        M4 = request_object.get_M4_GPS();
 
-        // self = request_object.get_self_GPS();
-        // M1 = request_object.get_M1_GPS();
-        // M2 = request_object.get_M2_GPS();
-        // M3 = request_object.get_M3_GPS();
-        // M4 = request_object.get_M4_GPS();
-        // pos[] = {target,self,M1,M2,M3,M4};
-        // member[] = {M1,M2,M3,M4};
-        
         if(checkLeader == 1){
-            // leader_object.leader(target);
+            leader_object.leader(target);
         }else{
             ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
-            follower_object.follower(*member,m_index,ref_ID);   //follow reference drone position
+            follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
         }
+
+        // if(type == "land"){
+        //     mode_object.set_Mode("LAND");
+        // }
+
+        
     }
     ros::waitForShutdown();
 
diff --git a/class_model/src/bio_main5.cpp b/class_model/src/bio_main5.cpp
index 4064955..84d3eb0 100644
--- a/class_model/src/bio_main5.cpp
+++ b/class_model/src/bio_main5.cpp
@@ -18,11 +18,12 @@ int main(int argc, char **argv) {
     RequestClass request_object;
     FollowerClass follower_object;
     LeaderClass leader_object;
+    ReceiverClass receiver_object;
     
-    // SelectionClass selection_object;
 
     global_location target,self,M1,M2,M3,M4;
-    int ref_ID,checkLeader;
+    int ref_ID,checkLeader,flag;
+    std::string type = "",pre_type ="x";
     
     YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
     target.lon = config["routh1"]["x"].as<float>()*1e7;
@@ -35,8 +36,7 @@ int main(int argc, char **argv) {
     M3 = request_object.get_M3_GPS();
     M4 = request_object.get_M4_GPS();
 
-    // global_location pos[] = {target,self,M1,M2};
-    // global_location member[] = {M1,M2};
+
     global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
     global_location* member[] = {&M1,&M2,&M3,&M4};
     size_t index = sizeof(pos)/sizeof(pos[0]);
@@ -45,28 +45,44 @@ int main(int argc, char **argv) {
     mode_object.set_Mode("GUIDED");
     control_object.arm();
     control_object.takeoff(2);
+    // while(flag!=1){
+    //     type = receiver_object.check_command();
+    //     if(type == "go"){
+    //         mode_object.set_Mode("GUIDED");
+    //         control_object.arm();
+    //         control_object.takeoff(2);
+    //         sleep(2);
+    //         flag = 1;
+    //         type ="";
+    //     }   
+    // }
 
     checkLeader = check_leader(*pos,index).is_leader;
     if(checkLeader != 1){
         ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID); 
     }
 
-    while(true){
+    while(ros::ok()&& flag == 1){
+
+        // type = receiver_object.check_command();
+        self = request_object.get_self_GPS();
+        M1 = request_object.get_M1_GPS();
+        M2 = request_object.get_M2_GPS();
+        M3 = request_object.get_M3_GPS();
+        M4 = request_object.get_M4_GPS();
 
-        // self = request_object.get_self_GPS();
-        // M1 = request_object.get_M1_GPS();
-        // M2 = request_object.get_M2_GPS();
-        // M3 = request_object.get_M3_GPS();
-        // M4 = request_object.get_M4_GPS();
-        // pos[] = {target,self,M1,M2,M3,M4};
-        // member[] = {M1,M2,M3,M4};
-        
         if(checkLeader == 1){
-            // leader_object.leader(target);
+            leader_object.leader(target);
         }else{
             ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
-            follower_object.follower(*member,m_index,ref_ID);   //follow reference drone position
+            follower_object.follower(member,m_index,ref_ID);   //follow reference drone position
         }
+
+        // if(type == "land"){
+        //     mode_object.set_Mode("LAND");
+        // }
+
+        
     }
     ros::waitForShutdown();
 
diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp
index 463d96a..4c5740c 100755
--- a/class_model/src/follower.cpp
+++ b/class_model/src/follower.cpp
@@ -12,17 +12,17 @@ FollowerClass::FollowerClass() : node_handle_(""){
 
 FollowerClass::~FollowerClass() { ros::shutdown(); }
 
-void FollowerClass::follower(global_location* member_data, size_t index, int refID){
+void FollowerClass::follower(global_location** member_data, size_t index, int refID){
 
-    if(refID == member_data[0].ID){
+    if(refID == member_data[0]->ID){
         ref = &RequestClass::get_M1_GPS;
-    }else if (refID == member_data[1].ID)
+    }else if (refID == member_data[1]->ID)
     {
         ref = &RequestClass::get_M2_GPS;
-    }else if (refID == member_data[2].ID)
+    }else if (refID == member_data[2]->ID)
     {
         ref = &RequestClass::get_M3_GPS;
-    }else if (refID == member_data[3].ID)
+    }else if (refID == member_data[3]->ID)
     {
         ref = &RequestClass::get_M4_GPS;
     }
@@ -146,7 +146,7 @@ int FollowerClass::direct(global_location* target, global_location* leader, Dron
 
 }
 
-int FollowerClass::find_ref_drone(global_location* data[], size_t index, int leaderID){
+int FollowerClass::find_ref_drone(global_location** data, size_t index, int leaderID){
 
     for(int i=1 ; i<index ; i++){                //check leader drone
         if(leaderID == data[i]->ID){
@@ -261,7 +261,7 @@ int FollowerClass::find_ref_drone(global_location* data[], size_t index, int lea
     check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));     //get minimum value's index
     ref_ID = samePlane[check_index].ID;
 
-    std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " <<  ref_ID << std::endl;
+    // std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " <<  ref_ID << std::endl;
 
     return ref_ID;   
 
diff --git a/class_model/src/test_lib.cpp b/class_model/src/test_lib.cpp
index 2550c8c..976153b 100644
--- a/class_model/src/test_lib.cpp
+++ b/class_model/src/test_lib.cpp
@@ -259,9 +259,14 @@ int main(int argc,char** argv){
 
     global_location data[]={target,self,M1,M2};
     size_t index = sizeof(data)/sizeof(data[0]);
+    
+    M2.lon = 1;
+    M2.lat = 1;
+    M2.ID = 3;
 
+    std::cout<< data[3].lon<<std::endl;
 
-    find_ref_drone(data,index,check_leader(data,index).leader_ID);
+    // find_ref_drone(data,index,check_leader(data,index).leader_ID);
 
 
 
diff --git a/class_model/src/uavlinkSubMain.py b/class_model/src/uavlinkSubMain.py
old mode 100644
new mode 100755
index fb42535..d129a6f
--- a/class_model/src/uavlinkSubMain.py
+++ b/class_model/src/uavlinkSubMain.py
@@ -22,7 +22,7 @@ from geometry_msgs.msg import Vector3
 
 def init_dataFormat(cfg:Read_SUB_Config):
     ros_namespace="/drone1"
-    f1data = b'\xf1drone550..............\r\x1a'  
+    f1data = b'\xf1drone550..............\r\x1a'
     
     sel.write(f1data)
     readTenByte = sel.read_until(size=5)
@@ -30,7 +30,7 @@ def init_dataFormat(cfg:Read_SUB_Config):
 
     Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
     Proto_msg_to_ros.rate = rospy.Rate(10)
-    Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
+    Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,String,queue_size=10)
 
     
 if __name__ == '__main__':
@@ -54,16 +54,29 @@ if __name__ == '__main__':
     logger.setLevel(logging.INFO)
     logger.addHandler(file_handler)
     logger.addHandler(stream_handler)
-    logger.info(cfg)    
+    logger.info(cfg)
+    
+
+    rospy.init_node(cfg.ROSClientNameSub)  
+    init_dataFormat(cfg)
 
+    flight_information_msg = flight_information_pb2.flight_information_message()
+    flight_information_msg.gps.LAT = 34123.1231515
+    flight_information_msg.gps.LON = 23423.1231515
+    flight_information_msg.gps.ALT = 12123.1231515
+    flight_information_msg.heading = 155.12215
+    proto = flight_information_msg.SerializeToString()
+    readTenByte = b'\xf1' + proto + b'\r\x1a'
     while True:
         try:
             # test json
-            readTenByte = sel.read_until(expected= b'\x01\x01', size=25)
+            # readTenByte = sel.read_until(expected= b'\x01\x01', size=25)
 
+            Proto_msg_to_ros.on_message_Flight_Information(readTenByte)
 
         except KeyboardInterrupt as e:
             print("End of program")
+            sys.exit()
 
 
 
diff --git a/class_model/src/utils/proto_uavlink_pub_data_to_ros.py b/class_model/src/utils/proto_uavlink_pub_data_to_ros.py
index d943acd..626dbc3 100644
--- a/class_model/src/utils/proto_uavlink_pub_data_to_ros.py
+++ b/class_model/src/utils/proto_uavlink_pub_data_to_ros.py
@@ -24,9 +24,8 @@ class Proto_msg_to_ros:
         proto_msg = cls.flight_information_msg.FromString(proto)
         protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True)
         logger.info(protoTOJson_msg)
+        print(protoTOJson_msg)
         cls.publisher_Flight_Information.publish(protoTOJson_msg)
         cls.rate.sleep()
 
-        
-
-
+        
\ No newline at end of file
diff --git a/class_model/src/utils/readConfig.py b/class_model/src/utils/readConfig.py
index f42a9a8..54386da 100644
--- a/class_model/src/utils/readConfig.py
+++ b/class_model/src/utils/readConfig.py
@@ -79,15 +79,15 @@ class Read_SUB_Config(Config):
         super().__init__(inFileName)  
         self.options = {
             self.sectionNames[0]:{
-                            "msg_format": (str,True),
-                            "MQTTClientNameSub": (str,True),
-                            "host": (str,True),
-                            "port": (int,True),
-                            "keepalive": (int,True),
-                            "willTopic":(str,True),
-                            "lwt":(str, True),
+                            "msg_format": (str,False),
+                            "MQTTClientNameSub": (str,False),
+                            "host": (str,False),
+                            "port": (int,False),
+                            "keepalive": (int,False),
+                            "willTopic":(str,False),
+                            "lwt":(str, False),
                             "willRetain":(bool,False),
-                            "willTopicQOS":(int,True),
+                            "willTopicQOS":(int,False),
                             "Drone550_Flight_Information_topicToMqtt": (str,False),
                             "Drone380_Flight_Information_topicToMqtt":(str,False),
                             "Drone650_Flight_Information_topicToMqtt":(str,False),
@@ -96,7 +96,7 @@ class Read_SUB_Config(Config):
                             "Fly_Formation_topicToMqtt": (str,False),
                             "Fly_Formation_topicToMqtt_QOS":(int,False)},
             self.sectionNames[1]:{
-                            "ROSClientNameSub": (str,False),
+                            "ROSClientNameSub": (str,True),
                             "Dron550_ROStopicName_Flight_Information": (str,False),
                             "Dron380_ROStopicName_Flight_Information": (str,False),
                             "Dron380_ROStopicName_Flight_Information": (str,False),
diff --git a/class_model/src/utils/uavlinkConfig_SUB.yml b/class_model/src/utils/uavlinkConfig_SUB.yml
index e5bbf0f..5cb21d7 100644
--- a/class_model/src/utils/uavlinkConfig_SUB.yml
+++ b/class_model/src/utils/uavlinkConfig_SUB.yml
@@ -2,11 +2,10 @@ UAVLINK:
   uavlink_msg_format: Proto
   uav_id: \x01\x01
   baudrate: 250000
-  ttyport: /dev/ttyUSB2
+  ttyport: /dev/ttyUSB0
 MQTT: "None"
 ROS:
   ROSClientNameSub: Drone550UAVLINKSub
-  ROStopicName_Flight_Information: Flight_Information_reciver
-  ROStopicName_Fly_Formation: Fly_Formation_reciver
+  Dron550_ROStopicName_Flight_Information: Flight_Information_reciver
 LOG:
   logFileName: UAVLINKsub.log