merge conflict

class_model/CMakeLists.txt

@@ -255,11 +255,16 @@ target_include_directories(PID_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
 target_link_libraries(PID_lib ${catkin_LIBRARIES})
 add_dependencies(PID_lib main_generate_messages_cpp)
 
+add_library(PubData_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/PubData.cpp)
+target_include_directories(PubData_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
+target_link_libraries(PubData_lib ${catkin_LIBRARIES})
+add_dependencies(PubData_lib main_generate_messages_cpp)
+
 ##################
 
 add_library(formation_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/formation.cpp)
 target_include_directories(formation_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
-target_link_libraries(formation_lib ${catkin_LIBRARIES} sensor_lib mode_lib control_lib command_lib requestData_lib PID_lib)
+target_link_libraries(formation_lib ${catkin_LIBRARIES} sensor_lib mode_lib control_lib command_lib requestData_lib PID_lib PubData_lib)
 add_dependencies(formation_lib main_generate_messages_cpp)
 
 add_library(select_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/select.cpp)

class_model/include/class_model/PubData.h

@@ -0,0 +1,39 @@
+/*Publish Compass & velocity data to topic "DroneStatus"*/
+#ifndef PUBDATA_H
+#define PUBDATA_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <std_msgs/Float64.h>
+#include <mavros_msgs/Waypoint.h>
+#include "class_model/velocity.h"
+
+class PubDataClass {
+public:
+    PubDataClass();
+    virtual ~PubDataClass();
+    void publishData(velocity v);
+
+
+
+private:
+    // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+
+    //SUBSCRIBE
+    ros::Subscriber Compass;
+
+    //PUBLISH
+    ros::Publisher PubData;
+
+    mavros_msgs::Waypoint msg;
+    float heading;
+    void Compass_callback(const std_msgs::Float64::ConstPtr &degree);    
+
+
+
+    
+
+};
+
+#endif // PUBDATA_H

class_model/include/class_model/formation.h

@@ -10,6 +10,7 @@
 #include "class_model/command.h"
 #include "class_model/requestData.h"
 #include "class_model/PID.h"
+#include "class_model/PubData.h"
 #include <std_msgs/String.h>
 
 
@@ -25,6 +26,7 @@ public:
     ControlClass control_object;
     CommandClass command_object;
     RequestClass request_object;
+    PubDataClass PubData_object;
     PIDClass PID_x;
     PIDClass PID_y;
     
@@ -48,6 +50,7 @@ private:
     global_location follower_location;
     global_location leader_location;
     global_location current_location;
+    velocity drone_speed;
     int flag = 0,heading_status = 0;
     float pre_alt,cur_alt;
     float leader_pid[3] = {0.5 , 0.000000000001 ,0.5};

class_model/include/class_model/requestData.h

@@ -27,9 +27,8 @@ private:
 
     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);
-    void StringToJson(std::string data);
+    void L_INFO(std::string data);
     float heading;
     global_location leader_position;
     // rapidjson::Document document;

class_model/include/class_model/velocity.h

@@ -0,0 +1,8 @@
+#ifndef VELOCITY_H
+#define VELOCITY_H
+
+typedef struct velocity{
+	float x,y;
+}velocity;
+
+#endif // VELOCITY_H

class_model/src/PubData.cpp

@@ -0,0 +1,32 @@
+#include "class_model/PubData.h"
+
+PubDataClass::PubDataClass() : node_handle_("~"){
+
+    std::string ros_namespace;
+    if (!node_handle_.hasParam("namespace"))
+	{
+	}else{
+		node_handle_.getParam("namespace", ros_namespace);
+	}
+
+    PubData=node_handle_.advertise<mavros_msgs::Waypoint>(ros_namespace+"/DroneStatus",100);
+    Compass=node_handle_.subscribe(ros_namespace+"/mavros/global_position/compass_hdg", 10,
+                                   &PubDataClass::Compass_callback, this);
+
+}
+
+PubDataClass::~PubDataClass() { ros::shutdown(); }
+
+void PubDataClass::publishData(velocity v){
+
+    msg.param2 = v.x;
+    msg.param3 = v.y;
+    PubData.publish(msg);
+    ROS_INFO("pub drone status");
+
+}
+
+void PubDataClass::Compass_callback(const std_msgs::Float64::ConstPtr &degree){
+    msg.param1 = degree->data;
+    
+}

class_model/src/formation.cpp

@@ -578,25 +578,26 @@ void FormationClass::go2target(float x,float y){
         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);
+    drone_speed.x = PID_x.update(current_location.lon*1e5 , target_location.lon ,leader_pid);   //leader_pid defined in header file
+    drone_speed.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 (drone_speed.x < 0.3 && drone_speed.x > -0.3){                     //ignore small errors                  
+        drone_speed.x = 0;
     }
-    if (error_y < 0.3 && error_y > -0.3){
-        error_y = 0;
+    if (drone_speed.y < 0.3 && drone_speed.y > -0.3){
+        drone_speed.y = 0;
     }
 
-    if (error_x == 0 && error_y == 0){
+    if (drone_speed.x == 0 && drone_speed.y == 0){
         flag = 1;
     }
 
-    command_object.set_velocity(error_x ,error_y ,0,0,0.1);
-
+    PubData_object.publishData(drone_speed);
+    command_object.set_velocity(drone_speed.x ,drone_speed.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);

class_model/src/main.cpp

@@ -1,5 +1,6 @@
 #include "class_model/mission.h"
 #include "class_model/choose_leader.h"
+#include "class_model/PubData.h"
 
 int main(int argc, char **argv) {
 
@@ -15,28 +16,32 @@ int main(int argc, char **argv) {
     ReceiverClass receiver_object;
     MissionClass mission_object;
     CommandClass command_object;
-    SelectionClass selection_object;
+    SelectClass select_formation;
+    PubDataClass PubData_object;
+    
+    // SelectionClass selection_object;
     
     
     mode_object.set_Mode("GUIDED");
     control_object.arm();
     control_object.takeoff(4.5);
-    sleep(5);
+    sleep(2);
 
-    selection_object.choose_leader();
+    // selection_object.choose_leader();
     // mission_object.start();   //default hover in goose formation
 
     while(ros::ok()){
 
-    //json_object.get_command();       //for follower
-
 
+    //json_object.get_command();       //for follower
+    select_formation.goose_formation();
+    }
 
     ros::waitForShutdown();
 
     return 0;
 }
-//////////////////////////////////////////////////////
+
 
 /*
 ThreadGPSClass gps_object;

class_model/src/mqttPubMain.py

@@ -18,7 +18,7 @@ from sensor_msgs.msg import NavSatFix
 from sensor_msgs.msg import Imu
 from sensor_msgs.msg import Range
 from geometry_msgs.msg import Vector3
-
+'''
 # test proto
 import random
 
@@ -35,11 +35,13 @@ class fake_hdg():
 class state():
     def __init__(self):
         self.mode = "test"
+'''
 
 
 
 
 def init_dataFormat(cfg):
+    ros_namespace="/drone1"
     if cfg.msg_format == "Proto":
         utils.Proto_msg_from_ros.client = client
         utils.Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()
@@ -47,11 +49,15 @@ def init_dataFormat(cfg):
         utils.Proto_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
         utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
         utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
+        rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Proto_msg_from_ros.callBack_gps)
+        rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Proto_msg_from_ros.callBack_compass_hdg)
     elif cfg.msg_format == "Json":
         utils.Json_msg_from_ros.client = client
         utils.Json_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
         utils.Json_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
         utils.Json_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
+        rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
+        rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
     else:
         print("msg_format not found")
 
@@ -62,16 +68,16 @@ def on_connect(self, userdata, flags, rc):
     print("Connected with result code " + str(rc))
 
 def on_publish(self, userdata, mid):
-    pass
-
+    print("pub ok")
 
 if __name__ == '__main__':
     # Read Config
-    cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml")
+    FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig.yml")
+    cfg = utils.MQTT_ROS_Config(FilePath)
     client = utils.MQTTClient(cfg.MQTTClientNamePub)
-
+    print(cfg)
     # Setting Config
-    init_dataFormat(cfg)
+    
 
     # Mqtt
     client.on_connect = on_connect
@@ -81,12 +87,23 @@ if __name__ == '__main__':
     client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
     client.loop_start()
 
+    # Ros
+    rosClient = cfg.ROSClientNamePub
+    rospy.init_node(rosClient)
+    init_dataFormat(cfg)
+    
+  
+
+    rospy.spin()
+
+'''
     #test proto
     gps = fakeGps()
     hdg = fake_hdg()
     state = state()
     while True:
         #test proto
+
         gps.latitude = random.uniform(0, 10)
         gps.longitude = random.uniform(0, 10)
         gps.altitude = random.uniform(0, 10)
@@ -100,12 +117,4 @@ if __name__ == '__main__':
         # utils.Json_msg_from_ros.callBack_compass_hdg(hdg)
         # utils.Json_msg_from_ros.callBack_state(state)
         # time.sleep(1)
-
-    # Ros
-    # rosClient = cfg.ROSClientNamePub
-    # rospy.init_node(rosClient)
-    # subscriber = rospy.Subscriber('/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
-    # subscriber = rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
-
-
-    # rospy.spin()
+'''

class_model/src/mqttSubMain.py

@@ -12,27 +12,30 @@ from std_msgs.msg import String
 
 def init_dataFormat(cfg):
     if cfg.msg_format == "Proto":
-        # utils.Proto_msg_to_ros.rate = rospy.Rate(10)
-        # utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
-        # utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
+        utils.Proto_msg_to_ros.rate = rospy.Rate(10)
+        utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
+        utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
         
         utils.Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
         utils.Proto_msg_to_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
+        
+        client.on_message = utils.Proto_msg_to_ros.on_message
 
         utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
         utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
     elif cfg.msg_format == "Json":
-        utils.Proto_msg_to_ros.rate = rospy.Rate(10)
-        utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
-        utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
-
-        utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
-        utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
+        utils.Json_msg_to_ros.rate = rospy.Rate(10)
+        utils.Json_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
+        utils.Json_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
+        
+        client.on_message = utils.Json_msg_to_ros.on_message
+        utils.Json_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
+        utils.Json_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
     else:
         print("msg_format not found")
 
-def on_message(client, userdata, msg):
-    utils.Proto_msg_to_ros.ros_pub(msg)
+# def on_message(client, userdata, msg):
+#     utils.Json_msg_to_ros.ros_pub(msg)
 
 def on_connect(self, userdata, flags, rc):
     print("Connected with result code " + str(rc))
@@ -45,22 +48,23 @@ def on_publish(self, userdata, mid):
 
 if __name__ == '__main__':
     # Read Config
-    cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml")
-  
+    FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig.yml")
+    # Read Config
+    cfg = utils.MQTT_ROS_Config(FilePath)
+    print(cfg)
    
     # Mqtt
-    init_dataFormat(cfg)
     client = utils.MQTTClient(cfg.MQTTClientNameSub)
     client.on_connect = on_connect
-    client.on_message = on_message
+    client.on_message = utils.Json_msg_to_ros.on_message
     client.on_publish = on_publish
     client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
-    client.loop_forever()
+    
 
     # Ros
-    # rospy.init_node(cfg.ROSClientNameSub)
+    rospy.init_node(cfg.ROSClientNameSub)
     # initialize
-    # init_dataFormat(cfg)
-
+    init_dataFormat(cfg)
 
-    # rospy.spin()
+    client.loop_start()
+    rospy.spin()

class_model/src/requestData.cpp

@@ -7,9 +7,9 @@ int command;
 
 RequestClass::RequestClass() : node_handle_(""){
 
-  mqtt_data = node_handle_.subscribe("/uav_message", 100,
+  mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100,
                                    &RequestClass::Data_callback, this);
-  formation_data = node_handle_.subscribe("/formation_message", 10,
+  formation_data = node_handle_.subscribe("/Fly_Formation_reciver", 10,
                                    &RequestClass::Message_callback, this);
 
 }
@@ -17,13 +17,13 @@ RequestClass::RequestClass() : node_handle_(""){
 RequestClass::~RequestClass() { ros::shutdown(); }
 
 void RequestClass::Data_callback(const std_msgs::String::ConstPtr &sensor) {
-  StringToJson(sensor->data);
+  L_INFO(sensor->data);
 }
 
 void RequestClass::Message_callback(const std_msgs::String::ConstPtr &message) {
 
 	std::string data = message->data;
-  jsonToString(data);
+  // jsonToString(data);
   
 
 }
@@ -45,37 +45,11 @@ int RequestClass::get_formation_message(){
 
 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 = std::stoi(list[1]);
-
-  // ROS_INFO("command: %s",command);
 }
 
-void RequestClass::StringToJson(std::string data){
+void RequestClass::L_INFO(std::string data){
 	
-  std::cout <<"string: "<< data << std::endl;
+  // std::cout <<"string: "<< data << std::endl;
 
   // document.Parse(data.c_str());                  //china's library
   // leader_position.lat=document["lat"].GetInt();   
@@ -102,7 +76,7 @@ void RequestClass::StringToJson(std::string data){
   leader_position.alt=j_data["alt"];
   heading = j_data["heading"];
   
-  std::cout <<"Json: " <<j_data << std::endl;
+  // std::cout <<"Json: " <<j_data << std::endl;
   // std::cout << leader_position.lat << std::endl;
   // std::cout << leader_position.lon << std::endl;
   // std::cout << leader_position.alt << std::endl;

class_model/src/test_pub.py

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

class_model/src/utils/mqttConfig.yml

@@ -1,8 +1,8 @@
 MQTT_ROS:
-  msg_format: Proto
+  msg_format: Json
   MQTTClientNamePub: Drone650Pub
   MQTTClientNameSub: Drone650Sub
-  host: 192.168.50.117
+  host: 192.168.50.27
   port: 1883
   keepalive: 60
   willTopic: CheckDoneConnect
@@ -18,4 +18,4 @@ MQTT_ROS:
   ROSClientNamePub: Drone650Pub
   ROSClientNameSub: Drone650Sub
   ROStopicName_Flight_Information: Flight_Information_reciver
-  ROStopicName_Fly_Formation: Fly_Formation_reciver
+  ROStopicName_Fly_Formation: Fly_Formation_reciver

class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py

@@ -27,16 +27,20 @@ class Proto_msg_to_ros:
             proto_msg = cls.flight_information_msg.FromString(dataProto.payload)
             protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True)
             print(protoTOJson_msg)
-            # cls.publisher_flight_information.publish(protoTOJson_msg)
-            # cls.rate.sleep()
+            cls.publisher_Flight_Information.publish(protoTOJson_msg)
+            cls.rate.sleep()
         elif dataProto.topic == cls.Fly_Formation_topicToMqtt:
             proto_msg = cls.fly_formation_msg.FromString(dataProto.payload)
             protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True)
-            cls.publisher_Formation.publish(protoTOJson_msg)
+            cls.publisher_Fly_Formation.publish(protoTOJson_msg)
             cls.rate.sleep()
         else:
             print("topic not found")
+            
 
+    @staticmethod
+    def on_message(client, userdata, msg):
+        Proto_msg_to_ros.ros_pub(msg)
 
 
 class Json_msg_to_ros:
@@ -51,10 +55,15 @@ class Json_msg_to_ros:
     @classmethod
     def ros_pub(cls, dataJson):
         if dataJson.topic == cls.Flight_Information_topicToMqtt:
-            cls.publisher_flight_information.publish(dataJson.payload.decode('utf-8'))
+            cls.publisher_Flight_Information.publish(dataJson.payload.decode('utf-8'))
             cls.rate.sleep()
         elif dataJson.topic == cls.Fly_Formation_topicToMqtt:
             cls.publisher_Fly_Formation.publish(dataJson.payload.decode('utf-8'))
             cls.rate.sleep()
         else:
-            print("topic not found")
+            print("topic not found")
+            
+
+    @staticmethod
+    def on_message(client, userdata, msg):
+        Json_msg_to_ros.ros_pub(msg)

class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py

@@ -18,7 +18,7 @@ class Proto_msg_from_ros:
         cls.flight_information_msg.gps.LON = GPS.longitude
         cls.flight_information_msg.gps.ALT = GPS.altitude
         # print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
-        # print(flight_information_msg)
+        # print(cls.flight_information_msg)
 
     @classmethod
     def callBack_compass_hdg(cls, Compass):