first commit

class_model/src/PID.cpp

@@ -0,0 +1,70 @@
+#include "class_model/PID.h"
+
+// PIDClass::PIDClass() : node_handle_(""){
+    
+
+// }
+
+// PIDClass::~PIDClass() { ros::shutdown(); }
+
+float PIDClass::update(float current ,float target ,float pidvals[3]){
+
+    current_time = std::clock();
+    // pTime = current_time -
+    double t = (current_time - pTime);
+    float error = target - current;
+
+    double P,D,result;
+    P = pidvals[0] * error;
+    I = I + (pidvals[1] * error *t);
+    D = (pidvals[2] * (error - pError))/t;
+
+    result = P + I + D;
+
+    if (limit !=NULL){
+        if (result > limit[0]){
+            result = limit[0];
+        }
+        if (result < limit[1]){
+            result = limit[1];
+        }
+    }
+
+    pError = error;
+    pTime = current_time; 
+
+    // ROS_INFO("%f,%f,%f",P,I,D);
+
+    return result;
+}
+
+float PIDClass::update1(float current ,float target){
+
+    current_time = std::clock();
+    // pTime = current_time -
+    double t = (current_time - pTime);
+    float error = target - current;
+
+    double P,D,result;
+    P = pidval1[0] * error;
+    I = I + (pidval1[1] * error *t);
+    D = (pidval1[2] * (error - pError))/t;
+
+    result = P + I + D;
+
+    if (limit !=NULL){
+        if (result > limit[0]){
+            result = limit[0];
+        }
+        if (result < limit[1]){
+            result = limit[1];
+        }
+    }
+
+    pError = error;
+    pTime = current_time; 
+
+    // ROS_INFO("%f,%f,%f",P,I,D);
+
+    return result;
+}

class_model/src/PID.py

@@ -0,0 +1,49 @@
+import cvzone
+import cv2
+import numpy as np
+import time
+
+
+class PID:
+    def __init__(self, pidVals, targetVal, axis=0, limit=None):
+        self.pidVals = pidVals
+        self.targetVal = targetVal
+        self.axis = axis
+        self.pError = 0
+        self.limit = limit
+        self.I = 0
+        self.pTime = 0
+
+    def update(self, cVal):
+        # Current Value - Target Value
+        t = time.time() - self.pTime
+        error = cVal - self.targetVal
+        P = self.pidVals[0] * error
+        self.I = self.I + (self.pidVals[1] * error * t)
+        D = (self.pidVals[2] * (error - self.pError)) / t
+
+        result = P + self.I + D
+
+        if self.limit is not None:
+            result = float(np.clip(result, self.limit[0], self.limit[1]))
+        self.pError = error
+        self.ptime = time.time()
+
+        return result
+
+
+
+def main():
+    # For a 640x480 image center target is 320 and 240
+    xPID = PID([1, 0.000000000001, 1], 640 // 2)
+    yPID = PID([1, 0.000000000001, 1], 480 // 2, axis=1, limit=[-100, 100])
+
+    while True:
+
+        xVal = int(xPID.update(cx))
+        yVal = int(yPID.update(cy))
+
+
+
+if __name__ == "__main__":
+    main()

class_model/src/command.cpp

@@ -0,0 +1,184 @@
+#include "class_model/command.h"
+
+CommandClass::CommandClass() : node_handle_("~"){
+
+	std::string ros_namespace;
+	if (!node_handle_.hasParam("namespace"))
+	{
+	}else{
+		node_handle_.getParam("namespace", ros_namespace);
+	}
+
+    velocity_command=node_handle_.advertise<geometry_msgs::TwistStamped>(ros_namespace+"/mavros/setpoint_velocity/cmd_vel",100);
+    gps_command=node_handle_.advertise<mavros_msgs::GlobalPositionTarget>(ros_namespace+"/mavros/setpoint_raw/global",10);
+}
+
+CommandClass::~CommandClass() { ros::shutdown(); }
+
+void CommandClass::velocity_init(){
+
+    msg.twist.linear.x = 0;
+	msg.twist.angular.x = 0;
+	msg.twist.linear.y = 0;
+	msg.twist.angular.y = 0;
+	msg.twist.linear.z = 0;
+	msg.twist.angular.z = 0;
+
+    velocity_command.publish(msg);
+
+    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)>10){
+			break;
+	}
+	}
+
+}
+
+void CommandClass::fix_velocity(float x,float y,float alt,float yaw,float second){
+
+    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);
+	pre_location=gps_object.gps_position();
+	ROS_INFO("%f,%f",pre_location.lat,pre_location.lon);
+
+    uint64_t last_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+	int flag = 0;
+	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();
+		flag=1;
+		ROS_INFO("fin");
+		break;
+	}
+	}
+
+	while(flag==1){
+		sleep(1);
+		if(fix_position(pre_location,x,y,second)!=0){
+			ROS_INFO("position_fixed");
+			break;
+		}
+
+	}
+
+}
+
+void CommandClass::set_global_position(float lat,float lon,float alt){
+    
+    mavros_msgs::GlobalPositionTarget goal_position;
+	goal_position.coordinate_frame = goal_position.FRAME_GLOBAL_TERRAIN_ALT;
+	goal_position.type_mask = goal_position.IGNORE_VX | goal_position.IGNORE_VY | goal_position.IGNORE_VZ | goal_position.IGNORE_AFX | goal_position.IGNORE_AFY | goal_position.IGNORE_AFZ | goal_position.IGNORE_YAW | goal_position.IGNORE_YAW_RATE;
+    goal_position.latitude = lat;
+    goal_position.longitude = lon;
+    goal_position.altitude = alt;
+	gps_command.publish(goal_position);
+
+}
+
+int CommandClass::fix_position(global_location pre_location,float x,float y,float s){
+
+	current_location=gps_object.gps_position();
+	float offset_lon=((pre_location.lon-100)*1e+5)+(x*s)-((current_location.lon-100)*1e+5);
+	float offset_lat=(pre_location.lat*1e+5)+(y*s)-(current_location.lat*1e+5);
+	// ROS_INFO("pre(%f,%f)",(pre_location.lon-100)*1e+5,(pre_location.lat*1e+5));
+	// ROS_INFO("cur(%f,%f)",((current_location.lon-100)*1e+5),(current_location.lat*1e+5));
+	ROS_INFO("of_lon:%f,of_lat:%f",offset_lon,offset_lat);
+	if(offset_lon>1 || offset_lat>1 || offset_lon<-1 || offset_lat<-1){
+		fix_velocity(offset_lon/10,offset_lat/10,0,0,1);
+		// sleep(1);
+		return 0;
+	}
+	else{
+		return 1;
+	}
+}
+void CommandClass::set_velocity(float x,float y,float alt,float yaw,float second){
+
+    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();
+
+    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;
+	}
+	}
+}
+
+void CommandClass::set_target_position(float x ,float y ){
+
+	origin_location = gps_object.gps_position();
+	target_location.lon = (origin_location.lon*1e+5)+x;
+	target_location.lat = (origin_location.lat*1e+5)+y;
+	float PID[]={0.25,0.1,0};
+
+	while(true){
+
+		current_location = gps_object.gps_position();
+		errorX = (current_location.lon-origin_location.lon)*1e+5;
+		errorY = (current_location.lat-origin_location.lat)*1e+5;
+
+		lon_speed = clip(PID[0]*x + PID[1]*(x-errorX),1.5,-1.5);
+		lat_speed = clip(PID[0]*y + PID[1]*(y-errorY),1.5,-1.5);
+		ROS_INFO("%f,%f",lon_speed,lat_speed);
+
+
+		if(sqrt(pow(errorX,2)+pow(errorY,2)) < (sqrt(x*x+y*y)-0.5)){
+			fix_velocity(lon_speed,lat_speed,0,0,0.2);
+		}
+		else if(fix_position(origin_location,x,y,1) != 0){
+			break;
+		}
+	}
+
+}
+
+float CommandClass::clip(float speed,float max_speed,float min_speed){
+
+	if (speed > 0){
+		if(speed > max_speed){
+			speed = max_speed;
+		}
+		// if(speed < 0.5){
+		// 	speed = 0.5;
+		// }
+		return speed;
+	}
+	else if(speed < 0){
+		if(speed < min_speed){
+			speed = min_speed;
+		}
+		// if(speed > -0.5){
+		// 	speed = -0.5;
+		// }
+		return speed;
+	}
+	else{
+		return speed;
+	}
+
+}

