Xuan0319/DroneControl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

gazebo test ok,but PID control weird.

Browse Source
This commit is contained in:
Xuan0319
2023-03-14 23:21:39 +08:00
parent 68c9756b4c
commit 01262ec88c
18 changed files with 558 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
class_model/.vscode/launch.json
Vendored
+1 -1
View File
@@ -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",
class_model/include/class_model/FindLeader.h
+1 -1
View File
@@ -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;
class_model/include/class_model/follower.h
+1 -1
View File
@@ -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];
class_model/include/class_model/requestData.h
+1 -2
View File
@@ -47,8 +47,7 @@ private:
std::array <int,3> 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<int>(),drone_ID;
int self_ID,drone_ID;
// SUBSCRIBE
ros::Subscriber mqtt_data;
class_model/include/class_model/requestData2.h
Executable
+62
View File
@@ -0,0 +1,62 @@
/*Subscribe Data Which MQTT Pubilsh */
#ifndef REQUESTDATA_H
#define REQUESTDATA_H
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <iostream>
#include <string>
#include <class_model/gps_location.h>
#include <nlohmann/json.hpp>
#include <algorithm>
#include <ros/message_event.h>
#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 <int,3> 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<int>(),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
class_model/include/class_model/requestData3.h
Executable
+62
View File
@@ -0,0 +1,62 @@
/*Subscribe Data Which MQTT Pubilsh */
#ifndef REQUESTDATA_H
#define REQUESTDATA_H
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <iostream>
#include <string>
#include <class_model/gps_location.h>
#include <nlohmann/json.hpp>
#include <algorithm>
#include <ros/message_event.h>
#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 <int,3> 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<int>(),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
class_model/src/Leader.cpp
+11 -10
View File
@@ -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);
}
class_model/src/bio_main.cpp
+12 -12
View File
@@ -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<float>();
target.lat = config["routh1"]["y"].as<float>();
target.lon = config["routh1"]["x"].as<float>()*1e7;
target.lat = config["routh1"]["y"].as<float>()*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();
class_model/src/bio_main2.cpp
+14 -12
View File
@@ -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<float>();
target.lat = config["routh1"]["y"].as<float>();
target.lon = config["routh1"]["x"].as<float>()*1e7;
target.lat = config["routh1"]["y"].as<float>()*1e7;
sleep(5);
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
M2 = request_object.get_M2_GPS();
std::cout <<M1.lat<<M1.ID<<std::endl;
global_location pos[] = {target,self,M1,M2};
global_location member[] = {M1,M2};
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 +50,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();
class_model/src/bio_main3.cpp
+12 -12
View File
@@ -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/config3.yaml"); //reading yaml parameter(target point)
target.lon = config["routh1"]["x"].as<float>();
target.lat = config["routh1"]["y"].as<float>();
target.lon = config["routh1"]["x"].as<float>()*1e7;
target.lat = config["routh1"]["y"].as<float>()*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();
class_model/src/config.yaml
+2 -2
View File
@@ -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
class_model/src/config2.yaml
+2 -2
View File
@@ -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
class_model/src/config3.yaml
+2 -2
View File
@@ -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
class_model/src/follower.cpp
+58 -56
View File
@@ -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,37 +42,18 @@ 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 <<std::endl;
if(follower.lat >= y){
if(follower.lat <= y){
// std::cout<< follower.lat <<","<< y <<std::endl;
switch (follower.ID){
case 1:
drone1.init_location.lat = follower.lat;
drone1.init_location.lon = follower.lon;
drone1.ID = follower.ID;
drone1.plane = -1; //right plane
break;
case 2:
drone2.init_location.lat = follower.lat;
drone2.init_location.lon = follower.lon;
drone2.ID = follower.ID;
drone2.plane = -1;
break;
case 3:
drone3.init_location.lat = follower.lat;
drone3.init_location.lon = follower.lon;
drone3.ID = follower.ID;
drone3.plane = -1;
break;
}
}else if(follower_location.lat < y){
switch (follower.ID){
case 1:
drone1.init_location.lat = follower.lat;
drone1.init_location.lon = follower.lon;
drone1.ID = follower.ID;
drone1.plane = 1; //left plane
drone1.plane = 1; //right plane
break;
case 2:
drone2.init_location.lat = follower.lat;
@@ -84,7 +65,29 @@ void FollowerClass::plane(global_location target, global_location leader, global
drone3.init_location.lat = follower.lat;
drone3.init_location.lon = follower.lon;
drone3.ID = follower.ID;
drone3.plane = 1;
drone3.plane = 1;
break;
}
}else if(follower.lat > y){
// std::cout<< follower.lat <<","<< y <<std::endl;
switch (follower.ID){
case 1:
drone1.init_location.lat = follower.lat;
drone1.init_location.lon = follower.lon;
drone1.ID = follower.ID;
drone1.plane = -1; //left plane
break;
case 2:
drone2.init_location.lat = follower.lat;
drone2.init_location.lon = follower.lon;
drone2.ID = follower.ID;
drone2.plane = -1;
break;
case 3:
drone3.init_location.lat = follower.lat;
drone3.init_location.lon = follower.lon;
drone3.ID = follower.ID;
drone3.plane = -1;
break;
}
}
@@ -211,19 +214,16 @@ int FollowerClass::find_ref_drone(global_location data[], size_t index, int lead
void FollowerClass::calculate_position(float k,float theta,int plane){
refpos = (request_object.*ref)(); //get reference drone data
refpos = (request_object.*ref)(); //get reference drone data(GPS+heading)
float deg_phi = refpos.heading/100;
follower_location=GPS_object.gps_position();
float heading = GPS_object.get_heading();
float phi = deg_phi*3.14/180; //degree-->radian
theta = theta*3.14/180;
theta = plane*theta*3.14/180;
leader_location=request_object.get_leader_GPS(); //get Leader/Follower 's GPS
refpos = (request_object.*ref)();
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;
// }
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);
// 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);
}
class_model/src/requestData.cpp
+18 -2
View File
@@ -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_ID <<","<< ID_array[0] <<ID_array[1]<<std::endl;
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<std::string>());
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<std::string>());
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<std::string>());
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(){
class_model/src/requestData2.cpp
Executable
+148
View File
@@ -0,0 +1,148 @@
#include"class_model/requestData2.h"
#include <cstdlib>
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<std::string>());
leader_position.lon = std::stoi(j_data["lon"].get<std::string>());
// leader_position.alt = std::stoi(j_data["alt"].get<std::string>());
heading = std::stoi(j_data["heading"].get<std::string>());
// 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;
// std::cout << heading << std::endl;
}
/*test biomic*/
void RequestClass::drone1_callback(const std_msgs::String::ConstPtr &sensor) {
drone_ID = 1;
j_data = json::parse(sensor->data); //open source
// std::cout <<"Json: " <<j_data["lat"] << std::endl;
Bio_INFO(j_data,drone_ID);
}
void RequestClass::drone2_callback(const std_msgs::String::ConstPtr &sensor) {
drone_ID = 2;
j_data2 = json::parse(sensor->data); //open source
// std::cout <<"Json: " <<j_data2["lat"] << std::endl;
Bio_INFO(j_data2,drone_ID);
}
void RequestClass::drone3_callback(const std_msgs::String::ConstPtr &sensor) {
drone_ID = 3;
j_data3 = json::parse(sensor->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<std::string>());
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<std::string>());
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<std::string>());
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;
}
class_model/src/requestData3.cpp
Executable
+148
View File
@@ -0,0 +1,148 @@
#include"class_model/requestData3.h"
#include <cstdlib>
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<std::string>());
leader_position.lon = std::stoi(j_data["lon"].get<std::string>());
// leader_position.alt = std::stoi(j_data["alt"].get<std::string>());
heading = std::stoi(j_data["heading"].get<std::string>());
// 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;
// std::cout << heading << std::endl;
}
/*test biomic*/
void RequestClass::drone1_callback(const std_msgs::String::ConstPtr &sensor) {
drone_ID = 1;
j_data = json::parse(sensor->data); //open source
// std::cout <<"Json: " <<j_data["lat"] << std::endl;
Bio_INFO(j_data,drone_ID);
}
void RequestClass::drone2_callback(const std_msgs::String::ConstPtr &sensor) {
drone_ID = 2;
j_data2 = json::parse(sensor->data); //open source
// std::cout <<"Json: " <<j_data2["lat"] << std::endl;
Bio_INFO(j_data2,drone_ID);
}
void RequestClass::drone3_callback(const std_msgs::String::ConstPtr &sensor) {
drone_ID = 3;
j_data3 = json::parse(sensor->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<std::string>());
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<std::string>());
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<std::string>());
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;
}