ver2

class_model/.vscode/settings.json

@@ -4,5 +4,73 @@
     ],
     "python.analysis.extraPaths": [
         "/opt/ros/noetic/lib/python3/dist-packages"
-    ]
+    ],
+    "files.associations": {
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "hash_map": "cpp",
+        "strstream": "cpp",
+        "bit": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "complex": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "hash_set": "cpp",
+        "fstream": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "ostream": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cfenv": "cpp",
+        "cinttypes": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "variant": "cpp",
+        "*.ipp": "cpp"
+    }
 }

class_model/include/class_model/requestData.h

@@ -13,17 +13,20 @@ public:
     virtual ~RequestClass();
     global_location get_leader_GPS();
     float get_leader_heading();
+    std::string get_formation_message();
     
 private:
   // ROS NodeHandle
     ros::NodeHandle node_handle_;
 
     void Data_callback(const std_msgs::String::ConstPtr &gps);
+    void Message_callback(const std_msgs::String::ConstPtr &message);
     void jsonToInt(std::string data);
+    void jsonToString(std::string data);
     float heading;
   // SUBSCRIBE
     ros::Subscriber mqtt_data; 
-
+    ros::Subscriber formation_data;
 
 };
 

class_model/include/class_model/select.h

@@ -17,7 +17,7 @@ public:
 
     void init_formation(float x=0,float y=0);
     void line_formation(float x=0,float y=0);
-    void row_formation();
+    void row_formation(float x=0,float y=0);
     void circle_formation();
     void goose_formation(float x=0,float y=0);
     void protect_formation(float x=0,float y=0);

class_model/src/formation.cpp

@@ -65,300 +65,23 @@ void FormationClass::leader(){
 }
 }
 
-
-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 = "";
+    std_msgs::String message;
+
     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;
+    message.data = "1";
     while(true){
         if(flag==0){
             go2target(x,y);
         }else{
-            next_command.publish("1");
+            next_command.publish(message);
             break;
         }
 
@@ -383,6 +106,303 @@ void FormationClass::leader1(float x,float y){
     }
 }
 