class_model/src/command.py

@@ -0,0 +1,77 @@
+#! /usr/bin/env python3
+#coding:utf-8
+
+import ssl
+import rospy
+from std_msgs.msg import String
+import paho.mqtt.client as mqtt
+import json
+
+
+ # Ros
+def ros_pub(dataJson):
+    global publisher, rate ,publisher1 ,publisher2,publisher3,publisher4,publisher5
+    # data = json.loads(dataJson)
+    publisher.publish(dataJson)     #將date字串發布到topic
+    publisher1.publish(dataJson)
+    publisher2.publish(dataJson)
+    publisher3.publish(dataJson)
+    publisher4.publish(dataJson)
+    publisher5.publish(dataJson)
+
+    rate.sleep()
+    # print(f"publish data {data}")
+
+# MQTT
+def on_connect(self, userdata, flags, rc):
+    print("Connected with result code " + str(rc))
+    client.subscribe("cmd/broadcast")
+
+
+def on_message(self, userdata, msg):
+    #msg = msg.payload.decode('utf-8')
+    print(f"msg.topic {msg}")
+    ros_pub(msg.payload.decode('utf-8'))
+
+
+def initialise_clients(clientName):
+    # callback assignment
+    initialise_client = mqtt.Client(clientName, False)
+    initialise_client.topic_ack = []
+    return initialise_client
+
+
+if __name__ == '__main__':
+    # Mqtt
+    mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "cmd/broadcast"}
+    client = initialise_clients("receiver")
+    client.on_connect = on_connect
+    client.on_message = on_message
+    client.connect(mqtt_config["host"], mqtt_config["port"], 60)
+
+    # Ros
+    Mqtt_Node = 'publisher_py'
+    rospy.init_node("cmd_receiver")
+    # initialize Ros node
+    topicName = 'cmd_receiver'
+    publisher = rospy.Publisher(topicName,String,queue_size=10)
+
+    topicName1 = '/drone1/cmd_receiver'
+    publisher1 = rospy.Publisher(topicName1,String,queue_size=10)
+
+    topicName2 = '/drone2/cmd_receiver'
+    publisher2 = rospy.Publisher(topicName2,String,queue_size=10)
+
+    topicName3 = '/drone3/cmd_receiver'
+    publisher3 = rospy.Publisher(topicName3,String,queue_size=10)
+
+    topicName4 = '/drone4/cmd_receiver'
+    publisher4 = rospy.Publisher(topicName4,String,queue_size=10)
+
+    topicName5 = '/drone5/cmd_receiver'
+    publisher5 = rospy.Publisher(topicName5,String,queue_size=10)
+
+
+    rate = rospy.Rate(10)
+
+    client.loop_forever()

class_model/src/control.cpp

@@ -0,0 +1,60 @@
+#include"class_model/control.h"
+
+ControlClass::ControlClass() : node_handle_("~"){
+	
+	std::string ros_namespace;
+	if (!node_handle_.hasParam("namespace"))
+	{
+	}else{
+		node_handle_.getParam("namespace", ros_namespace);
+	}
+    arming_client=node_handle_.serviceClient<mavros_msgs::CommandBool>(ros_namespace+"/mavros/cmd/arming");
+    takeoff_client=node_handle_.serviceClient<mavros_msgs::CommandTOL>(ros_namespace+"/mavros/cmd/takeoff");
+    state_sub = node_handle_.subscribe<mavros_msgs::State>(ros_namespace+"/mavros/state", 10, &ControlClass::state_cb,this);
+}
+
+ControlClass::~ControlClass() { ros::shutdown(); }
+
+
+void ControlClass::state_cb(const mavros_msgs::State::ConstPtr& msg){
+
+    current_state_g = *msg;
+    
+}
+
+int ControlClass::arm() {
+	
+	ROS_INFO("Arming drone");
+	mavros_msgs::CommandBool arm_request;
+	arm_request.request.value = true;
+	while (!current_state_g.armed && !arm_request.response.success && ros::ok())
+	{
+		ros::Duration(.1).sleep();
+		arming_client.call(arm_request);
+	}
+	if(arm_request.response.success)
+	{
+		ROS_INFO("Arming Successful");	
+		return 0;
+	}else{
+		ROS_INFO("Arming failed with %d", arm_request.response.success);
+		return -1;	
+	}
+
+}
+
+int ControlClass::takeoff(float takeoff_alt) {
+	
+	mavros_msgs::CommandTOL srv_takeoff;
+	srv_takeoff.request.altitude = takeoff_alt;
+	if(takeoff_client.call(srv_takeoff)){
+		sleep(3);
+		ROS_INFO("takeoff sent %d", srv_takeoff.response.success);
+	}else{
+		ROS_ERROR("Failed Takeoff");
+		return -2;
+	}
+	sleep(2);
+	return 0;
+
+}

class_model/src/drone2.cpp

@@ -0,0 +1,19 @@
+#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

