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