diff --git a/class_model/include/class_model/gps_location.h b/class_model/include/class_model/gps_location.h index a7fd9b4..11178f6 100755 --- a/class_model/include/class_model/gps_location.h +++ b/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 \ No newline at end of file diff --git a/class_model/include/class_model/requestData.h b/class_model/include/class_model/requestData.h index 26fa369..13dcd7a 100755 --- a/class_model/include/class_model/requestData.h +++ b/class_model/include/class_model/requestData.h @@ -7,6 +7,7 @@ #include #include #include +#include // #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 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; }; diff --git a/class_model/launch/mqtt_old.launch b/class_model/launch/mqtt_old.launch index f078a75..9f718b2 100755 --- a/class_model/launch/mqtt_old.launch +++ b/class_model/launch/mqtt_old.launch @@ -11,7 +11,7 @@ - + diff --git a/class_model/src/local_mqtt_pub_data_to_ros.py b/class_model/src/local_mqtt_pub_data_to_ros.py new file mode 100755 index 0000000..c8fa50a --- /dev/null +++ b/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. diff --git a/class_model/src/local_mqtt_sub_data_from_ros.py b/class_model/src/local_mqtt_sub_data_from_ros.py new file mode 100755 index 0000000..e572aad --- /dev/null +++ b/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() \ No newline at end of file diff --git a/class_model/src/requestData.cpp b/class_model/src/requestData.cpp index fb8047e..31c8c46 100755 --- a/class_model/src/requestData.cpp +++ b/class_model/src/requestData.cpp @@ -7,13 +7,19 @@ int command; RequestClass::RequestClass() : node_handle_(""){ - mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100, - &RequestClass::Data_callback, this); + // mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100, + // &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, - // &RequestClass::Data_callback, this); + mqtt_data = node_handle_.subscribe("/uav_message", 100, + &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(); } @@ -83,4 +89,53 @@ void RequestClass::L_INFO(std::string data){ // std::cout << leader_position.lon << std::endl; // 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; } \ No newline at end of file diff --git a/class_model/src/test_lib.cpp b/class_model/src/test_lib.cpp index 463c454..9750ab6 100644 --- a/class_model/src/test_lib.cpp +++ b/class_model/src/test_lib.cpp @@ -1,14 +1,22 @@ #include #include "yaml-cpp/yaml.h" +#include 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:"<()<()< a {1,2,3}; + remove(a.begin(),a.end(),b); + std::cout << a[0]<()<()<