From 01262ec88caee4f2889ef9172ff756f1d7fc2b0d Mon Sep 17 00:00:00 2001 From: Xuan0319 Date: Tue, 14 Mar 2023 23:21:39 +0800 Subject: [PATCH] gazebo test ok,but PID control weird. --- class_model/.vscode/launch.json | 2 +- class_model/include/class_model/FindLeader.h | 2 +- class_model/include/class_model/follower.h | 2 +- class_model/include/class_model/requestData.h | 3 +- .../include/class_model/requestData2.h | 62 ++++++++ .../include/class_model/requestData3.h | 62 ++++++++ class_model/src/Leader.cpp | 21 +-- class_model/src/bio_main.cpp | 24 +-- class_model/src/bio_main2.cpp | 26 +-- class_model/src/bio_main3.cpp | 24 +-- class_model/src/config.yaml | 4 +- class_model/src/config2.yaml | 4 +- class_model/src/config3.yaml | 4 +- class_model/src/follower.cpp | 84 +++++----- class_model/src/requestData.cpp | 20 ++- class_model/src/requestData2.cpp | 148 ++++++++++++++++++ class_model/src/requestData3.cpp | 148 ++++++++++++++++++ iq_sim/worlds/three_drone.world | 6 +- 18 files changed, 543 insertions(+), 103 deletions(-) create mode 100755 class_model/include/class_model/requestData2.h create mode 100755 class_model/include/class_model/requestData3.h create mode 100755 class_model/src/requestData2.cpp create mode 100755 class_model/src/requestData3.cpp diff --git a/class_model/.vscode/launch.json b/class_model/.vscode/launch.json index b9471b3..8757b87 100644 --- a/class_model/.vscode/launch.json +++ b/class_model/.vscode/launch.json @@ -9,7 +9,7 @@ "name": "ROS: Launch", "type": "ros", "request": "launch", - "target": "/home/dodo/ardupilot_ws/src/class_model/launch/test.launch", + "target": "/home/dodo/ardupilot_ws/src/class_model/launch/bio_drones.launch", "launch": [ "rviz", "gz", diff --git a/class_model/include/class_model/FindLeader.h b/class_model/include/class_model/FindLeader.h index f76cb81..784e528 100644 --- a/class_model/include/class_model/FindLeader.h +++ b/class_model/include/class_model/FindLeader.h @@ -7,7 +7,7 @@ typedef struct result{ bool is_leader; }result; -result check_leader(global_location data[], size_t index){ +result check_leader(global_location* data, size_t index){ float d[index]; int check_ID; diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h index 9fd5852..27fe4ad 100755 --- a/class_model/include/class_model/follower.h +++ b/class_model/include/class_model/follower.h @@ -34,7 +34,7 @@ public: DroneStatus self; DroneStatus samePlane[5]; - void follower(global_location data[], size_t index, int refID); + void follower(global_location* member_data, size_t index, int refID); int find_ref_drone(global_location data[], size_t index, int leaderID); float lon[3],lat[3]; diff --git a/class_model/include/class_model/requestData.h b/class_model/include/class_model/requestData.h index 6856bc4..41d16d0 100755 --- a/class_model/include/class_model/requestData.h +++ b/class_model/include/class_model/requestData.h @@ -47,8 +47,7 @@ private: std::array ID_array {1,2,3}; // rapidjson::Document document; json j_data,j_data2,j_data3; - YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point) - int self_ID= config["DroneParam"]["ID"].as(),drone_ID; + int self_ID,drone_ID; // SUBSCRIBE ros::Subscriber mqtt_data; diff --git a/class_model/include/class_model/requestData2.h b/class_model/include/class_model/requestData2.h new file mode 100755 index 0000000..e949a26 --- /dev/null +++ b/class_model/include/class_model/requestData2.h @@ -0,0 +1,62 @@ +/*Subscribe Data Which MQTT Pubilsh */ +#ifndef REQUESTDATA_H +#define REQUESTDATA_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include "yaml-cpp/yaml.h" + +// #include "rapidjson/document.h" + +using json = nlohmann::json; + + +class RequestClass { +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(); + +private: + // ROS NodeHandle + ros::NodeHandle node_handle_; + + void Data_callback(const std_msgs::String::ConstPtr &gps); + void drone1_callback(const std_msgs::String::ConstPtr &gps); + void drone2_callback(const std_msgs::String::ConstPtr &gps); + void drone3_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(json json_data,int drone_ID); + float heading; + global_location leader_position; + global_location self_position,M1_position,M2_position; + std::array ID_array {1,2,3}; + // rapidjson::Document document; + json j_data,j_data2,j_data3; + YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config2.yaml"); //reading yaml parameter(target point) + int self_ID= config["DroneParam"]["ID"].as(),drone_ID; + + // SUBSCRIBE + ros::Subscriber mqtt_data; + ros::Subscriber formation_data; + ros::Subscriber drone1_data; + ros::Subscriber drone2_data; + ros::Subscriber drone3_data; +}; + +#endif // REQUESTDATA_H + diff --git a/class_model/include/class_model/requestData3.h b/class_model/include/class_model/requestData3.h new file mode 100755 index 0000000..dd3a37d --- /dev/null +++ b/class_model/include/class_model/requestData3.h @@ -0,0 +1,62 @@ +/*Subscribe Data Which MQTT Pubilsh */ +#ifndef REQUESTDATA_H +#define REQUESTDATA_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include "yaml-cpp/yaml.h" + +// #include "rapidjson/document.h" + +using json = nlohmann::json; + + +class RequestClass { +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(); + +private: + // ROS NodeHandle + ros::NodeHandle node_handle_; + + void Data_callback(const std_msgs::String::ConstPtr &gps); + void drone1_callback(const std_msgs::String::ConstPtr &gps); + void drone2_callback(const std_msgs::String::ConstPtr &gps); + void drone3_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(json json_data,int drone_ID); + float heading; + global_location leader_position; + global_location self_position,M1_position,M2_position; + std::array ID_array {1,2,3}; + // rapidjson::Document document; + json j_data,j_data2,j_data3; + YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config3.yaml"); //reading yaml parameter(target point) + int self_ID= config["DroneParam"]["ID"].as(),drone_ID; + + // SUBSCRIBE + ros::Subscriber mqtt_data; + ros::Subscriber formation_data; + ros::Subscriber drone1_data; + ros::Subscriber drone2_data; + ros::Subscriber drone3_data; +}; + +#endif // REQUESTDATA_H + diff --git a/class_model/src/Leader.cpp b/class_model/src/Leader.cpp index 03d9f06..e272ad9 100644 --- a/class_model/src/Leader.cpp +++ b/class_model/src/Leader.cpp @@ -12,9 +12,6 @@ void LeaderClass::leader(global_location target){ std::string command = "",pre_command = receiver_object.check_command();; std_msgs::String message; - target_location.lon = target.lon*1e5; - target_location.lat = target.lat*1e5; - sleep(3); flag = 0; heading_status = 0; while(true){ @@ -47,12 +44,16 @@ void LeaderClass::leader(global_location target){ } } -void LeaderClass::go2target(float x,float y){ +void LeaderClass::go2target(float target_lon,float target_lat){ current_location=GPS_object.gps_position(); float target_heading,error_heading; float heading = GPS_object.get_heading(); + + float x,y; + x = (target_lon/100) - (current_location.lon*1e5); + y = (target_lat/100) - (current_location.lat*1e5); target_heading = ConvertDeg(x,y); // float error_yaw = 0.2; @@ -63,8 +64,8 @@ void LeaderClass::go2target(float x,float y){ face2target(target_heading); } - 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); + drone_speed.x = PID_x.update(current_location.lon*1e5 , target_lon/100 ,leader_pid); //leader_pid defined in header file + drone_speed.y = PID_y.update(current_location.lat*1e5 , target_lat/100 ,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 ); @@ -92,15 +93,15 @@ void LeaderClass::go2target(float x,float y){ flag = 1; } - PubData_object.publishData(drone_speed); + // 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",drone_speed.x,drone_speed.y); + // ROS_INFO("%f,%f",drone_speed.x,drone_speed.y); // ROS_INFO("heading:%f,target_heading:%f",heading,target_heading); // ROS_INFO("target:%f,%f",target_location.lon ,target_location.lat); // ROS_INFO("current:%f,%f",current_location.lon*1e5 ,current_location.lat*1e5); - ROS_INFO("************************************"); + // ROS_INFO("************************************"); @@ -119,5 +120,5 @@ void LeaderClass::face2target(float target_heading){ heading_status = 1; } command_object.set_velocity(0 ,0 ,0,error_yaw,0.1); - ROS_INFO("face2target..."); + // ROS_INFO("face2target...%f",target_heading); } \ No newline at end of file diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp index 4712e5c..a7f5f70 100644 --- a/class_model/src/bio_main.cpp +++ b/class_model/src/bio_main.cpp @@ -25,10 +25,10 @@ int main(int argc, char **argv) { int ref_ID,checkLeader; YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point) - target.lon = config["routh1"]["x"].as(); - target.lat = config["routh1"]["y"].as(); - + target.lon = config["routh1"]["x"].as()*1e7; + target.lat = config["routh1"]["y"].as()*1e7; + sleep(5); self = request_object.get_self_GPS(); M1 = request_object.get_M1_GPS(); M2 = request_object.get_M2_GPS(); @@ -38,9 +38,9 @@ int main(int argc, char **argv) { size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); - // mode_object.set_Mode("GUIDED"); - // control_object.arm(); - // control_object.takeoff(2); + mode_object.set_Mode("GUIDED"); + control_object.arm(); + control_object.takeoff(2); checkLeader = check_leader(pos,index).is_leader; if(checkLeader != 1){ @@ -48,12 +48,12 @@ int main(int argc, char **argv) { } while(true){ - // - // if(checkLeader == 1){ - // leader_object.leader(target); - // }else{ - // follower_object.follower(member,m_index,ref_ID); //follow reference drone position - // } + + if(checkLeader == 1){ + // leader_object.leader(target); + }else{ + follower_object.follower(member,m_index,ref_ID); //follow reference drone position + } } ros::waitForShutdown(); diff --git a/class_model/src/bio_main2.cpp b/class_model/src/bio_main2.cpp index 67d0afa..dabc7f7 100644 --- a/class_model/src/bio_main2.cpp +++ b/class_model/src/bio_main2.cpp @@ -25,22 +25,24 @@ int main(int argc, char **argv) { int ref_ID,checkLeader; YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config2.yaml"); //reading yaml parameter(target point) - target.lon = config["routh1"]["x"].as(); - target.lat = config["routh1"]["y"].as(); - + target.lon = config["routh1"]["x"].as()*1e7; + target.lat = config["routh1"]["y"].as()*1e7; + sleep(5); self = request_object.get_self_GPS(); M1 = request_object.get_M1_GPS(); M2 = request_object.get_M2_GPS(); + std::cout <(); - target.lat = config["routh1"]["y"].as(); - + target.lon = config["routh1"]["x"].as()*1e7; + target.lat = config["routh1"]["y"].as()*1e7; + sleep(5); self = request_object.get_self_GPS(); M1 = request_object.get_M1_GPS(); M2 = request_object.get_M2_GPS(); @@ -38,9 +38,9 @@ int main(int argc, char **argv) { size_t index = sizeof(pos)/sizeof(pos[0]); size_t m_index = sizeof(member)/sizeof(member[0]); - // mode_object.set_Mode("GUIDED"); - // control_object.arm(); - // control_object.takeoff(2); + mode_object.set_Mode("GUIDED"); + control_object.arm(); + control_object.takeoff(2); checkLeader = check_leader(pos,index).is_leader; if(checkLeader != 1){ @@ -48,12 +48,12 @@ int main(int argc, char **argv) { } while(true){ - // - // if(checkLeader == 1){ - // leader_object.leader(target); - // }else{ - // follower_object.follower(member,m_index,ref_ID); //follow reference drone position - // } + + if(checkLeader == 1){ + // leader_object.leader(target); + }else{ + follower_object.follower(member,m_index,ref_ID); //follow reference drone position + } } ros::waitForShutdown(); diff --git a/class_model/src/config.yaml b/class_model/src/config.yaml index d1cd7be..a8d4e77 100644 --- a/class_model/src/config.yaml +++ b/class_model/src/config.yaml @@ -1,8 +1,8 @@ DroneParam: ID: 1 routh1: - x: 120.6743661 #(24.1218859, 120.6743161) - y: 24.1219359 + x: 120.6743161 #(24.1218859, 120.6743161) + y: 24.1219310 z: 0.0 formation: distance: 4.0 diff --git a/class_model/src/config2.yaml b/class_model/src/config2.yaml index 1462b45..0e34982 100644 --- a/class_model/src/config2.yaml +++ b/class_model/src/config2.yaml @@ -1,8 +1,8 @@ DroneParam: ID: 2 routh1: - x: 120.6743661 #(24.1218859, 120.6743161) - y: 24.1219359 + x: 120.6743161 #(24.1218859, 120.6743161) + y: 24.1219310 z: 0.0 formation: distance: 4.0 diff --git a/class_model/src/config3.yaml b/class_model/src/config3.yaml index 9d7b30d..4694e31 100644 --- a/class_model/src/config3.yaml +++ b/class_model/src/config3.yaml @@ -1,8 +1,8 @@ DroneParam: ID: 3 routh1: - x: 120.6743661 #(24.1218859, 120.6743161) - y: 24.1219359 + x: 120.6743161 #(24.1218859, 120.6743161) + y: 24.1219310 z: 0.0 formation: distance: 4.0 diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp index 2e9ac91..9f2c7ed 100755 --- a/class_model/src/follower.cpp +++ b/class_model/src/follower.cpp @@ -12,11 +12,11 @@ FollowerClass::FollowerClass() : node_handle_(""){ FollowerClass::~FollowerClass() { ros::shutdown(); } -void FollowerClass::follower(global_location data[], size_t index, int refID){ +void FollowerClass::follower(global_location* member_data, size_t index, int refID){ - if(refID == data[0].ID){ + if(refID == member_data[0].ID){ ref = &RequestClass::get_M1_GPS; - }else if (refID == data[1].ID) + }else if (refID == member_data[1].ID) { ref = &RequestClass::get_M2_GPS; } @@ -42,49 +42,52 @@ void FollowerClass::plane(global_location target, global_location leader, global //caculate drone is locate on R/L plane float slope,c,y; slope = (target.lat-leader.lat)/(target.lon-leader.lon); - c = target.lat - slope*target.lon; + c = leader.lat - slope*leader.lon; y = slope * follower.lon + c; + // std::cout<< follower.ID << ","<< follower.lat <<","<< y <= y){ + if(follower.lat <= y){ + // std::cout<< follower.lat <<","<< y < y){ + // std::cout<< follower.lat <<","<< y <radian - theta = theta*3.14/180; - - leader_location=request_object.get_leader_GPS(); //get Leader/Follower 's GPS - refpos = (request_object.*ref)(); - + theta = plane*theta*3.14/180; - double Pf[]={},Pl[]={leader_location.lon,leader_location.lat}; //transfer maxtrix + double Pf[]={},Pl[]={refpos.lon ,refpos.lat}; //transfer maxtrix float transfer[2][2]={ cos(phi),-sin(phi), sin(phi),cos(phi) @@ -234,18 +234,18 @@ void FollowerClass::calculate_position(float k,float theta,int plane){ Pf[0]=(transfer[0][0]*Q[0]+transfer[0][1]*Q[1])*T[0]+Pl[0]/100; Pf[1]=(transfer[1][0]*Q[0]+transfer[1][1]*Q[1])*T[1]+Pl[1]/100; - error_lon = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid); //caculate error (PID) - error_lat = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid); + // error_lon = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid); //caculate error (PID) + // error_lat = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid); // float error_x = PID_x.update1(follower_location.lon*1e5 , Pf[0] ); //caculate error // float error_y = PID_y.update1(follower_location.lat*1e5 , Pf[1] ); - // error_lon = Pf[0] - (follower_location.lon*1e+5); //caculate error - // error_lat = Pf[1] - (follower_location.lat*1e+5); + error_lon = Pf[0] - (follower_location.lon*1e+5); //caculate error + error_lat = Pf[1] - (follower_location.lat*1e+5); - float error_degree = deg_phi - heading + plane; + float error_degree = deg_phi - heading; float error_yaw = 0.2; //CCW if (error_degree >= 360){ @@ -269,20 +269,20 @@ void FollowerClass::calculate_position(float k,float theta,int plane){ error_yaw = 0; } - // if (error_lon < -1){ - // error_lon = -1; - // } - // if (error_lon > 1){ - // error_lon = 1; - // } - // if (error_lat < -1){ - // error_lat = -1; - // } - // if (error_lat > 1){ - // error_lat = 1; - // } - - // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100); + if (error_lon < -1){ + error_lon = -1; + } + if (error_lon > 1){ + error_lon = 1; + } + if (error_lat < -1){ + error_lat = -1; + } + if (error_lat > 1){ + error_lat = 1; + } + + // ROS_INFO("%d,%f,%f",refpos.ID,refpos.lon ,refpos.lat ); // ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5); // ROS_INFO("%f,%f",Pf[0],Pf[1]); // // ROS_INFO("%f,%f",error_x,error_y); @@ -292,6 +292,8 @@ void FollowerClass::calculate_position(float k,float theta,int plane){ // // ROS_INFO("error_degree:%f",error_degree); // // ROS_INFO("error_yaw:%f",error_yaw); // ROS_INFO("************************************"); - - command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.3); + + // ROS_INFO("drone%d,plane%d",self.ID,plane); + + command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.1); } diff --git a/class_model/src/requestData.cpp b/class_model/src/requestData.cpp index 292a1a8..2794dff 100755 --- a/class_model/src/requestData.cpp +++ b/class_model/src/requestData.cpp @@ -7,6 +7,17 @@ int command; RequestClass::RequestClass() : node_handle_("~"){ + std::string ros_namespace; + if (!node_handle_.hasParam("namespace")) + { + ROS_INFO("using default namespace"); + }else{ + node_handle_.getParam("namespace", ros_namespace); + // ROS_INFO("using namespace %s", ros_namespace.c_str()); + } + node_handle_.getParam(ros_namespace+ros_namespace+"/droneID", self_ID); + ROS_INFO("using namespace %s,ID:%d", ros_namespace.c_str(),self_ID); + // mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100, // &RequestClass::Data_callback, this); formation_data = node_handle_.subscribe("/Fly_Formation_reciver", 10, @@ -20,6 +31,8 @@ RequestClass::RequestClass() : node_handle_("~"){ &RequestClass::drone2_callback, this); drone3_data = node_handle_.subscribe("/Dron380_Flight_Information_reciver", 10, &RequestClass::drone3_callback, this); + + } RequestClass::~RequestClass() { ros::shutdown(); } @@ -113,27 +126,30 @@ void RequestClass::drone3_callback(const std_msgs::String::ConstPtr &sensor) { void RequestClass::Bio_INFO(json json_data,int drone_ID){ std::remove(ID_array.begin(),ID_array.end(),self_ID); + // std::cout <<"ID: " <()); - self_position.heading = json_data["heading"]; + self_position.heading = json_data["heading"]; + self_position.ID = drone_ID; }else if (drone_ID == ID_array[0]){ M1_position.lat = json_data["lat"]; M1_position.lon = json_data["lon"]; // M1_position.alt = std::stoi(json_data["alt"].get()); M1_position.heading = json_data["heading"]; + M1_position.ID = drone_ID; }else if (drone_ID == ID_array[1]){ M2_position.lat = json_data["lat"]; M2_position.lon = json_data["lon"]; // M2_position.alt = std::stoi(json_data["alt"].get()); M2_position.heading = json_data["heading"]; + M2_position.ID = drone_ID; } } global_location RequestClass::get_self_GPS(){ - return self_position; } global_location RequestClass::get_M1_GPS(){ diff --git a/class_model/src/requestData2.cpp b/class_model/src/requestData2.cpp new file mode 100755 index 0000000..ca84216 --- /dev/null +++ b/class_model/src/requestData2.cpp @@ -0,0 +1,148 @@ +#include"class_model/requestData2.h" +#include + + +int command; +// json j_data; //Don't set json as global variable + +RequestClass::RequestClass() : node_handle_("~"){ + + // 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", 10, + // &RequestClass::Data_callback, this); + + drone1_data = node_handle_.subscribe("/Dron650_Flight_Information_reciver", 10, + &RequestClass::drone1_callback, this); //test bionic + drone2_data = node_handle_.subscribe("/Dron550_Flight_Information_reciver", 10, + &RequestClass::drone2_callback, this); + drone3_data = node_handle_.subscribe("/Dron380_Flight_Information_reciver", 10, + &RequestClass::drone3_callback, this); +} + +RequestClass::~RequestClass() { ros::shutdown(); } + +void RequestClass::Data_callback(const std_msgs::String::ConstPtr &sensor) { + L_INFO(sensor->data); +} + +void RequestClass::Message_callback(const std_msgs::String::ConstPtr &message) { + + std::string data = message->data; + // jsonToString(data); + + +} + +global_location RequestClass::get_leader_GPS(){ + + return leader_position; +} + +float RequestClass::get_leader_heading(){ + + return heading; +} + +int RequestClass::get_formation_message(){ + + return command; +} + +void RequestClass::jsonToString(std::string data){ + +} + +void RequestClass::L_INFO(std::string data){ + + // std::cout <<"string: "<< data << std::endl; + + // document.Parse(data.c_str()); //china's library + // leader_position.lat=document["lat"].GetInt(); + // leader_position.lon=document["lon"].GetInt(); + // leader_position.alt=document["alt"].GetInt(); + // heading = document["heading"].GetInt(); + + // std::cout << document["lat"].GetInt() << std::endl; + // std::cout << document["lon"].GetInt() << std::endl; + // std::cout << document["alt"].GetInt() << std::endl; + // std::cout << document["heading"].GetInt() << std::endl; + + //********************************************// + j_data = json::parse(data); //open source + + // std::string lat = "",lon = "",alt = "",degree = ""; + // lat = j_data["lat"]; + // lon = j_data["lon"]; + // alt = j_data["alt"]; + // degree = j_data["heading"]; + + leader_position.lat = std::stoi(j_data["lat"].get()); + leader_position.lon = std::stoi(j_data["lon"].get()); + // leader_position.alt = std::stoi(j_data["alt"].get()); + heading = std::stoi(j_data["heading"].get()); + + // std::cout <<"Json: " <data); //open source + // std::cout <<"Json: " <data); //open source + // std::cout <<"Json: " <data); + Bio_INFO(j_data3,drone_ID); +} + +void RequestClass::Bio_INFO(json json_data,int drone_ID){ + + std::remove(ID_array.begin(),ID_array.end(),self_ID); + + if (drone_ID == self_ID){ + self_position.lat = json_data["lat"]; + self_position.lon = json_data["lon"]; + // self_position.alt = std::stoi(json_data["alt"].get()); + self_position.heading = json_data["heading"]; + self_position.ID = drone_ID; + }else if (drone_ID == ID_array[0]){ + M1_position.lat = json_data["lat"]; + M1_position.lon = json_data["lon"]; + // M1_position.alt = std::stoi(json_data["alt"].get()); + M1_position.heading = json_data["heading"]; + M1_position.ID = drone_ID; + }else if (drone_ID == ID_array[1]){ + M2_position.lat = json_data["lat"]; + M2_position.lon = json_data["lon"]; + // M2_position.alt = std::stoi(json_data["alt"].get()); + M2_position.heading = json_data["heading"]; + M2_position.ID = drone_ID; + } + +} +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/requestData3.cpp b/class_model/src/requestData3.cpp new file mode 100755 index 0000000..5996de1 --- /dev/null +++ b/class_model/src/requestData3.cpp @@ -0,0 +1,148 @@ +#include"class_model/requestData3.h" +#include + + +int command; +// json j_data; //Don't set json as global variable + +RequestClass::RequestClass() : node_handle_("~"){ + + // 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", 10, + // &RequestClass::Data_callback, this); + + drone1_data = node_handle_.subscribe("/Dron650_Flight_Information_reciver", 10, + &RequestClass::drone1_callback, this); //test bionic + drone2_data = node_handle_.subscribe("/Dron550_Flight_Information_reciver", 10, + &RequestClass::drone2_callback, this); + drone3_data = node_handle_.subscribe("/Dron380_Flight_Information_reciver", 10, + &RequestClass::drone3_callback, this); +} + +RequestClass::~RequestClass() { ros::shutdown(); } + +void RequestClass::Data_callback(const std_msgs::String::ConstPtr &sensor) { + L_INFO(sensor->data); +} + +void RequestClass::Message_callback(const std_msgs::String::ConstPtr &message) { + + std::string data = message->data; + // jsonToString(data); + + +} + +global_location RequestClass::get_leader_GPS(){ + + return leader_position; +} + +float RequestClass::get_leader_heading(){ + + return heading; +} + +int RequestClass::get_formation_message(){ + + return command; +} + +void RequestClass::jsonToString(std::string data){ + +} + +void RequestClass::L_INFO(std::string data){ + + // std::cout <<"string: "<< data << std::endl; + + // document.Parse(data.c_str()); //china's library + // leader_position.lat=document["lat"].GetInt(); + // leader_position.lon=document["lon"].GetInt(); + // leader_position.alt=document["alt"].GetInt(); + // heading = document["heading"].GetInt(); + + // std::cout << document["lat"].GetInt() << std::endl; + // std::cout << document["lon"].GetInt() << std::endl; + // std::cout << document["alt"].GetInt() << std::endl; + // std::cout << document["heading"].GetInt() << std::endl; + + //********************************************// + j_data = json::parse(data); //open source + + // std::string lat = "",lon = "",alt = "",degree = ""; + // lat = j_data["lat"]; + // lon = j_data["lon"]; + // alt = j_data["alt"]; + // degree = j_data["heading"]; + + leader_position.lat = std::stoi(j_data["lat"].get()); + leader_position.lon = std::stoi(j_data["lon"].get()); + // leader_position.alt = std::stoi(j_data["alt"].get()); + heading = std::stoi(j_data["heading"].get()); + + // std::cout <<"Json: " <data); //open source + // std::cout <<"Json: " <data); //open source + // std::cout <<"Json: " <data); + Bio_INFO(j_data3,drone_ID); +} + +void RequestClass::Bio_INFO(json json_data,int drone_ID){ + + std::remove(ID_array.begin(),ID_array.end(),self_ID); + + if (drone_ID == self_ID){ + self_position.lat = json_data["lat"]; + self_position.lon = json_data["lon"]; + // self_position.alt = std::stoi(json_data["alt"].get()); + self_position.heading = json_data["heading"]; + self_position.ID = drone_ID; + }else if (drone_ID == ID_array[0]){ + M1_position.lat = json_data["lat"]; + M1_position.lon = json_data["lon"]; + // M1_position.alt = std::stoi(json_data["alt"].get()); + M1_position.heading = json_data["heading"]; + M1_position.ID = drone_ID; + }else if (drone_ID == ID_array[1]){ + M2_position.lat = json_data["lat"]; + M2_position.lon = json_data["lon"]; + // M2_position.alt = std::stoi(json_data["alt"].get()); + M2_position.heading = json_data["heading"]; + M2_position.ID = drone_ID; + } + +} +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/iq_sim/worlds/three_drone.world b/iq_sim/worlds/three_drone.world index 7c03902..d00b822 100644 --- a/iq_sim/worlds/three_drone.world +++ b/iq_sim/worlds/three_drone.world @@ -17,7 +17,7 @@ -1 - + 0.0 0.0 0.0 1.0 0 @@ -42,7 +42,7 @@ - +