gazebo test ok,but PID control weird.
parent
68c9756b4c
commit
01262ec88c
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
Loading…
Reference in New Issue