From 9701eb22d06f1043dc12e192c5905cf497a0b265 Mon Sep 17 00:00:00 2001 From: Xuan0319 Date: Thu, 9 Feb 2023 15:23:28 +0800 Subject: [PATCH] merge tony conflict --- class_model/include/class_model/DroneStatus.h | 7 +++++ class_model/include/class_model/follower.h | 9 +++++++ class_model/src/follower.cpp | 26 ++++++++++++------- class_model/src/mode.cpp | 2 +- class_model/src/mqttPubMain.py | 1 - class_model/src/requestData.cpp | 8 +++--- class_model/src/utils/__init__.py | 5 ++-- class_model/src/utils/mqttConfig_PUB.yml | 4 +-- class_model/src/utils/mqttConfig_SUB.yml | 4 +-- .../utils/protoJson_mqtt_sub_data_from_ros.py | 6 ++--- 10 files changed, 48 insertions(+), 24 deletions(-) create mode 100644 class_model/include/class_model/DroneStatus.h diff --git a/class_model/include/class_model/DroneStatus.h b/class_model/include/class_model/DroneStatus.h new file mode 100644 index 0000000..cbf3387 --- /dev/null +++ b/class_model/include/class_model/DroneStatus.h @@ -0,0 +1,7 @@ +#include +typedef struct DroneStatus +{ + /* data */ + int plane; + std::string leader; +} DroneStatus; diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h index 28d1042..a24bfbf 100755 --- a/class_model/include/class_model/follower.h +++ b/class_model/include/class_model/follower.h @@ -8,6 +8,7 @@ #include "class_model/control.h" #include "class_model/command.h" #include "class_model/requestData.h" +#include "class_model/DroneStatus.h" class FollowerClass { @@ -20,7 +21,12 @@ public: ControlClass control_object; CommandClass command_object; RequestClass request_object; + DroneStatus drone1; + DroneStatus drone2; + DroneStatus drone3; + void follower(); + private: // ROS NodeHandle ros::NodeHandle node_handle_; @@ -30,6 +36,9 @@ private: global_location leader_location; void calculate_position(float k,float theta); + void plane(global_location target_location,global_location leader_location,global_location follower_location); + void choose_leader(); + void follow(); }; diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp index 0c695f5..ea772c4 100755 --- a/class_model/src/follower.cpp +++ b/class_model/src/follower.cpp @@ -9,17 +9,25 @@ FollowerClass::~FollowerClass() { ros::shutdown(); } void FollowerClass::follower(){ - mode_object.set_Mode("GUIDED"); - control_object.arm(); - control_object.takeoff(1.5); - sleep(5); - while(true){ - calculate_position(4,30); - } + //plane() + //follow() + +} - sleep(2); +void FollowerClass::plane(global_location target_location,global_location leader_location,global_location follower_location){ + + //caculate drone is locate on R/L plane + float slope,c,y; + slope = (target_location.lat-leader_location.lat)/(target_location.lon-leader_location.lon); + c = target_location.lat - slope*target_location.lon; + y = slope * follower_location.lon + c; + + // if() + + //choose_leader() +} - mode_object.set_Mode("LAND"); +void FollowerClass::choose_leader(){ } diff --git a/class_model/src/mode.cpp b/class_model/src/mode.cpp index c97b02d..cd7d1cd 100755 --- a/class_model/src/mode.cpp +++ b/class_model/src/mode.cpp @@ -27,7 +27,7 @@ int ModeClass::set_Mode(std::string mode) { return 0; }else{ ROS_ERROR("Failed SetMode %s,retry...",mode.c_str()); - // set_Mode(mode); + set_Mode(mode); return 0; } diff --git a/class_model/src/mqttPubMain.py b/class_model/src/mqttPubMain.py index fbc4465..dde23ed 100755 --- a/class_model/src/mqttPubMain.py +++ b/class_model/src/mqttPubMain.py @@ -82,7 +82,6 @@ if __name__ == '__main__': # Mqtt client.on_connect = on_connect - client.on_message = on_message client.on_publish = on_publish client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain) diff --git a/class_model/src/requestData.cpp b/class_model/src/requestData.cpp index fd7e0f2..fb8047e 100755 --- a/class_model/src/requestData.cpp +++ b/class_model/src/requestData.cpp @@ -7,12 +7,12 @@ 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); } diff --git a/class_model/src/utils/__init__.py b/class_model/src/utils/__init__.py index 71fc5b7..fb9ca5c 100644 --- a/class_model/src/utils/__init__.py +++ b/class_model/src/utils/__init__.py @@ -2,5 +2,6 @@ from utils.protoJson_mqtt_pub_data_to_ros import Proto_msg_to_ros from utils.protoJson_mqtt_pub_data_to_ros import Json_msg_to_ros from utils.protoJson_mqtt_sub_data_from_ros import Proto_msg_from_ros from utils.protoJson_mqtt_sub_data_from_ros import Json_msg_from_ros -from utils.readConfig import MQTT_ROS_Config -from utils.basicMqtt import MQTTClient \ No newline at end of file +from utils.readConfig import Read_PUB_Config +from utils.readConfig import Read_SUB_Config +from utils.basicMqtt import MQTTClient diff --git a/class_model/src/utils/mqttConfig_PUB.yml b/class_model/src/utils/mqttConfig_PUB.yml index f4b79f7..566e029 100644 --- a/class_model/src/utils/mqttConfig_PUB.yml +++ b/class_model/src/utils/mqttConfig_PUB.yml @@ -1,7 +1,7 @@ MQTT: - msg_format: Json + msg_format: Proto MQTTClientNamePub: Drone550Pub - host: 192.168.50.118 + host: 140.120.31.133 port: 1883 keepalive: 60 willTopic: CheckDoneConnect diff --git a/class_model/src/utils/mqttConfig_SUB.yml b/class_model/src/utils/mqttConfig_SUB.yml index ec37b3d..313539a 100644 --- a/class_model/src/utils/mqttConfig_SUB.yml +++ b/class_model/src/utils/mqttConfig_SUB.yml @@ -1,7 +1,7 @@ MQTT: - msg_format: Json + msg_format: Proto MQTTClientNameSub: Drone550Sub - host: 192.168.50.118 + host: 140.120.31.133 port: 1883 keepalive: 60 willTopic: CheckDoneConnect diff --git a/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py b/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py index a81ceb4..8602d41 100644 --- a/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py +++ b/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py @@ -87,8 +87,8 @@ class Json_msg_from_ros: def callBack_compass_hdg(cls, Compass): heading = int(Compass.data*100) dataGpsUpdate = {"heading": heading} - cls.data.update(dataGpsUpdate) - dataJsonFormate = orjson.dumps(cls.data) + cls.GPS_Data.update(dataGpsUpdate) + dataJsonFormate = orjson.dumps(cls.GPS_Data) cls.mqtt_Pub(message=dataJsonFormate, topics=cls.Flight_Information_topicToMqtt) @classmethod def callBack_fly_formation(cls, Formation): @@ -103,4 +103,4 @@ class Json_msg_from_ros: while mid not in cls.client.topic_ack: print("wait for ack") time.sleep(0.25) - cls.client.topic_ack.remove(mid) \ No newline at end of file + cls.client.topic_ack.remove(mid)