+void FormationClass::follower1(int type){
+
+    std::string message,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();
+        message = request_object.get_formation_message();
+
+        if(command != pre_command || message == "1"){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+
+    // sleep(2);
+
+    // mode_object.set_Mode("LAND");
+
+}
+
+void FormationClass::follower2(int type){
+
+    std::string message,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();
+        message = request_object.get_formation_message();
+
+        if(command != pre_command || message == "1"){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+
+}
+
+void FormationClass::follower3(int type){
+
+    std::string message,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();
+        message = request_object.get_formation_message();
+
+        if(command != pre_command || message == "1"){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+
+}
+
+void FormationClass::follower4(int type){
+
+    std::string message,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();
+        message = request_object.get_formation_message();
+
+        if(command != pre_command || message == "1"){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+
+    }
+
+}
+
+void FormationClass::follower5(int type){
+
+    std::string message,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();
+        message = request_object.get_formation_message();
+
+        if(command != pre_command || message == "1"){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+
+}
+
+void FormationClass::sph_follower1(int type){
+
+    std::string message,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();
+        message = request_object.get_formation_message();
+
+        if(command != pre_command || message == "1"){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+
+}
+
+void FormationClass::sph_follower2(int type){
+
+    std::string message,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();
+        message = request_object.get_formation_message();
+
+        if(command != pre_command || message == "1"){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+
+    }
+
+}
+
+void FormationClass::sph_follower3(int type){
+
+    std::string message,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();
+        message = request_object.get_formation_message();
+
+        if(command != pre_command || message == "1"){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+}
+
+void FormationClass::sph_follower4(int type){
+
+    std::string message,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();
+        message = request_object.get_formation_message();
+
+        if(command != pre_command || message == "1"){
+            // ROS_INFO("change formation");
+            break;
+        }
+        pre_command = command;
+        
+
+    }
+}
+
+
+
+
 
 void FormationClass::calculate_position(float k,float theta,int direction){
 

class_model/src/local_mqtt_pub_data_to_ros.py

@@ -16,6 +16,12 @@ 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
@@ -26,18 +32,21 @@ def initialise_clients(clientname):
 def on_connect(client, userdata, flags, rc):
     print("Connected with result code " + str(rc))
     # client.subscribe(mqtt_config["topic"])
-    client.subscribe("data/imu")
+    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/imu"}
+    mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor", "topic1":"data/message"}
+     
     client = initialise_clients("123456")
     client.on_connect = on_connect
     client.on_message = on_message
@@ -50,7 +59,10 @@ 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

@@ -16,7 +16,7 @@ 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"}
+mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor", "topic1": "data/message"}
 data ={}
 # Ros
 def callBack_imu(IMU):
@@ -57,6 +57,15 @@ def callBack_state(state):
     dataGpsUpdate = {"mode": mode}
     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):
     param = self.get_param_srv(param_name)
     if param.success:
@@ -90,6 +99,14 @@ 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)
@@ -122,8 +139,9 @@ if __name__ == '__main__':
     # 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/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')
+    # ID = rospy.get_param(ros_namespace+'/leader/droneID')
     # subscriber = rospy.Subscriber('/mavros/rangefinder/rangefinder',Range,callBack_rng)  #從topic獲取string再呼叫callback
 
     # ID = rospy.GetParam("droneID")

class_model/src/main.cpp

@@ -28,29 +28,29 @@ int main(int argc, char **argv) {
     type = receiver_object.check_command();  
     // // ROS_INFO("%s",type.c_str());
     
-    if(type == "" || type == "init"){
+    // if(type == "" || type == "init"){
-        select_formation.goose_formation();
+    //     select_formation.goose_formation();
-        ROS_INFO("init formation");
+    //     ROS_INFO("init formation");
-    }else if(type == "line"){
+    // }else if(type == "line"){
-        select_formation.line_formation();
+    //     select_formation.line_formation();
-        // ROS_INFO("line foemation");
+    //     // ROS_INFO("line foemation");
-    }else if(type == "row"){
+    // }else if(type == "row"){
-        select_formation.row_formation();
+    //     select_formation.row_formation();
-    }else if(type == "circle"){
+    // }else if(type == "circle"){
-        select_formation.circle_formation();
+    //     select_formation.circle_formation();
-    }else if(type == "stop"){
+    // }else if(type == "stop"){
-        select_formation.stop();
+    //     select_formation.stop();
-    }else if(type == "land"){
+    // }else if(type == "land"){
-        mode_object.set_Mode("LAND");
+    //     mode_object.set_Mode("LAND");
-    }
+    // }
     
 
     //mission_object.cruise(20,20);
-    // select_formation.goose_formation();
+    select_formation.goose_formation(5,0);
     // }else if (type == "hex"){
     // select_formation.Hex_formation();
     // }
-    // select_formation.Hex_formation();
+    select_formation.row_formation(-5,0);
     sleep(0.5);
     // command_object.set_velocity(0 ,0 ,0,0,1);
 

class_model/src/requestData.cpp

@@ -3,11 +3,14 @@
 #include <iostream>
 
 global_location leader_position;
+std::string command; 
 
 RequestClass::RequestClass() : node_handle_(""){
 
   mqtt_data = node_handle_.subscribe("/uav_message", 10,
                                    &RequestClass::Data_callback, this);
+  formation_data = node_handle_.subscribe("/formation_message", 10,
+                                   &RequestClass::Message_callback, this);
 
 }
 
@@ -20,6 +23,13 @@ void RequestClass::Data_callback(const std_msgs::String::ConstPtr &sensor) {
 
 }
 
+void RequestClass::Message_callback(const std_msgs::String::ConstPtr &message) {
+
+	std::string data = message->data;
+  
+
+}
+
 global_location RequestClass::get_leader_GPS(){
 
   return leader_position; 
@@ -30,6 +40,11 @@ float RequestClass::get_leader_heading(){
   return heading;
 }
 
+std::string RequestClass::get_formation_message(){
+
+  return command;
+}
+
 void RequestClass::jsonToInt(std::string data){
 
   std::string lat,lon,degree;
@@ -68,6 +83,40 @@ 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);
+}
+
+void RequestClass::jsonToString(std::string data){
+
+  std::string list[1]={""};
+  int j = 0;
+
+  for(int i=0;i<data.length();i++){
+    if(data[i] == ':'){
+      i+=3;
+      while(data[i] != ','){
+        
+        if(data[i] == ',' || data[i] == '}'){
+            j++;
+            break;
+        }
+
+        list[j]=list[j]+data[i];
+        i++; 
+        
+        if(j>sizeof(list)){
+            break;
+        }     
+      }
+    }
+  }
+
+  command = list[1];
+
+
   // std::cout<<sizeof(data)<<std::endl;
   //std::cout<<data<<std::endl;
   // ROS_INFO("leader_GPS [%f,%f]", leader_position.lat, leader_position.lon);

class_model/src/select.cpp

@@ -46,7 +46,7 @@ void SelectClass::goose_formation(float x ,float y){
 
     counter = 0;
     if(param_object.getID() == 1){
-        formation_object.leader1();
+        formation_object.leader1(x,y);
     }else if(param_object.getID() == 2){
         formation_object.follower1(counter);
     }else if(param_object.getID() == 3){
@@ -88,11 +88,11 @@ void SelectClass::line_formation(float x ,float y){
 
 }
 
-void SelectClass::row_formation(){
+void SelectClass::row_formation(float x ,float y){
 
     counter = 2;
     if(param_object.getID() == 1){
-        formation_object.leader1();
+        formation_object.leader1(x,y);
     }else if(param_object.getID() == 2){
         formation_object.follower1(counter);
     }else if(param_object.getID() == 3){