ver3

class_model/src/MQTT_Pub_message.py

@@ -0,0 +1,70 @@
+#!/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/formation")
+
+
+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/formation"}
+     
+    client = initialise_clients("command")
+    client.on_connect = on_connect
+    client.on_message = on_message
+    client.connect(mqtt_config["host"], mqtt_config["port"], 60)
+
+
+    
+    # Ros
+    Mqtt_Node = 'publisher_message_py'
+    rospy.init_node(Mqtt_Node)
+    # initialize Ros node
+    topicName = 'formation_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/MQTT_sub_message.py

@@ -0,0 +1,85 @@
+#!/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/formation"}
+data ={}
+# Ros
+    
+def callBack_message(command):
+
+    dataUpdate = {"message": command.data}
+    data.update(dataUpdate)
+    dataJsonFormate = json.dumps(data)
+    mqtt_Pub(message=dataJsonFormate)
+
+
+# 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 = 'message_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"
+
+    subscriber = rospy.Subscriber("/mavros/next_command",String,callBack_message)
+    #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/formation.cpp

@@ -76,12 +76,13 @@ void FormationClass::leader1(float x,float y){
     sleep(3);
     flag = 0;
     heading_status = 0;
-    message.data = "1";
+    // message.data = "1";
     while(true){
         if(flag==0){
             go2target(x,y);
         }else{
-            next_command.publish(message);
+            // next_command.publish(message);
+            ROS_INFO("break");
             break;
         }
 
@@ -108,7 +109,8 @@ void FormationClass::leader1(float x,float y){
 
 void FormationClass::follower1(int type){
 
-    std::string message,command,pre_command = "";
+    std::string command,pre_command = "";
+    int message;
     while(true){
         
         if(type == 0){
@@ -126,9 +128,10 @@ void FormationClass::follower1(int type){
         }
 
         command = receiver_object.check_command();
-        message = request_object.get_formation_message();
+        // message = request_object.get_formation_message();
+        // ROS_INFO("%d",message);
 
-        if(command != pre_command || message == "1"){
+        if(command != pre_command ){
             // ROS_INFO("change formation");
             break;
         }
@@ -163,9 +166,9 @@ void FormationClass::follower2(int type){
         }
 
         command = receiver_object.check_command();
-        message = request_object.get_formation_message();
+        //message = request_object.get_formation_message();
 
-        if(command != pre_command || message == "1"){
+        if(command != pre_command ){
             // ROS_INFO("change formation");
             break;
         }
@@ -197,9 +200,9 @@ void FormationClass::follower3(int type){
         
 
         command = receiver_object.check_command();
-        message = request_object.get_formation_message();
+        //message = request_object.get_formation_message();
 
-        if(command != pre_command || message == "1"){
+        if(command != pre_command ){
             // ROS_INFO("change formation");
             break;
         }
@@ -230,9 +233,9 @@ void FormationClass::follower4(int type){
         }
 
         command = receiver_object.check_command();
-        message = request_object.get_formation_message();
+        //message = request_object.get_formation_message();
 
-        if(command != pre_command || message == "1"){
+        if(command != pre_command ){
             // ROS_INFO("change formation");
             break;
         }
@@ -262,9 +265,9 @@ void FormationClass::follower5(int type){
         }
 
         command = receiver_object.check_command();
-        message = request_object.get_formation_message();
+        //message = request_object.get_formation_message();
 
-        if(command != pre_command || message == "1"){
+        if(command != pre_command ){
             // ROS_INFO("change formation");
             break;
         }

class_model/src/json.cpp

@@ -0,0 +1,74 @@
+#include <iostream>
+#include <nlohmann/json.hpp>
+#include <std_msgs/String.h>
+#include <string>
+#include <ros/ros.h>
+
+using json = nlohmann::json;
+
+
+void callBack(const std_msgs::String::ConstPtr &data){
+
+    json j_data = json::parse(data->data);
+    std::cout <<"original message"<< data->data << std::endl;
+    std::cout <<"parse message"<< j_data << std::endl;
+    std::cout <<"parse message"<< j_data["lat"] << std::endl;
+
+    std::string k = j_data["lat"] , q = j_data["lon"];
+    int lat = std::stoi(k) ,lon = std::stoi(q);
+    std::cout <<"lat:"<< k << std::endl;
+    std::cout <<"lon:"<< q << std::endl;
+
+}
+
+int main(int argc ,char **argv)
+{
+    // create JSON arrays
+    std::string j,q,k;
+    std_msgs::String mes;
+    std::string data ="{\"lat\": \"241218859\", \"lon\": \"1206743161\", \"alt\": \"8489\", \"heading\": \"128\"}";
+    // std::string data = "{'lat': 241217244, 'lon': '1206741391', 'alt': '8937', 'heading': 8800}";
+    // std::string data ='"{"lat": "241218859", "lon": "1206743161", "alt": "8489", "heading": "128"}"';
+    
+    // Init ROS node
+    ros::init(argc, argv, "ros_node");
+    ros::NodeHandle json_node;
+    // reate Assync spiner
+    ros::AsyncSpinner spinner(0);
+	spinner.start();   
+
+    std::string rostopicName = "jsonData";
+    ros::Publisher data_pub = json_node.advertise<std_msgs::String>(rostopicName,100);
+    ros::Subscriber data_receive = json_node.subscribe<std_msgs::String>(rostopicName,100,callBack); 
+    //ros::Subscriber data_receive1 = json_node.subscribe<std_msgs::String>("/uav_message",100,callBack);
+
+    mes.data = data;
+    while(ros::ok()){
+    data_pub.publish(mes);
+    }
+    // json j_no_init_list = json::array();
+    // json j_empty_init_list = json::array({});
+    // json j_nonempty_init_list = json::array({1, 2, 3, 4});
+    // json j_list_of_pairs = json::array({ {"one", 1}, {"two", 2} });
+    // json j_data_list = json::parse(data) ;
+
+    // j = j_data_list["lat"];
+    // q = j_data_list["lon"];
+    // k = j_data_list["alt"];
+    
+
+    // serialize the JSON arrays
+    // std::cout << j_no_init_list << '\n';
+    // std::cout << j_empty_init_list << '\n';
+    // std::cout << j_nonempty_init_list << '\n';
+    // std::cout << j_list_of_pairs << '\n';
+    // while(true){
+    // std::cout << j_data_list << '\n';
+    // std::cout << j_data_list["lat"] << '\n';
+    // std::cout << std::stoi(j)<< std::stoi(q)<< std::stoi(k) << "\n";
+    // }
+
+    ros::waitForShutdown();
+
+    return 0;
+}

class_model/src/json1.cpp

@@ -0,0 +1,76 @@
+#include <iostream>
+#include <std_msgs/String.h>
+#include <string>
+#include <ros/ros.h>
+#include "rapidjson/document.h"
+
+rapidjson::Document document;
+
+void callBack(const std_msgs::String::ConstPtr &data){
+
+    document.Parse(data->data.c_str());
+    std::cout <<"original message "<< data->data << std::endl;
+    std::cout <<"parse message "<< document["lat"].GetInt() << std::endl;
+    std::cout <<"parse message "<< document["lon"].GetInt() << std::endl;
+    std::cout <<"parse message "<< document["alt"].GetInt() << std::endl;
+    std::cout <<"parse message "<< document["heading"].GetInt() << std::endl;
+    // std::cout <<"parse message"<< j_data["lat"] << std::endl;
+
+    // std::string k = j_data["lat"] , q = j_data["lon"];
+    // int lat = std::stoi(k) ,lon = std::stoi(q);
+    // std::cout <<"lat:"<< k << std::endl;
+    // std::cout <<"lon:"<< q << std::endl;
+
+}
+
+int main(int argc ,char **argv)
+{
+    // create JSON arrays
+    std::string j,q,k;
+    std_msgs::String mes;
+    std::string data ="{\"lat\": \"241218859\", \"lon\": \"1206743161\", \"alt\": \"8489\", \"heading\": \"128\"}";
+    // std::string data = "{'lat': 241217244, 'lon': '1206741391', 'alt': '8937', 'heading': 8800}";
+    // std::string data ='"{"lat": "241218859", "lon": "1206743161", "alt": "8489", "heading": "128"}"';
+    
+    // Init ROS node
+    ros::init(argc, argv, "ros_node");
+    ros::NodeHandle json_node;
+    // reate Assync spiner
+    ros::AsyncSpinner spinner(0);
+	spinner.start();   
+
+    std::string rostopicName = "jsonData";
+    ros::Publisher data_pub = json_node.advertise<std_msgs::String>(rostopicName,100);
+    // ros::Subscriber data_receive = json_node.subscribe<std_msgs::String>(rostopicName,100,callBack); 
+    ros::Subscriber data_receive1 = json_node.subscribe<std_msgs::String>("/uav_message",100,callBack);
+
+    mes.data = data;
+    while(ros::ok()){
+    data_pub.publish(mes);
+    }
+    // json j_no_init_list = json::array();
+    // json j_empty_init_list = json::array({});
+    // json j_nonempty_init_list = json::array({1, 2, 3, 4});
+    // json j_list_of_pairs = json::array({ {"one", 1}, {"two", 2} });
+    // json j_data_list = json::parse(data) ;
+
+    // j = j_data_list["lat"];
+    // q = j_data_list["lon"];
+    // k = j_data_list["alt"];
+    
+
+    // serialize the JSON arrays
+    // std::cout << j_no_init_list << '\n';
+    // std::cout << j_empty_init_list << '\n';
+    // std::cout << j_nonempty_init_list << '\n';
+    // std::cout << j_list_of_pairs << '\n';
+    // while(true){
+    // std::cout << j_data_list << '\n';
+    // std::cout << j_data_list["lat"] << '\n';
+    // std::cout << std::stoi(j)<< std::stoi(q)<< std::stoi(k) << "\n";
+    // }
+
+    ros::waitForShutdown();
+
+    return 0;
+}

class_model/src/local_mqtt_pub_data_to_ros.py

@@ -6,6 +6,7 @@ import rospy
 from std_msgs.msg import String
 import paho.mqtt.client as mqtt
 import json
+# import json_transport 
 
 
 # Ros
@@ -16,12 +17,6 @@ def ros_pub(dataJson):
     rate.sleep()
     # print(f"publish data {data}")
 
-def PubMessage(dataJson):
-    global publisher1
-    # data = json.loads(dataJson)
-    publisher1.publish(dataJson)     #將date字串發布到topic
-    rate.sleep()
-
 # MQTT
 def initialise_clients(clientname):
     # callback assignment
@@ -33,19 +28,18 @@ def on_connect(client, userdata, flags, rc):
     print("Connected with result code " + str(rc))
     # client.subscribe(mqtt_config["topic"])
     client.subscribe("data/sensor")
-    client.subscribe("data/message")
 
 
 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'))
-    PubMessage(msg.payload.decode('utf-8'))
+    
 
 
 if __name__ == '__main__':
     # Mqtt
-    mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor", "topic1":"data/message"}
+    mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor"}
      
     client = initialise_clients("123456")
     client.on_connect = on_connect
@@ -59,9 +53,7 @@ if __name__ == '__main__':
     rospy.init_node(Mqtt_Node)
     # initialize Ros node
     topicName = 'uav_message'
-    topicName1 = 'formation_message'
     publisher = rospy.Publisher(topicName,String,queue_size=10)
-    publisher1 = rospy.Publisher(topicName1,String,queue_size=10)
 
     rate = rospy.Rate(10)
 

class_model/src/local_mqtt_sub_data_from_ros.py

@@ -15,8 +15,9 @@ from sensor_msgs.msg import NavSatFix
 from sensor_msgs.msg import Imu
 from sensor_msgs.msg import Range
 from geometry_msgs.msg import Vector3
+import time
 
-mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor", "topic1": "data/message"}
+mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor"}
 data ={}
 # Ros
 def callBack_imu(IMU):
@@ -31,13 +32,13 @@ def callBack_imu(IMU):
     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')
-
+    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))
+    lat=int(GPS.latitude*10000000)   #change the scale to centimeters
+    lon=int(GPS.longitude*10000000)
+    alt=int(GPS.altitude*100)
     dataGpsUpdate = {"lat": lat, "lon": lon, "alt": alt}
     data.update(dataGpsUpdate)
     # dataJsonFormate = json.dumps(data)
@@ -46,7 +47,7 @@ def callBack_gps(GPS):
 
 
 def callBack_compass_hdg(Compass):
-    heading = str(int(Compass.data*100))
+    heading = int(Compass.data*100)
     dataGpsUpdate = {"heading": heading}
     data.update(dataGpsUpdate)
     dataJsonFormate = json.dumps(data)
@@ -58,12 +59,6 @@ def callBack_state(state):
     data.update(dataGpsUpdate)
     
 
-def callBack_message(data):
-    MessageUpdate = {"message": data}
-    data.update(MessageUpdate)
-    dataMessageFormate = json.dumps(data)
-    mqtt_Pub_message(message=dataMessageFormate)
-
 
 # get parameter
 def GetParam(self,param_name):
@@ -99,15 +94,6 @@ def mqtt_Pub(message, topics = mqtt_config["topic"], waitForAck=False):
             time.sleep(0.25)
         client.topic_ack.remove(mid)
 
-def mqtt_Pub_message(message, topics = mqtt_config["topic1"], 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)
 
@@ -136,10 +122,10 @@ if __name__ == '__main__':
 	
     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/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/next_command",String,callBack_message)
+    # subscriber = rospy.Subscriber(ros_namespace+"/mavros/next_command",String,callBack_message)
     #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

class_model/src/main.cpp

@@ -25,7 +25,7 @@ int main(int argc, char **argv) {
 
     while(ros::ok()){
 
-    type = receiver_object.check_command();  
+    // type = receiver_object.check_command();  
     // // ROS_INFO("%s",type.c_str());
     
     // if(type == "" || type == "init"){

class_model/src/proto/duration_pb2.py

@@ -0,0 +1,78 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: duration.proto
+
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+  name='duration.proto',
+  package='google.protobuf',
+  syntax='proto3',
+  serialized_options=b'\n\023com.google.protobufB\rDurationProtoP\001Z1google.golang.org/protobuf/types/known/durationpb\370\001\001\242\002\003GPB\252\002\036Google.Protobuf.WellKnownTypes',
+  create_key=_descriptor._internal_create_key,
+  serialized_pb=b'\n\x0e\x64uration.proto\x12\x0fgoogle.protobuf\"*\n\x08\x44uration\x12\x0f\n\x07seconds\x18\x01 \x01(\x03\x12\r\n\x05nanos\x18\x02 \x01(\x05\x42\x83\x01\n\x13\x63om.google.protobufB\rDurationProtoP\x01Z1google.golang.org/protobuf/types/known/durationpb\xf8\x01\x01\xa2\x02\x03GPB\xaa\x02\x1eGoogle.Protobuf.WellKnownTypesb\x06proto3'
+)
+
+
+
+
+_DURATION = _descriptor.Descriptor(
+  name='Duration',
+  full_name='google.protobuf.Duration',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  create_key=_descriptor._internal_create_key,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='seconds', full_name='google.protobuf.Duration.seconds', index=0,
+      number=1, type=3, cpp_type=2, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='nanos', full_name='google.protobuf.Duration.nanos', index=1,
+      number=2, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=35,
+  serialized_end=77,
+)
+
+DESCRIPTOR.message_types_by_name['Duration'] = _DURATION
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+Duration = _reflection.GeneratedProtocolMessageType('Duration', (_message.Message,), {
+  'DESCRIPTOR' : _DURATION,
+  '__module__' : 'duration_pb2'
+  # @@protoc_insertion_point(class_scope:google.protobuf.Duration)
+  })
+_sym_db.RegisterMessage(Duration)
+
+
+DESCRIPTOR._options = None
+# @@protoc_insertion_point(module_scope)

class_model/src/proto/flight_information.proto

@@ -0,0 +1,14 @@
+syntax = 'proto3';
+
+// GPS + compass
+
+message GPS {
+    float LAT = 1;
+    float LON = 2;
+    float ALT = 3;
+}
+
+message flight_information_message {
+    GPS gps = 1;
+    float heading = 2;
+}

class_model/src/proto/flight_information_pb2.py

@@ -0,0 +1,130 @@
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: flight_information.proto
+
+import sys
+_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1'))
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+  name='flight_information.proto',
+  package='',
+  syntax='proto3',
+  serialized_options=None,
+  serialized_pb=_b('\n\x18\x66light_information.proto\",\n\x03GPS\x12\x0b\n\x03LAT\x18\x01 \x01(\x02\x12\x0b\n\x03LON\x18\x02 \x01(\x02\x12\x0b\n\x03\x41LT\x18\x03 \x01(\x02\"@\n\x1a\x66light_information_message\x12\x11\n\x03gps\x18\x01 \x01(\x0b\x32\x04.GPS\x12\x0f\n\x07heading\x18\x02 \x01(\x02\x62\x06proto3')
+)
+
+
+
+
+_GPS = _descriptor.Descriptor(
+  name='GPS',
+  full_name='GPS',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='LAT', full_name='GPS.LAT', index=0,
+      number=1, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='LON', full_name='GPS.LON', index=1,
+      number=2, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='ALT', full_name='GPS.ALT', index=2,
+      number=3, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=28,
+  serialized_end=72,
+)
+
+
+_FLIGHT_INFORMATION_MESSAGE = _descriptor.Descriptor(
+  name='flight_information_message',
+  full_name='flight_information_message',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='gps', full_name='flight_information_message.gps', index=0,
+      number=1, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='heading', full_name='flight_information_message.heading', index=1,
+      number=2, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=74,
+  serialized_end=138,
+)
+
+_FLIGHT_INFORMATION_MESSAGE.fields_by_name['gps'].message_type = _GPS
+DESCRIPTOR.message_types_by_name['GPS'] = _GPS
+DESCRIPTOR.message_types_by_name['flight_information_message'] = _FLIGHT_INFORMATION_MESSAGE
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+GPS = _reflection.GeneratedProtocolMessageType('GPS', (_message.Message,), dict(
+  DESCRIPTOR = _GPS,
+  __module__ = 'flight_information_pb2'
+  # @@protoc_insertion_point(class_scope:GPS)
+  ))
+_sym_db.RegisterMessage(GPS)
+
+flight_information_message = _reflection.GeneratedProtocolMessageType('flight_information_message', (_message.Message,), dict(
+  DESCRIPTOR = _FLIGHT_INFORMATION_MESSAGE,
+  __module__ = 'flight_information_pb2'
+  # @@protoc_insertion_point(class_scope:flight_information_message)
+  ))
+_sym_db.RegisterMessage(flight_information_message)
+
+
+# @@protoc_insertion_point(module_scope)

class_model/src/proto/flyformatioln.proto

@@ -0,0 +1,17 @@
+syntax = "proto3";
+
+// leader only
+
+enum FLY_FORMATION{
+    FLY_FORMATION_UNSPECIFIED = 0;
+    FLY_FORMATION_v = 1;
+    FLY_FORMATION_X = 2;
+    FLY_FORMATION_O = 3;
+    FLY_FORMATION_LINE = 4;
+    FLY_FORMATION_ROW = 5;
+    FLY_FORMATION_HEX = 6;
+  }
+  message fly_formation_message{
+    float velocity = 1;
+    FLY_FORMATION fly_formation= 2;
+  }

class_model/src/proto/flyformatioln_pb2.py

@@ -0,0 +1,138 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: flyformatioln.proto
+
+from google.protobuf.internal import enum_type_wrapper
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+  name='flyformatioln.proto',
+  package='',
+  syntax='proto3',
+  serialized_options=None,
+  create_key=_descriptor._internal_create_key,
+  serialized_pb=b'\n\x13\x66lyformatioln.proto\"P\n\x15\x66ly_formation_message\x12\x10\n\x08velocity\x18\x01 \x01(\x02\x12%\n\rfly_formation\x18\x02 \x01(\x0e\x32\x0e.FLY_FORMATION*\xb3\x01\n\rFLY_FORMATION\x12\x1d\n\x19\x46LY_FORMATION_UNSPECIFIED\x10\x00\x12\x13\n\x0f\x46LY_FORMATION_v\x10\x01\x12\x13\n\x0f\x46LY_FORMATION_X\x10\x02\x12\x13\n\x0f\x46LY_FORMATION_O\x10\x03\x12\x16\n\x12\x46LY_FORMATION_LINE\x10\x04\x12\x15\n\x11\x46LY_FORMATION_ROW\x10\x05\x12\x15\n\x11\x46LY_FORMATION_HEX\x10\x06\x62\x06proto3'
+)
+
+_FLY_FORMATION = _descriptor.EnumDescriptor(
+  name='FLY_FORMATION',
+  full_name='FLY_FORMATION',
+  filename=None,
+  file=DESCRIPTOR,
+  create_key=_descriptor._internal_create_key,
+  values=[
+    _descriptor.EnumValueDescriptor(
+      name='FLY_FORMATION_UNSPECIFIED', index=0, number=0,
+      serialized_options=None,
+      type=None,
+      create_key=_descriptor._internal_create_key),
+    _descriptor.EnumValueDescriptor(
+      name='FLY_FORMATION_v', index=1, number=1,
+      serialized_options=None,
+      type=None,
+      create_key=_descriptor._internal_create_key),
+    _descriptor.EnumValueDescriptor(
+      name='FLY_FORMATION_X', index=2, number=2,
+      serialized_options=None,
+      type=None,
+      create_key=_descriptor._internal_create_key),
+    _descriptor.EnumValueDescriptor(
+      name='FLY_FORMATION_O', index=3, number=3,
+      serialized_options=None,
+      type=None,
+      create_key=_descriptor._internal_create_key),
+    _descriptor.EnumValueDescriptor(
+      name='FLY_FORMATION_LINE', index=4, number=4,
+      serialized_options=None,
+      type=None,
+      create_key=_descriptor._internal_create_key),
+    _descriptor.EnumValueDescriptor(
+      name='FLY_FORMATION_ROW', index=5, number=5,
+      serialized_options=None,
+      type=None,
+      create_key=_descriptor._internal_create_key),
+    _descriptor.EnumValueDescriptor(
+      name='FLY_FORMATION_HEX', index=6, number=6,
+      serialized_options=None,
+      type=None,
+      create_key=_descriptor._internal_create_key),
+  ],
+  containing_type=None,
+  serialized_options=None,
+  serialized_start=106,
+  serialized_end=285,
+)
+_sym_db.RegisterEnumDescriptor(_FLY_FORMATION)
+
+FLY_FORMATION = enum_type_wrapper.EnumTypeWrapper(_FLY_FORMATION)
+FLY_FORMATION_UNSPECIFIED = 0
+FLY_FORMATION_v = 1
+FLY_FORMATION_X = 2
+FLY_FORMATION_O = 3
+FLY_FORMATION_LINE = 4
+FLY_FORMATION_ROW = 5
+FLY_FORMATION_HEX = 6
+
+
+
+_FLY_FORMATION_MESSAGE = _descriptor.Descriptor(
+  name='fly_formation_message',
+  full_name='fly_formation_message',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  create_key=_descriptor._internal_create_key,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='velocity', full_name='fly_formation_message.velocity', index=0,
+      number=1, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='fly_formation', full_name='fly_formation_message.fly_formation', index=1,
+      number=2, type=14, cpp_type=8, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=23,
+  serialized_end=103,
+)
+
+_FLY_FORMATION_MESSAGE.fields_by_name['fly_formation'].enum_type = _FLY_FORMATION
+DESCRIPTOR.message_types_by_name['fly_formation_message'] = _FLY_FORMATION_MESSAGE
+DESCRIPTOR.enum_types_by_name['FLY_FORMATION'] = _FLY_FORMATION
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+fly_formation_message = _reflection.GeneratedProtocolMessageType('fly_formation_message', (_message.Message,), {
+  'DESCRIPTOR' : _FLY_FORMATION_MESSAGE,
+  '__module__' : 'flyformatioln_pb2'
+  # @@protoc_insertion_point(class_scope:fly_formation_message)
+  })
+_sym_db.RegisterMessage(fly_formation_message)
+
+
+# @@protoc_insertion_point(module_scope)

class_model/src/requestData.cpp

@@ -3,11 +3,12 @@
 #include <iostream>
 
 global_location leader_position;
-std::string command; 
+json j_data;
+int command; 
 
 RequestClass::RequestClass() : node_handle_(""){
 
-  mqtt_data = node_handle_.subscribe("/uav_message", 10,
+  mqtt_data = node_handle_.subscribe("/uav_message", 100,
                                    &RequestClass::Data_callback, this);
   formation_data = node_handle_.subscribe("/formation_message", 10,
                                    &RequestClass::Message_callback, this);
@@ -18,14 +19,19 @@ RequestClass::~RequestClass() { ros::shutdown(); }
 
 void RequestClass::Data_callback(const std_msgs::String::ConstPtr &sensor) {
 
-	std::string data = sensor->data;
-  jsonToInt(data);
+
+  StringToJson(sensor->data);
+
+  // std::string data = sensor->data;
+  // jsonToInt(data);
+
 
 }
 
 void RequestClass::Message_callback(const std_msgs::String::ConstPtr &message) {
 
 	std::string data = message->data;
+  jsonToString(data);
   
 
 }
@@ -40,14 +46,19 @@ float RequestClass::get_leader_heading(){
   return heading;
 }
 
-std::string RequestClass::get_formation_message(){
+int RequestClass::get_formation_message(){
 
   return command;
 }
 
+// json RequestClass::get_data(){
+
+//   return data;
+// }
+
 void RequestClass::jsonToInt(std::string data){
 
-  std::string lat,lon,degree;
+  std::string lat,lon,alt,degree;
   std::string list[5]={"","","","",""};
   int j = 0;
 
@@ -86,7 +97,7 @@ void RequestClass::jsonToInt(std::string data){
   // 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);
+  //ROS_INFO("leader_heading: %f",heading);
 }
 
 void RequestClass::jsonToString(std::string data){
@@ -114,11 +125,41 @@ void RequestClass::jsonToString(std::string data){
     }
   }
 
-  command = list[1];
+  command = std::stoi(list[1]);
+
+  // ROS_INFO("command: %s",command);
+}
+
+void RequestClass::StringToJson(std::string data){
+
+  std::string lat = "",lon = "",alt = "",degree = "";
+	// j_data = json::parse(data);
+  std::cout << data << std::endl;
+  document.Parse(data.c_str());
+  // lat = j_data["lat"];
+  // lon = j_data["lon"];
+  // alt = j_data["alt"];
+  // degree = j_data["heading"];
+  
+  // leader_position.lat=std::stoi(lat);   
+	// leader_position.lon=std::stoi(lon);
+  // leader_position.alt=std::stoi(alt);
+  // heading = std::stoi(degree);
+
+  leader_position.lat=document["lat"].GetInt();   
+	leader_position.lon=document["lon"].GetInt();
+  leader_position.alt=document["alt"].GetInt();
+  heading = document["heading"].GetInt();
 
 
-  // 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);
+  
+  //std::cout << "sensor" << sensor->data << std::endl;
+  std::cout << document["lat"].GetInt() << std::endl;
+  std::cout << document["lon"].GetInt() << std::endl;
+  std::cout << document["alt"].GetInt() << std::endl;
+  std::cout << document["heading"].GetInt() << std::endl;
+  // std::cout << leader_position.lat << std::endl;
+  // std::cout << leader_position.lon << std::endl;
+  // std::cout << leader_position.alt << std::endl;
+  // std::cout << heading << std::endl;
 }