@@ -0,0 +1,64 @@
+#include "class_model/follower.h"
+
+FollowerClass::FollowerClass() : node_handle_(""){
+    
+
+}
+
+FollowerClass::~FollowerClass() { ros::shutdown(); }
+
+void FollowerClass::follower(){
+
+    mode_object.set_Mode("GUIDED");
+    control_object.arm();
+    control_object.takeoff(1.5);
+    sleep(5);
+    while(true){
+        calculate_position(4,30);
+    }
+
+    sleep(2);
+
+    mode_object.set_Mode("LAND");
+
+}
+
+void FollowerClass::calculate_position(float k,float theta){
+
+    theta = theta*3.14/180;
+    float phi = request_object.get_leader_heading()/100;
+    phi = phi*3.14/180;
+    leader_location=request_object.get_leader_GPS();
+    follower_location=GPS_object.gps_position();
+
+    double Pf[]={},Pl[]={leader_location.lon,leader_location.lat};
+    float transfer[2][2]={
+                            cos(phi),-sin(phi),
+                            sin(phi),cos(phi)
+                        };
+    float Q[2]={k*sin(theta),k*cos(theta)};
+    float T[2]={1,-1};
+
+    Pf[0]=(transfer[0][0]*Q[0]+transfer[0][1]*Q[1])*T[0]+Pl[0]/100;
+    Pf[1]=(transfer[1][0]*Q[0]+transfer[1][1]*Q[1])*T[1]+Pl[1]/100;
+    
+    float error_x = Pf[0] - (follower_location.lon*1e+5);
+    float error_y = Pf[1] - (follower_location.lat*1e+5);
+
+    if (error_x < 0.3 & error_x > -0.3){
+        error_x = 0;
+    }
+    if (error_y < 0.3 & error_y > -0.3){
+        error_y = 0;
+    }
+    
+    // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
+    ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
+    ROS_INFO("%f,%f",Pf[0],Pf[1]);
+    ROS_INFO("%f,%f",error_x,error_y);
+    ROS_INFO("************************************");
+    
+    command_object.fix_velocity(error_x,error_y,0,0,0.1);
+    // sleep(0.5);
+
+}

class_model/src/formation.cpp

