Merge branch 'protobuf' of https://github.com/Xuan0319/DroneControl into protobuf

class_model/src/PubInfo.cpp

@@ -0,0 +1,22 @@
+#include "class_model/PubInfo.h"
+
+PubInfoClass::PubInfoClass():node_handle_("~"){
+
+    std::string ros_namespace;
+    if (!node_handle_.hasParam("namespace"))
+	{
+	}else{
+		node_handle_.getParam("namespace", ros_namespace);
+	}
+    PubInfo = node_handle_.advertise<diagnostic_msgs::KeyValue>(ros_namespace+"/Flight_Information_reciver",100);
+
+}
+
+PubInfoClass::~PubInfoClass() { ros::shutdown(); }
+
+void PubInfoClass::pub_info(INFO msg){
+    info.key = msg.type;           //key == drone's formation
+    info.value = msg.mission;      //value == drone's mission
+    PubInfo.publish(info);
+}
+

class_model/src/config.yaml

@@ -0,0 +1,4 @@
+routh1:
+  x: 1.0
+  y: 1.5
+  z: 0.0

class_model/src/main.cpp

@@ -1,6 +1,7 @@
 #include "class_model/mission.h"
 #include "class_model/choose_leader.h"
-#include "class_model/PubData.h"
+#include "class_model/PubInfo.h"
+#include "yaml-cpp/yaml.h"
 
 int main(int argc, char **argv) {
 
@@ -17,11 +18,12 @@ int main(int argc, char **argv) {
     MissionClass mission_object;
     CommandClass command_object;
     SelectClass select_formation;
-    PubDataClass PubData_object;
+    PubInfoClass PubInfo_object;
     
     // SelectionClass selection_object;
     
-    
+    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
+
     mode_object.set_Mode("GUIDED");
     control_object.arm();
     control_object.takeoff(4.5);
@@ -30,11 +32,16 @@ int main(int argc, char **argv) {
     // selection_object.choose_leader();
     // mission_object.start();   //default hover in goose formation
 
+    INFO msg;
+    msg.type = "V_formation";
+    msg.mission = "fly2target";
+
     while(ros::ok()){
 
 
     //json_object.get_command();       //for follower
-    select_formation.goose_formation();
+    //select_formation.goose_formation();
+    PubInfo_object.pub_info(msg);
     }
 
     ros::waitForShutdown();

class_model/src/test_lib.cpp

@@ -0,0 +1,15 @@
+#include <iostream>
+#include "yaml-cpp/yaml.h"
+
+using namespace std;
+
+int main(int argc,char** argv){
+
+    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");
+
+    cout <<"x:"<<config["routh1"]["x"].as<float>()<<endl;
+    cout <<"y:"<<config["routh1"]["y"].as<float>()<<endl;
+
+
+    return 0;
+}

class_model/src/test_pub.py

@@ -1,14 +1,16 @@
 #!/usr/bin/env python3
 #coding:utf-8
 import rospy
-from mavros_msgs.msg import Waypoint
+# from mavros_msgs.msg import Waypoint
+from diagnostic_msgs.msg import KeyValue
  
 def callBack(data):
-    print (data.param1)
+    print (data)
+
  
 if __name__ == '__main__':
     nodeName = 'subscriber_py'
     rospy.init_node(nodeName)
-    topicName = '/DroneStatus'
-    subscriber = rospy.Subscriber(topicName,Waypoint,callBack)  #從topic獲取string再呼叫callback
+    topicName = '/Flight_Information_reciver'
+    subscriber = rospy.Subscriber(topicName,KeyValue,callBack)  #從topic獲取string再呼叫callback
     rospy.spin()