You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
63 lines
1.8 KiB
C++
63 lines
1.8 KiB
C++
/*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
|
|
|