@@ -0,0 +1,604 @@
+#include "class_model/formation.h"
+#include "class_model/convert_degree.h"
+
+FormationClass::FormationClass() : node_handle_(""){
+  	
+    std::string ros_namespace;
+	if (!node_handle_.hasParam("namespace"))
+	{
+	}else{
+		node_handle_.getParam("namespace", ros_namespace);
+	}
+
+    next_command=node_handle_.advertise<std_msgs::String>(ros_namespace+"/mavros/next_command",100); 
+
+
+
+}
+
+FormationClass::~FormationClass() { ros::shutdown(); }
+
+void FormationClass::leader(){
+
+    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(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;
+            }
+            
+        }
+
+    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::follower1(int type){
+
+    std::string command,pre_command = "";
+    while(true){
+        
+        if(type == 0){
+            calculate_position(4,30);
+        }else if(type == 1){
+            calculate_position(4,0);
+        }else if(type == 2){
+            calculate_position(4,90);
+        }else if(type == 3){
+            calculate_position(4,120);
+        }else if(type == 4){
+            calculate_position(2.5,120);
+        }else if(type == 5){
+            calculate_position(4,60,60);
+        }
+
+        command = receiver_object.check_command();
+        if(command != pre_command){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+
+    // sleep(2);
+
+    // mode_object.set_Mode("LAND");
+
+}
+
+void FormationClass::follower2(int type){
+
+    std::string command,pre_command = "";
+    while(true){
+        
+        if(type == 0){
+            calculate_position(4,-30);
+        }else if(type == 1){
+            calculate_position(8,0);
+        }else if(type == 2){
+            calculate_position(4,-90);
+        }else if(type == 3){
+            calculate_position(4,-120);
+        }else if(type == 4){
+            calculate_position(2.5,-120);
+        }else if(type == 5){
+            calculate_position(4,-60,-60);
+        }
+
+        command = receiver_object.check_command();
+        if(command != pre_command){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+
+}
+
+void FormationClass::follower3(int type){
+
+    std::string command,pre_command = "";
+    while(true){
+        
+        if(type == 0){
+            calculate_position(8,30);
+        }else if(type == 1){
+            calculate_position(12,0);
+        }else if(type == 2){
+            calculate_position(8,90);
+        }else if(type == 3){
+            calculate_position(6,160);
+        }else if(type == 4){
+            calculate_position(2.5,60);
+        }else if(type == 5){
+            calculate_position(6.93,30,120);
+        }
+        
+
+        command = receiver_object.check_command();
+        if(command != pre_command){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+
+}
+
+void FormationClass::follower4(int type){
+
+    std::string command,pre_command = "";
+    while(true){
+        
+        if(type == 0){
+            calculate_position(8,-30);
+        }else if(type == 1){
+            calculate_position(16,0);
+        }else if(type == 2){
+            calculate_position(8,-90);
+        }else if(type == 3){
+            calculate_position(6,-160);
+        }else if(type == 4){
+            calculate_position(2.5,-60);
+        }else if(type == 5){
+            calculate_position(6.93,-30,-120);
+        }
+
+        command = receiver_object.check_command();
+        if(command != pre_command){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+
+}
+
+void FormationClass::follower5(int type){
+
+    std::string command,pre_command = "";
+    while(true){
+        
+        if(type == 0){
+            calculate_position(12,-30);
+        }else if(type == 1){
+            calculate_position(20,0);
+        }else if(type == 2){
+            calculate_position(6,-90);
+        }else if(type == 3){
+            calculate_position(6,180);
+        }else if(type == 4){
+            calculate_position(3,0);
+        }else if(type == 5){
+            calculate_position(8,0,180);
+        }
+
+        command = receiver_object.check_command();
+        if(command != pre_command){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+
+}
+
+void FormationClass::sph_follower1(int type){
+
+    std::string command,pre_command = "";
+    while(true){
+        
+        if(type == 0){
+            spherical_coordinate(4,30,30);
+        // }else if(type == 1){
+        //     calculate_position(12,0,1);
+        // }else if(type == 2){
+        //     calculate_position(8,90,1);
+        // }else if(type == 3){
+        //     calculate_position(6,160,1);
+        // }else if(type == 4){
+        //     calculate_position(2.5,60,1);
+        }
+        
+
+        command = receiver_object.check_command();
+        if(command != pre_command){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+
+}
+
+void FormationClass::sph_follower2(int type){
+
+    std::string command,pre_command = "";
+    while(true){
+        
+        if(type == 0){
+            spherical_coordinate(4,-30,30);
+        // }else if(type == 1){
+        //     calculate_position(12,0,1);
+        // }else if(type == 2){
+        //     calculate_position(8,90,1);
+        // }else if(type == 3){
+        //     calculate_position(6,160,1);
+        // }else if(type == 4){
+        //     calculate_position(2.5,60,1);
+        }
+        
+
+        command = receiver_object.check_command();
+        if(command != pre_command){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+
+}
+
+void FormationClass::sph_follower3(int type){
+
+    std::string command,pre_command = "";
+    while(true){
+        
+        if(type == 0){
+            spherical_coordinate(4,30,-30);
+        // }else if(type == 1){
+        //     calculate_position(12,0,1);
+        // }else if(type == 2){
+        //     calculate_position(8,90,1);
+        // }else if(type == 3){
+        //     calculate_position(6,160,1);
+        // }else if(type == 4){
+        //     calculate_position(2.5,60,1);
+        }
+        
+
+        command = receiver_object.check_command();
+        if(command != pre_command){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+}
+
+void FormationClass::sph_follower4(int type){
+
+    std::string command,pre_command = "";
+    while(true){
+        
+        if(type == 0){
+            spherical_coordinate(4,-30,-30);
+        // }else if(type == 1){
+        //     calculate_position(12,0,1);
+        // }else if(type == 2){
+        //     calculate_position(8,90,1);
+        // }else if(type == 3){
+        //     calculate_position(6,160,1);
+        // }else if(type == 4){
+        //     calculate_position(2.5,60,1);
+        }
+        
+
+        command = receiver_object.check_command();
+        if(command != pre_command){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+}
+
+
+void FormationClass::leader1(float x,float y){
+
+    std::string command = "",pre_command = "";
+    leader_location=request_object.get_leader_GPS();
+    target_location.lon = leader_location.lon/100 + x;
+    target_location.lat = leader_location.lat/100 + y/1.1;
+    sleep(3);
+    flag = 0;
+    heading_status = 0;
+    while(true){
+        if(flag==0){
+            go2target(x,y);
+        }else{
+            next_command.publish("1");
+            break;
+        }
+
+        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 != pre_command){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+    }
+}
+
+
+void FormationClass::calculate_position(float k,float theta,int direction){
+
+    float deg_phi = request_object.get_leader_heading()/100;
+    float heading = GPS_object.get_heading();
+    
+    float phi = deg_phi*3.14/180;             //degree-->radian
+    theta = theta*3.14/180;
+
+    leader_location=request_object.get_leader_GPS();           //get Leader/Follower 's GPS         
+    follower_location=GPS_object.gps_position();
+
+
+    double Pf[]={},Pl[]={leader_location.lon,leader_location.lat};       //transfer maxtrix  
+    float transfer[2][2]={
+                            cos(phi),-sin(phi),
+                            sin(phi),cos(phi)
+                        };
+    float Q[2]={k*sin(theta),k*cos(theta)};
+    float T[2]={1,-1};
+
+    Pf[0]=(transfer[0][0]*Q[0]+transfer[0][1]*Q[1])*T[0]+Pl[0]/100;
+    Pf[1]=(transfer[1][0]*Q[0]+transfer[1][1]*Q[1])*T[1]+Pl[1]/100;
+    
+    
+    float error_x = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid);       //caculate error
+    float error_y = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid);
+    
+    // float error_x = PID_x.update1(follower_location.lon*1e5 , Pf[0] );       //caculate error
+    // float error_y = PID_y.update1(follower_location.lat*1e5 , Pf[1] );
+
+    // float error_x = Pf[0] - (follower_location.lon*1e+5);            //caculate error
+    // float error_y = Pf[1] - (follower_location.lat*1e+5);
+    
+    float error_degree = deg_phi - heading + direction;
+    float error_yaw = 0.2;   //CCW
+
+    if (error_degree >= 360){
+        error_degree -= 360;
+    }
+    if (error_degree <= -360){
+        error_degree += 360;
+    }
+
+    error_yaw = check_direction(error_degree)*error_yaw;              //check yaw direction
+
+    if (error_x < 0.3 && error_x > -0.3){                     //ignore small errors                  
+        error_x = 0;
+    }
+    if (error_y < 0.3 && error_y > -0.3){
+        error_y = 0;
+    }
+    if (error_degree < 5 && error_degree > -5 ){
+        error_yaw = 0;
+    }else if(error_degree >355 || error_degree < -355){
+        error_yaw = 0;
+    }
+
+    // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
+    // ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
+    // ROS_INFO("%f,%f",Pf[0],Pf[1]);
+    // ROS_INFO("%f,%f",error_x,error_y);
+    // ROS_INFO("deg:%f",deg_phi);
+    // ROS_INFO("heading:%f",heading);
+    // ROS_INFO("error_degree:%f",error_degree);
+    // ROS_INFO("error_yaw:%f",error_yaw);
+    // ROS_INFO("************************************");
+    
+    command_object.set_velocity(error_x,error_y,0,error_yaw,0.1);
+
+}
+
+void FormationClass::spherical_coordinate(float k,float theta,float psi){
+
+    float deg_phi = request_object.get_leader_heading()/100;
+    float heading = GPS_object.get_heading();
+    
+    float phi = deg_phi*3.14/180;             //degree-->radian
+    theta = theta*3.14/180;
+    psi = psi*3.14/180;
+
+    leader_location=request_object.get_leader_GPS();           //get Leader/Follower 's GPS         
+    follower_location=GPS_object.gps_position();  
+
+    cur_alt = leader_location.alt;
+    if (cur_alt == NAN || cur_alt == -NAN){
+        cur_alt = pre_alt;
+    }  
+
+    double Pf[3]={},Pl[]={leader_location.lon,leader_location.lat,cur_alt};       //transfer maxtrix  
+    float transfer[3][3]={
+                            cos(phi),-sin(phi),0,
+                            sin(phi),cos(phi),0,
+                                0   ,   0   ,1,
+                        };
+    float Q[3]={k*cos(psi)*sin(theta),k*cos(psi)*cos(theta),k*sin(psi)};
+    float T[3]={1,-1,1};
+
+    Pf[0]=(transfer[0][0]*Q[0] + transfer[0][1]*Q[1] + transfer[0][2]*Q[2])*T[0] + Pl[0]/100;    //calculate follower coordinate
+    Pf[1]=(transfer[1][0]*Q[0] + transfer[1][1]*Q[1] + transfer[1][2]*Q[2])*T[1] + Pl[1]/100;    //rigid body ---> world
+    Pf[2]=(transfer[2][2]*Q[2])*T[2] + Pl[2]/100;
+
+    if(Pf[2] == NAN || Pf[2] == -NAN){
+        Pf[2] = k*sin(psi) + cur_alt/100;
+    }
+    
+    float error_x = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid);       //caculate error
+    float error_y = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid);       //follower_pid defined in header file
+    float error_alt = Pf[2] - follower_location.alt;
+    
+    // float error_x = PID_x.update1(follower_location.lon*1e5 , Pf[0] );       //caculate error
+    // float error_y = PID_y.update1(follower_location.lat*1e5 , Pf[1] );
+
+    // float error_x = Pf[0] - (follower_location.lon*1e+5);            //caculate error
+    // float error_y = Pf[1] - (follower_location.lat*1e+5);
+    
+    float error_degree = deg_phi - heading;
+    float error_yaw = 0.2;
+    float error_z = (error_alt/std::abs(error_alt))*0.1;
+
+    error_yaw = check_direction(error_degree)*error_yaw;              //check yaw direction
+
+    if (error_x < 0.3 && error_x > -0.3){                     //ignore small errors                  
+        error_x = 0;
+    }
+    if (error_y < 0.3 && error_y > -0.3){
+        error_y = 0;
+    }
+    if (error_degree < 5 && error_degree > -5 ){
+        error_yaw = 0;
+    }else if(error_degree >355 || error_degree < -355){
+        error_yaw = 0;
+    }
+    if(error_alt < 0.3 && error_alt > -0.3){
+        error_z = 0;
+    }
+    
+    pre_alt = leader_location.alt;
+
+    // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
+    // ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
+    // ROS_INFO("%f,%f",Pf[0],Pf[1]);
+    // ROS_INFO("%f,%f",error_x,error_y);
+    // ROS_INFO("deg:%f",deg_phi);
+    // ROS_INFO("heading:%f",heading);
+    // ROS_INFO("error_degree:%f",error_degree);
+    // ROS_INFO("error_yaw:%f",error_yaw);
+    ROS_INFO("Pl[2]:%f ,error_alt:%f,follower_location.alt:%f",Pl[2],error_alt,follower_location.alt);
+    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);
+
+}
+
+void FormationClass::go2target(float x,float y){
+
+    leader_location=request_object.get_leader_GPS();
+    current_location=GPS_object.gps_position(); 
+
+    float target_heading,error_heading;
+    float heading = GPS_object.get_heading();
+    target_heading = ConvertDeg(x,y);   
+
+    // float error_yaw = 0.2;
+    // error_heading =  target_heading - heading;
+    // error_yaw = check_direction(error_heading)*error_yaw;              //check yaw direction
+
+    while(heading_status != 1){
+        face2target(target_heading);
+    }
+
+    float error_x = PID_x.update(current_location.lon*1e5 , target_location.lon ,leader_pid);   //leader_pid defined in header file
+    float error_y = PID_y.update(current_location.lat*1e5 , target_location.lat ,leader_pid);
+
+    // float error_x = PID_x.update1(current_location.lon*1e5 , target_location.lon );
+    // float error_y = PID_y.update1(current_location.lat*1e5 , target_location.lat );
+
+    if (error_x < 0.3 && error_x > -0.3){                     //ignore small errors                  
+        error_x = 0;
+    }
+    if (error_y < 0.3 && error_y > -0.3){
+        error_y = 0;
+    }
+
+    if (error_x == 0 && error_y == 0){
+        flag = 1;
+    }
+
+    command_object.set_velocity(error_x ,error_y ,0,0,0.1);
+
+    // ROS_INFO("slope:%f,%f,%f",slope,x,y);
+    // ROS_INFO("%f,%f",error_x,error_y);
+    // ROS_INFO("heading:%f,target_heading:%f",heading,target_heading);
+    // ROS_INFO("target:%f,%f",target_location.lon ,target_location.lat);
+    // ROS_INFO("current:%f,%f",current_location.lon*1e5 ,current_location.lat*1e5);
+    ROS_INFO("************************************");
+    
+
+
+}
+
+void FormationClass::face2target(float target_heading){
+
+    float error_heading;
+    float heading = GPS_object.get_heading();
+    float error_yaw = 0.2;
+    error_heading =  target_heading - heading;
+    error_yaw = check_direction(error_heading)*error_yaw; 
+
+    if (error_heading < 5 && error_heading > -5 ){
+        error_yaw = 0;
+        heading_status = 1;
+    }
+    command_object.set_velocity(0 ,0 ,0,error_yaw,0.1);
+}
+
+
+

class_model/src/getParam.cpp

@@ -0,0 +1,24 @@
+#include"class_model/getParam.h"
+
+ParamClass::ParamClass() : node_handle_("~"){
+
+
+}
+
+ParamClass::~ParamClass() { ros::shutdown(); }
+
+int ParamClass::getID() {
+	
+    int ParamData;
+	if (!node_handle_.hasParam("droneID"))
+	{
+		ROS_INFO("No Param Named droneID");
+        return 0;
+	}else{
+		node_handle_.getParam("droneID", ParamData);
+		// ROS_INFO("Drone ID %d", ParamData);
+        return ParamData;
+	}
+
+
+}

class_model/src/local_mqtt_pub_data_to_ros.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python3
+#coding:utf-8
+# license removed for brevity
+import ssl
+import rospy
+from std_msgs.msg import String
+import paho.mqtt.client as mqtt
+import json
+
+
+# Ros
+def ros_pub(dataJson):
+    global publisher, rate
+    # data = json.loads(dataJson)
+    publisher.publish(dataJson)     #將date字串發布到topic
+    rate.sleep()
+    # print(f"publish data {data}")
+
+# MQTT
+def initialise_clients(clientname):
+    # callback assignment
+    initialise_client = mqtt.Client(clientname, False) 
+    initialise_client.topic_ack = []
+    return initialise_client
+
+def on_connect(client, userdata, flags, rc):
+    print("Connected with result code " + str(rc))
+    # client.subscribe(mqtt_config["topic"])
+    client.subscribe("data/imu")
+
+
+def on_message(client, userdata, msg):
+    # print(f"got {msg.payload.decode('utf-8')}")
+    # print(f"msg.topic {msg.payload.decode('utf-8')}")
+    ros_pub(msg.payload.decode('utf-8'))
+
+
+if __name__ == '__main__':
+    # Mqtt
+    mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/imu"}
+    client = initialise_clients("123456")
+    client.on_connect = on_connect
+    client.on_message = on_message
+    client.connect(mqtt_config["host"], mqtt_config["port"], 60)
+
+
+    
+    # Ros
+    Mqtt_Node = 'publisher_py'
+    rospy.init_node(Mqtt_Node)
+    # initialize Ros node
+    topicName = 'uav_message'
+    publisher = rospy.Publisher(topicName,String,queue_size=10)
+    rate = rospy.Rate(10)
+
+
+
+    client.loop_forever()
+
+# mqtt connect code list
+# 0: Connection successful
+# 1: Connection refused – incorrect protocol version
+# 2: Connection refused – invalid client identifier
+# 3: Connection refused – server unavailable
+# 4: Connection refused – bad username or password
+# 5: Connection refused – not authorised
+# 6-255: Currently unused.

class_model/src/local_mqtt_sub_data_from_ros.py

@@ -0,0 +1,132 @@
+#!/usr/bin/env python3
+#coding:utf-8
+import sys
+import json
+import paho.mqtt.client as mqtt
+
+
+from traceback import print_tb
+import rospy
+from std_msgs.msg import String
+from std_msgs.msg import Float64
+from std_msgs.msg import Header
+from mavros_msgs.srv import ParamGet
+from sensor_msgs.msg import NavSatFix
+from sensor_msgs.msg import Imu
+from sensor_msgs.msg import Range
+from geometry_msgs.msg import Vector3
+
+mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/imu"}
+data ={}
+# Ros
+def callBack_imu(IMU):
+    gyro_x=str(IMU.linear_acceleration.x)
+    gyro_y=str(IMU.linear_acceleration.y)
+    gyro_z=str(IMU.linear_acceleration.z)
+
+    accel_x=str(IMU.angular_velocity.x)
+    accel_y=str(IMU.angular_velocity.y)
+    accel_z=str(IMU.angular_velocity.z)
+
+    dataImuUpdate = {"gyro_x": gyro_x, "gyro_y": gyro_y,"gyro_z":gyro_z, "accel_x": accel_x, "accel_y": accel_y, "accel_z": accel_z}
+    data.update(dataImuUpdate)
+    # print ('gyro_x:'+gyro_x+'\n'+'gyro_y:'+gyro_y+'\n'+'gyro_z:'+gyro_z +'\n')
+    # print ('accel_x:'+accel_x+'\n'+'accel_y:'+accel_y+'\n'+'accel_z:'+accel_z +'\n')
+
+
+def callBack_gps(GPS):
+    lat=str(int(GPS.latitude*10000000))   #change the scale to centimeters
+    lon=str(int(GPS.longitude*10000000))
+    alt=str(int(GPS.altitude*100))
+    dataGpsUpdate = {"lat": lat, "lon": lon, "alt": alt}
+    data.update(dataGpsUpdate)
+    # dataJsonFormate = json.dumps(data)
+    # mqtt_Pub(message=dataJsonFormate)
+    # print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
+
+
+def callBack_compass_hdg(Compass):
+    heading = str(int(Compass.data*100))
+    dataGpsUpdate = {"heading": heading}
+    data.update(dataGpsUpdate)
+    dataJsonFormate = json.dumps(data)
+    mqtt_Pub(message=dataJsonFormate)
+
+def callBack_state(state):
+    mode = state.mode
+    dataGpsUpdate = {"mode": mode}
+    data.update(dataGpsUpdate)
+    
+def GetParam(self,param_name):
+    param = self.get_param_srv(param_name)
+    if param.success:
+        if param.value.integer != 0:
+            value = param.value.integer
+    
+        else:
+            value = param.value.real
+
+    else:
+        rospy.logwarn("Parameter "+param_name+" not read") 
+        value = 0
+    return value 
+
+
+# MQTT
+def initialise_clients(clientname):
+    # callback assignment
+    initialise_client = mqtt.Client(clientname, False) 
+    initialise_client.topic_ack = []
+    return initialise_client
+
+
+# publish a message
+def mqtt_Pub(message, topics = mqtt_config["topic"], waitForAck=False):
+    mid = client.publish(topics, message, 0)[1]
+    # print(f"just published {message} to topic")
+    if waitForAck:
+        while mid not in client.topic_ack:
+            print("wait for ack")
+            time.sleep(0.25)
+        client.topic_ack.remove(mid)
+
+
+def on_publish(self, userdata, mid):
+    client.topic_ack.append(mid)
+
+def on_connect(self, userdata, flags, rc):
+    print("Connected with result code " + str(rc))
+
+ 
+if __name__ == '__main__':
+    # Mqtt
+    client = initialise_clients("client1")
+    client.on_publish = on_publish
+    client.on_connect = on_connect
+    client.connect(mqtt_config["host"], mqtt_config["port"], 60)
+    client.loop_start()
+
+    # Ros
+    nodeName = 'save_data_py'
+    rospy.init_node(nodeName)
+
+    ros_namespace = ""
+    if  not rospy.has_param("namespace"):
+        print("using default namespace")
+    else:
+        rospy.get_param("namespace", ros_namespace)
+        print("using namespace "+ros_namespace)
+	
+    ros_namespace="/drone1"
+    # topicName = 'data_topic'
+    # subscriber = rospy.Subscriber('/mavros/imu/data_raw',Imu,callBack_imu)  #從topic獲取string再呼叫callback
+    subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/global',NavSatFix,callBack_gps)  #從topic獲取string再呼叫callback
+    subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg',Float64,callBack_compass_hdg)  #從topic獲取string再呼叫callback
+    #subscriber = rospy.Subscriber(ros_namespace+'/mavros/mavros_msgs/State',Header,callBack_state)
+    ID = rospy.get_param(ros_namespace+'/leader/droneID')
+    # subscriber = rospy.Subscriber('/mavros/rangefinder/rangefinder',Range,callBack_rng)  #從topic獲取string再呼叫callback
+
+    # ID = rospy.GetParam("droneID")
+    # print(ID)
+
+    rospy.spin()

class_model/src/main.cpp

@@ -0,0 +1,72 @@
+#include "class_model/mission.h"
+
+int main(int argc, char **argv) {
+
+    // Init ROS node
+    ros::init(argc, argv, "drone1_node");
+
+    // reate Assync spiner
+    ros::AsyncSpinner spinner(0);
+	spinner.start();   
+
+    ThreadGPSClass gps_object;
+    ModeClass mode_object;
+    ControlClass control_object;
+    SelectClass select_formation;
+    ReceiverClass receiver_object;
+    MissionClass mission_object;
+    CommandClass command_object;
+    std::string type = "";
+
+    mode_object.set_Mode("GUIDED");
+    control_object.arm();
+    control_object.takeoff(4.5);
+    sleep(5);
+
+    while(ros::ok()){
+
+    type = receiver_object.check_command();  
+    // // ROS_INFO("%s",type.c_str());
+    
+    if(type == "" || type == "init"){
+        select_formation.goose_formation();
+        ROS_INFO("init formation");
+    }else if(type == "line"){
+        select_formation.line_formation();
+        // ROS_INFO("line foemation");
+    }else if(type == "row"){
+        select_formation.row_formation();
+    }else if(type == "circle"){
+        select_formation.circle_formation();
+    }else if(type == "stop"){
+        select_formation.stop();
+    }else if(type == "land"){
+        mode_object.set_Mode("LAND");
+    }
+    
+
+    //mission_object.cruise(20,20);
+    // select_formation.goose_formation();
+    // }else if (type == "hex"){
+    // select_formation.Hex_formation();
+    // }
+    // select_formation.Hex_formation();
+    sleep(0.5);
+    // command_object.set_velocity(0 ,0 ,0,0,1);
+
+    if(type == "stop"){
+        select_formation.stop();
+    }else if(type == "land"){
+        mode_object.set_Mode("LAND");
+    }
+     }
+
+    
+    // select_formation.square();
+
+    // RequestClass test_object;
+
+    ros::waitForShutdown();
+
+    return 0;
+}

class_model/src/mission.cpp

@@ -0,0 +1,50 @@
+#include "class_model/mission.h"
+
+
+MissionClass::MissionClass() : node_handle_(""){
+    
+
+}
+
+MissionClass::~MissionClass() { ros::shutdown(); }
+
+void MissionClass::fly2target(float x,float y){
+    select_formation.protect_formation(x,y); 
+    sleep(1);
+    select_formation.protect_formation(-x,-y);
+    // select_formation.stop();
+    ROS_INFO("-----------------------");
+}
+
+void MissionClass::cruise(float x, float y){
+    
+    // select_formation.protect_formation(0,y);
+    // sleep(1);
+    // select_formation.protect_formation(-x,0);
+    // sleep(1);
+    // select_formation.protect_formation(0,-y);
+    // sleep(1);
+    // select_formation.protect_formation(x,0);
+    // sleep(1);
+    
+    select_formation.init_formation(0,y);
+    sleep(1);
+    select_formation.init_formation(-x,0);
+    sleep(1);
+    select_formation.init_formation(0,-y);
+    sleep(1);
+    select_formation.init_formation(x,0);
+    sleep(1);
+}
+
+void MissionClass::snake(float x, float y){
+       
+    select_formation.line_formation(0,y);
+    sleep(1);
+    select_formation.line_formation(-x,0);
+    sleep(1);
+    select_formation.line_formation(0,-y);
+    sleep(1);
+    select_formation.line_formation(x,0);
+    sleep(1);
+}

class_model/src/mode.cpp

@@ -0,0 +1,33 @@
+#include"class_model/mode.h"
+
+ModeClass::ModeClass() : node_handle_("~"){
+
+	std::string ros_namespace;
+	if (!node_handle_.hasParam("namespace"))
+	{
+		ROS_INFO("using default namespace");
+	}else{
+		node_handle_.getParam("namespace", ros_namespace);
+		ROS_INFO("using namespace %s", ros_namespace.c_str());
+	}
+  
+  set_mode_client=node_handle_.serviceClient<mavros_msgs::SetMode>(ros_namespace+"/mavros/set_mode");
+
+}
+
+ModeClass::~ModeClass() { ros::shutdown(); }
+
+int ModeClass::set_Mode(std::string mode) {
+	
+	mavros_msgs::SetMode srv_setMode;
+  srv_setMode.request.base_mode = 0;
+  srv_setMode.request.custom_mode = mode.c_str();
+  if(set_mode_client.call(srv_setMode)){
+    // ROS_INFO("setmode %s ok",mode.c_str());
+    return 0;
+    }else{
+      ROS_ERROR("Failed SetMode %s",mode.c_str());
+      return -1;
+    }
+
+}

class_model/src/receiver.cpp

@@ -0,0 +1,26 @@
+#include"class_model/receiver.h"
+
+ReceiverClass::ReceiverClass() : node_handle_("~"){
+	
+    std::string ros_namespace;
+	if (!node_handle_.hasParam("namespace"))
+	{
+	}else{
+		node_handle_.getParam("namespace", ros_namespace);
+	}
+    mqtt_sub = node_handle_.subscribe(ros_namespace+"/cmd_receiver",10,&ReceiverClass::cmd_receiver, this);
+}
+
+ReceiverClass::~ReceiverClass() { ros::shutdown(); }
+
+void ReceiverClass::cmd_receiver(const std_msgs::String::ConstPtr &msg){
+
+    mqtt_command="";
+    mqtt_command.append(msg->data);
+    // std::cout<< "type:" << msg->data << std::endl;
+}
+
+std::string ReceiverClass::check_command(){
+    // ROS_INFO("sd:%s",mqtt_command);
+    return mqtt_command;
+}

class_model/src/requestData.cpp

@@ -0,0 +1,75 @@
+#include"class_model/requestData.h"
+#include <cstdlib>
+#include <iostream>
+
+global_location leader_position; 
+
+RequestClass::RequestClass() : node_handle_(""){
+
+  mqtt_data = node_handle_.subscribe("/uav_message", 10,
+                                   &RequestClass::Data_callback, this);
+
+}
+
+RequestClass::~RequestClass() { ros::shutdown(); }
+
+void RequestClass::Data_callback(const std_msgs::String::ConstPtr &sensor) {
+
+	std::string data = sensor->data;
+  jsonToInt(data);
+
+}
+
+global_location RequestClass::get_leader_GPS(){
+
+  return leader_position; 
+}
+
+float RequestClass::get_leader_heading(){
+  
+  return heading;
+}
+
+void RequestClass::jsonToInt(std::string data){
+
+  std::string lat,lon,degree;
+  std::string list[5]={"","","","",""};
+  int j = 0;
+
+  // lat.append(data,9,10);
+  // lon.append(data,29,10);
+  // degree.append(data,53,6);
+
+  // heading = std::stoi(degree);
+  // leader_position.lat=std::stoi(lat);   
+	// leader_position.lon=std::stoi(lon);
+
+  for(int i=0;i<data.length();i++){
+    if(data[i] == ':'){
+      i+=3;
+      while(data[i] != ','){
+        list[j]=list[j]+data[i];
+        i++; 
+        if(data[i] == ',' || data[i] == '}'){
+            j++;
+            break;
+        }
+        if(j>3){
+            break;
+        }     
+      }
+    }
+  }
+
+  leader_position.lat=std::stoi(list[0]);   
+	leader_position.lon=std::stoi(list[1]);
+  leader_position.alt=std::stoi(list[2]);
+  heading = std::stoi(list[3]);
+
+
+
+  // std::cout<<sizeof(data)<<std::endl;
+  //std::cout<<data<<std::endl;
+  // ROS_INFO("leader_GPS [%f,%f]", leader_position.lat, leader_position.lon);
+  // ROS_INFO("leader_heading: %f",heading);
+}

class_model/src/select.cpp

@@ -0,0 +1,178 @@
+#include "class_model/select.h"
+
+
+SelectClass::SelectClass() : node_handle_(""){
+    
+
+}
+
+SelectClass::~SelectClass() { ros::shutdown(); }
+
+void SelectClass::init_formation(float x ,float y){
+
+    counter = 0;
+    // if(param_object.getID() == 1){
+    //     formation_object.leader();
+    // }else if(param_object.getID() == 2){
+    //     formation_object.follower1(counter);
+    // }else if(param_object.getID() == 3){
+    //     formation_object.follower2(counter);
+    // }else if(param_object.getID() == 4){
+    //     formation_object.follower3(counter);
+    // }else if(param_object.getID() == 5){
+    //     formation_object.follower4(counter);
+    // }
+
+    if(param_object.getID() == 1){
+        formation_object.leader1(x,y);
+    }else if(param_object.getID() == 2){
+        formation_object.sph_follower1(counter);
+    }else if(param_object.getID() == 3){
+        formation_object.sph_follower2(counter);
+    }else if(param_object.getID() == 4){
+        formation_object.sph_follower3(counter);
+    }else if(param_object.getID() == 5){
+        formation_object.sph_follower4(counter);
+    }
+
+
+    // sleep(2);
+
+    // mode_object.set_Mode("LAND");
+
+}
+
+void SelectClass::goose_formation(float x ,float y){
+
+    counter = 0;
+    if(param_object.getID() == 1){
+        formation_object.leader1();
+    }else if(param_object.getID() == 2){
+        formation_object.follower1(counter);
+    }else if(param_object.getID() == 3){
+        formation_object.follower2(counter);
+    }else if(param_object.getID() == 4){
+        formation_object.follower3(counter);
+    }else if(param_object.getID() == 5){
+        formation_object.follower4(counter);
+    }else if(param_object.getID() == 6){
+        formation_object.follower5(counter);
+    }
+
+
+    // sleep(2);
+
+    // mode_object.set_Mode("LAND");
+
+}
+
+void SelectClass::line_formation(float x ,float y){
+    
+    counter = 1;
+    if(param_object.getID() == 1){
+        formation_object.leader1();
+    }else if(param_object.getID() == 2){
+        formation_object.follower1(counter);
+    }else if(param_object.getID() == 3){
+        formation_object.follower2(counter);
+    }else if(param_object.getID() == 4){
+        formation_object.follower3(counter);
+    }else if(param_object.getID() == 5){
+        formation_object.follower4(counter);
+    }else if(param_object.getID() == 6){
+        formation_object.follower5(counter);
+    }
+    // sleep(2);
+
+    // mode_object.set_Mode("LAND");
+
+}
+
+void SelectClass::row_formation(){
+
+    counter = 2;
+    if(param_object.getID() == 1){
+        formation_object.leader1();
+    }else if(param_object.getID() == 2){
+        formation_object.follower1(counter);
+    }else if(param_object.getID() == 3){
+        formation_object.follower2(counter);
+    }else if(param_object.getID() == 4){
+        formation_object.follower3(counter);
+    }else if(param_object.getID() == 5){
+        formation_object.follower4(counter);
+    }else if(param_object.getID() == 6){
+        formation_object.follower5(counter);
+    }
+    // sleep(2);
+
+    // mode_object.set_Mode("LAND");
+
+}
+
+void SelectClass::circle_formation(){
+
+    counter = 3;
+    if(param_object.getID() == 1){
+        formation_object.leader1();
+    }else if(param_object.getID() == 2){
+        formation_object.follower1(counter);
+    }else if(param_object.getID() == 3){
+        formation_object.follower2(counter);
+    }else if(param_object.getID() == 4){
+        formation_object.follower3(counter);
+    }else if(param_object.getID() == 5){
+        formation_object.follower4(counter);
+    }else if(param_object.getID() == 6){
+        formation_object.follower5(counter);
+    }
+    // sleep(2);
+
+    // mode_object.set_Mode("LAND");
+}
+
+void SelectClass::protect_formation(float x ,float y){
+
+    counter = 4;
+    if(param_object.getID() == 1){
+        formation_object.leader1(x,y);
+    }else if(param_object.getID() == 2){
+        formation_object.follower1(counter);
+    }else if(param_object.getID() == 3){
+        formation_object.follower2(counter);
+    }else if(param_object.getID() == 4){
+        formation_object.follower3(counter);
+    }else if(param_object.getID() == 5){
+        formation_object.follower4(counter);
+    }else if(param_object.getID() == 6){
+        formation_object.follower5(counter);
+    }
+    // sleep(2);
+
+    // mode_object.set_Mode("LAND");
+}
+
+void SelectClass::Hex_formation(float x ,float y){
+
+    counter = 5;
+    if(param_object.getID() == 1){
+        formation_object.leader1(x,y);
+    }else if(param_object.getID() == 2){
+        formation_object.follower1(counter);
+    }else if(param_object.getID() == 3){
+        formation_object.follower2(counter);
+    }else if(param_object.getID() == 4){
+        formation_object.follower3(counter);
+    }else if(param_object.getID() == 5){
+        formation_object.follower4(counter);
+    }else if(param_object.getID() == 6){
+        formation_object.follower5(counter);
+    }
+    // sleep(2);
+
+    // mode_object.set_Mode("LAND");
+}
+
+void SelectClass::stop(){
+
+}

class_model/src/sensor.cpp

@@ -0,0 +1,45 @@
+#include <class_model/sensor.h>
+
+global_location position; 
+float heading;
+
+
+ThreadGPSClass::ThreadGPSClass() : node_handle_("~"){
+
+    std::string ros_namespace;
+    if (!node_handle_.hasParam("namespace"))
+	{
+	}else{
+		node_handle_.getParam("namespace", ros_namespace);
+	}
+
+    gps_sub = node_handle_.subscribe(ros_namespace+"/mavros/global_position/global", 10,
+                                   &ThreadGPSClass::GPS_callback, this);
+    compass_sub = node_handle_.subscribe(ros_namespace+"/mavros/global_position/compass_hdg", 10,
+                                   &ThreadGPSClass::Compass_callback, this);
+
+}
+
+ThreadGPSClass::~ThreadGPSClass() { ros::shutdown(); }
+
+void ThreadGPSClass::GPS_callback(const sensor_msgs::NavSatFix::ConstPtr &gps) {
+	
+    position.lat=gps->latitude;   
+	position.lon=gps->longitude;
+    position.alt=gps->altitude;
+    // ROS_INFO("GPS [%f,%f]", position.lat, position.lon);
+}
+
+void ThreadGPSClass::Compass_callback(const std_msgs::Float64::ConstPtr &degree){
+    heading = degree->data;
+}
+
+global_location ThreadGPSClass::gps_position(){
+    // ROS_INFO("GPS [%f,%f]", position.lat, position.lon);
+    return position; 
+}
+
+float ThreadGPSClass::get_heading(){
+
+    return heading; 
+}

class_model/src/tower.py

@@ -0,0 +1,59 @@
+#! /usr/bin/env python3
+#coding:utf-8
+
+import paho.mqtt.client as mqtt
+import time
+
+
+def on_connect(self, userdata, flags, rc):
+    global connect_flag
+    print("Connected with result code " + str(rc))
+    connect_flag = True
+
+
+# def on_message(self, userdata, msg):
+# print(f"Receive UAV_Z550 {msg.payload.decode('utf-8')}")
+# print(f"Receive UAV_H380 {msg.payload.decode('utf-8')}")
+# print("command: ", end="")
+
+
+def initialise_clients(clientName):
+    # callback assignment
+    initialise_client = mqtt.Client(clientName, True)
+    initialise_client.topic_ack = []
+    return initialise_client
+
+
+# publish a message
+def publish(topics, message, waitForAck=False):
+    mid = client.publish(topics, message, 1)[1]
+    print(f"just published {message} to topic")
+    if waitForAck:
+        while mid not in client.topic_ack:
+            print("wait for ack")
+            time.sleep(0.25)
+        client.topic_ack.remove(mid)
+
+
+def on_publish(self, userdata, mid):
+    client.topic_ack.append(mid)
+
+
+connect_flag = False
+mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "cmd/broadcast", "name": "Tower"}
+client = initialise_clients(mqtt_config["name"])
+client.on_publish = on_publish
+client.on_connect = on_connect
+# client.on_message = on_message
+
+client.connect(mqtt_config["host"], mqtt_config["port"], 60)
+client.loop_start()
+
+# publish(topicBroadcast, "Connect", True)
+while True:
+    if connect_flag:
+        break
+        
+while True:
+    command = input("command: ")
+    publish(mqtt_config["topic"], command)