add 3 bionic drones subscriber

class_model/include/class_model/gps_location.h

@@ -3,6 +3,8 @@
 
 typedef struct global_location{
 	double lat,lon,alt;
+	float heading;
+	int ID;
 }global_location;
 
 #endif // GOBAL_LOCATION_H

class_model/include/class_model/requestData.h

@@ -7,6 +7,7 @@
 #include <string>
 #include <class_model/gps_location.h>
 #include <nlohmann/json.hpp>
+#include <algorithm>
 // #include "rapidjson/document.h"
 
 using json = nlohmann::json;
@@ -17,6 +18,9 @@ public:
     RequestClass();
     virtual ~RequestClass();
     global_location get_leader_GPS();
+    global_location get_self_GPS();
+    global_location get_M1_GPS();
+    global_location get_M2_GPS();
     float get_leader_heading();
     int get_formation_message();
     json get_data();
@@ -26,17 +30,27 @@ private:
     ros::NodeHandle node_handle_;
 
     void Data_callback(const std_msgs::String::ConstPtr &gps);
+    void self_callback(const std_msgs::String::ConstPtr &gps);
+    void M1_callback(const std_msgs::String::ConstPtr &gps);
+    void M2_callback(const std_msgs::String::ConstPtr &gps);
     void Message_callback(const std_msgs::String::ConstPtr &message);
     void jsonToString(std::string data);
     void L_INFO(std::string data);
+    void Bio_INFO(std::string data);
     float heading;
     global_location leader_position;
+    global_location self_position,M1_position,M2_position;
+    std::array <int,3> ID {1,2,3};
     // rapidjson::Document document;
     json j_data;
+    int self_ID,drone_ID;
 
   // SUBSCRIBE
     ros::Subscriber mqtt_data; 
     ros::Subscriber formation_data;
+    ros::Subscriber self_data;
+    ros::Subscriber member1_data;
+    ros::Subscriber member2_data;
 
 };
 

class_model/launch/mqtt_old.launch

@@ -11,7 +11,7 @@
 	</group>
 	<group>
 		
-		<node pkg="class_model" type="command.py" name="command" output="screen">
+		<node pkg="class_model" type="mqttCmdMain.py" name="command" output="screen">
 		</node>
 	</group>
 	

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": "140.120.31.133", "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,114 @@
+#!/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 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": "140.120.31.133", "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))
+    lon=str(int(GPS.longitude*10000000))
+    dataGpsUpdate = {"lat": lat, "lon": lon}
+    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_velocity(velocity):
+#     Vy=str(int(velocity.latitude*100))
+#     Vx=str(int(velocity.longitude*100))
+#     dataGpsUpdate = {"Vx": Vx, "Vy": Vy}
+#     data.update(dataGpsUpdate)
+#     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 = '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/leader_velocity',NavSatFix,callBack_velocity)  #從topic獲取string再呼叫callback
+    # subscriber = rospy.Subscriber('/mavros/rangefinder/rangefinder',Range,callBack_rng)  #從topic獲取string再呼叫callback
+
+    rospy.spin()

class_model/src/requestData.cpp

@@ -7,13 +7,19 @@ int command;
 
 RequestClass::RequestClass() : node_handle_(""){
 
-  mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100,
+  // mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100,
-                                   &RequestClass::Data_callback, this);
+  //                                  &RequestClass::Data_callback, this);
   formation_data = node_handle_.subscribe("/Fly_Formation_reciver", 10,
                                    &RequestClass::Message_callback, this);
-  // mqtt_data = node_handle_.subscribe("/uav_message", 100,
+  mqtt_data = node_handle_.subscribe("/uav_message", 100,
-  //                                  &RequestClass::Data_callback, this);
+                                   &RequestClass::Data_callback, this);
 
+  self_data = node_handle_.subscribe("/self_data_message", 100,
+                                   &RequestClass::self_callback, this);       //test bionic
+  member1_data = node_handle_.subscribe("/member1_data_message", 100,
+                                   &RequestClass::M1_callback, this);
+  member2_data = node_handle_.subscribe("/member2_data_message", 100,
+                                   &RequestClass::M2_callback, this);
 }
 
 RequestClass::~RequestClass() { ros::shutdown(); }
@@ -84,3 +90,52 @@ void RequestClass::L_INFO(std::string data){
   // std::cout << leader_position.alt << std::endl;
   // std::cout << heading << std::endl;
 }
+
+/*test biomic*/
+void RequestClass::self_callback(const std_msgs::String::ConstPtr &sensor) {
+  Bio_INFO(sensor->data);
+}
+void RequestClass::M1_callback(const std_msgs::String::ConstPtr &sensor) {
+  Bio_INFO(sensor->data);
+}
+void RequestClass::M2_callback(const std_msgs::String::ConstPtr &sensor) {
+  Bio_INFO(sensor->data);
+}
+
+void RequestClass::Bio_INFO(std::string data){
+
+  j_data = json::parse(data);        //open source
+
+  drone_ID = j_data["ID"];
+  std::remove(ID.begin(),ID.end(),self_ID);
+
+  if (drone_ID == self_ID){
+    self_position.lat=j_data["lat"];   
+	  self_position.lon=j_data["lon"];
+    self_position.alt=j_data["alt"];
+    self_position.heading = j_data["heading"];
+  }else if (drone_ID == ID[0]){
+    M1_position.lat=j_data["lat"];   
+	  M1_position.lon=j_data["lon"];
+    M1_position.alt=j_data["alt"];
+    M1_position.heading = j_data["heading"];
+  }else if (drone_ID == ID[1]){
+    M2_position.lat=j_data["lat"];   
+	  M2_position.lon=j_data["lon"];
+    M2_position.alt=j_data["alt"];
+    M2_position.heading = j_data["heading"];
+  }
+
+}
+global_location RequestClass::get_self_GPS(){
+
+  return self_position; 
+}
+global_location RequestClass::get_M1_GPS(){
+
+  return M1_position; 
+}
+global_location RequestClass::get_M2_GPS(){
+
+  return M2_position; 
+}

class_model/src/test_lib.cpp

@@ -1,14 +1,22 @@
 #include <iostream>
 #include "yaml-cpp/yaml.h"
+#include <algorithm>
 
 using namespace std;
 
 int main(int argc,char** argv){
 
-    YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");
+    // 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;
+
+    int ID[3] = {1,2,3};
+    int b = 1;
+    std::array <int,3> a {1,2,3};
+    remove(a.begin(),a.end(),b);
+    std::cout << a[0]<<a[1] <<std::endl;
 
-    cout <<"x:"<<config["routh1"]["x"].as<float>()<<endl;
-    cout <<"y:"<<config["routh1"]["y"].as<float>()<<endl;
 
 
     return 0;