test bio drones

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;

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);
+    void follower(global_location** member_data, size_t index, int refID);
-    int find_ref_drone(global_location* data[], size_t index, int leaderID);
+    int find_ref_drone(global_location** data, size_t index, int leaderID);
 
     float lon[3],lat[3];
     

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

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

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

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

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

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;   
 

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

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

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

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),
+                            "msg_format": (str,False),
-                            "MQTTClientNameSub": (str,True),
+                            "MQTTClientNameSub": (str,False),
-                            "host": (str,True),
+                            "host": (str,False),
-                            "port": (int,True),
+                            "port": (int,False),
-                            "keepalive": (int,True),
+                            "keepalive": (int,False),
-                            "willTopic":(str,True),
+                            "willTopic":(str,False),
-                            "lwt":(str, True),
+                            "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),

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
+  Dron550_ROStopicName_Flight_Information: Flight_Information_reciver
-  ROStopicName_Fly_Formation: Fly_Formation_reciver
 LOG:
   logFileName: UAVLINKsub.log