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

add bio drones into simulation environment(need debugging)

Browse Source
This commit is contained in:
Xuan0319
2023-03-08 23:33:03 +08:00
parent 81cb1c9043
commit 68c9756b4c
25 changed files with 966 additions and 141 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/testLib.launch",
"target": "/home/dodo/ardupilot_ws/src/class_model/launch/test.launch",
"launch": [
"rviz",
"gz",
class_model/.vscode/settings.json
Vendored
+2 -1
View File
@@ -76,6 +76,7 @@
"valarray": "cpp",
"any": "cpp",
"codecvt": "cpp",
"forward_list": "cpp"
"forward_list": "cpp",
"shared_mutex": "cpp"
}
}
class_model/CMakeLists.txt
+25 -1
View File
@@ -242,7 +242,7 @@ add_dependencies(receiver_lib main_generate_messages_cpp)
add_library(requestData_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/requestData.cpp)
target_include_directories(requestData_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(requestData_lib ${catkin_LIBRARIES})
target_link_libraries(requestData_lib ${catkin_LIBRARIES} /home/dodo/ardupilot_ws/src/class_model/libs/libyaml-cpp.a)
add_dependencies(requestData_lib main_generate_messages_cpp)
add_library(param_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/Param.cpp)
@@ -264,6 +264,7 @@ add_library(PubInfo_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/PubInfo.cpp)
target_include_directories(PubInfo_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(PubInfo_lib ${catkin_LIBRARIES})
add_dependencies(PubInfo_lib main_generate_messages_cpp)
##################
add_library(formation_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/formation.cpp)
@@ -281,11 +282,34 @@ target_include_directories(mission_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/includ
target_link_libraries(mission_lib ${catkin_LIBRARIES} select_lib)
add_dependencies(mission_lib main_generate_messages_cpp)
add_library(Leader_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/Leader.cpp)
target_include_directories(Leader_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(Leader_lib ${catkin_LIBRARIES} sensor_lib mode_lib control_lib command_lib requestData_lib PID_lib PubData_lib receiver_lib)
add_dependencies(Leader_lib main_generate_messages_cpp)
add_library(follower_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/follower.cpp)
target_include_directories(follower_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(follower_lib ${catkin_LIBRARIES} command_lib control_lib mode_lib requestData_lib sensor_lib PID_lib /home/dodo/ardupilot_ws/src/class_model/libs/libyaml-cpp.a)
add_dependencies(follower_lib main_generate_messages_cpp)
##################
add_executable(main src/main.cpp)
target_link_libraries(main ${catkin_LIBRARIES} mission_lib PubInfo_lib)
add_dependencies(main class_model_generate_messages_cpp)
add_executable(bio_main src/bio_main.cpp)
target_link_libraries(bio_main ${catkin_LIBRARIES} follower_lib Leader_lib /home/dodo/ardupilot_ws/src/class_model/libs/libyaml-cpp.a)
add_dependencies(bio_main class_model_generate_messages_cpp)
add_executable(bio_main2 src/bio_main2.cpp)
target_link_libraries(bio_main2 ${catkin_LIBRARIES} follower_lib Leader_lib /home/dodo/ardupilot_ws/src/class_model/libs/libyaml-cpp.a)
add_dependencies(bio_main2 class_model_generate_messages_cpp)
add_executable(bio_main3 src/bio_main3.cpp)
target_link_libraries(bio_main3 ${catkin_LIBRARIES} follower_lib Leader_lib /home/dodo/ardupilot_ws/src/class_model/libs/libyaml-cpp.a)
add_dependencies(bio_main3 class_model_generate_messages_cpp)
################json test
add_executable(json src/json.cpp)
target_link_libraries(json ${catkin_LIBRARIES} )
class_model/include/class_model/DroneStatus.h
+7 -3
View File
@@ -1,10 +1,14 @@
#ifndef DRONESTATUS_H
#define DRONESTATUS_H
#include <std_msgs/String.h>
#include "class_model/gps_location.h"
typedef struct DroneStatus
{
struct DroneStatus{
/* data */
global_location init_location;
int plane,ID;
std::string leader;
std::string DroneName;
} DroneStatus;
};
#endif // DRONESTATUS_H
class_model/include/class_model/Leader.h
+49
View File
@@ -0,0 +1,49 @@
/*Follow the leader in a fixed formation */
#ifndef Leader_H
#define Leader_H
#include <ros/ros.h>
#include "class_model/sensor.h"
#include "class_model/mode.h"
#include "class_model/receiver.h"
#include "class_model/control.h"
#include "class_model/command.h"
#include "class_model/requestData.h"
#include "class_model/PID.h"
#include "class_model/PubData.h"
#include <std_msgs/String.h>
class LeaderClass {
public:
LeaderClass();
virtual ~LeaderClass();
//ClassObject
ThreadGPSClass GPS_object;
ModeClass mode_object;
ReceiverClass receiver_object;
CommandClass command_object;
RequestClass request_object;
PubDataClass PubData_object;
PIDClass PID_x;
PIDClass PID_y;
void leader(global_location target);
void go2target(float x,float y);
private:
// ROS NodeHandle
ros::NodeHandle node_handle_;
global_location target_location;
global_location follower_location;
global_location leader_location;
global_location current_location;
velocity drone_speed;
int flag = 0,heading_status = 0,buf = 0;
float pre_alt,cur_alt;
float error_lon,error_lat;
float leader_pid[3] = {1 , 0.000000000001 ,0.5};
float ignore_small = 0.50;
void face2target(float target_heading);
};
#endif // Leader_H
class_model/include/class_model/follower.h
+22 -7
View File
@@ -7,8 +7,12 @@
#include "class_model/mode.h"
#include "class_model/control.h"
#include "class_model/command.h"
#include "class_model/receiver.h"
#include "class_model/requestData.h"
#include "class_model/DroneStatus.h"
#include "class_model/PID.h"
#include "class_model/convert_degree.h"
#include "yaml-cpp/yaml.h"
class FollowerClass {
@@ -19,15 +23,19 @@ public:
ThreadGPSClass GPS_object;
ModeClass mode_object;
ControlClass control_object;
ReceiverClass receiver_object;
CommandClass command_object;
RequestClass request_object;
PIDClass PID_x;
PIDClass PID_y;
DroneStatus drone1;
DroneStatus drone2;
DroneStatus drone3;
DroneStatus self;
DroneStatus samePlane[3];
DroneStatus samePlane[5];
void follower();
void follower(global_location data[], size_t index, int refID);
int find_ref_drone(global_location data[], size_t index, int leaderID);
float lon[3],lat[3];
@@ -40,15 +48,22 @@ private:
global_location follower_location;
global_location leader_location;
global_location leader_drone;
DroneStatus memberDrone[];
global_location refpos;
DroneStatus memberDrone[5];
global_location (RequestClass::*ref)();
void calculate_position(float k,float theta);
YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");
float distance = config["formation"]["distance"].as<float>();
float angle = config["formation"]["angle"].as<float>();
float error_lon,error_lat;
float follower_pid[3] = {1 , 0.00000000001 ,0.5};
float ignore_small = 0.50;
void calculate_position(float k,float theta,int plane);
void plane(global_location target, global_location leader, global_location follower);
void find_ref_drone(global_location data[], size_t index, int leaderID);
int direct(global_location target, global_location leader, DroneStatus self, DroneStatus member);
void choose_leader();
void follow();
void get_location();
};
class_model/include/class_model/requestData.h
+15 -11
View File
@@ -4,10 +4,14 @@
#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;
@@ -30,28 +34,28 @@ private:
ros::NodeHandle node_handle_;
void Data_callback(const std_msgs::String::ConstPtr &gps);
void self_callback(const std_msgs::String::ConstPtr &gps);
void M1_callback(const std_msgs::String::ConstPtr &gps);
void M2_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(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 {1,2,3};
std::array <int,3> ID_array {1,2,3};
// rapidjson::Document document;
json j_data;
int self_ID,drone_ID;
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;
// SUBSCRIBE
ros::Subscriber mqtt_data;
ros::Subscriber formation_data;
ros::Subscriber self_data;
ros::Subscriber member1_data;
ros::Subscriber member2_data;
ros::Subscriber drone1_data;
ros::Subscriber drone2_data;
ros::Subscriber drone3_data;
};
#endif // REQUESTDATA_H
class_model/src/bio_main.cpp
+11 -11
View File
@@ -22,14 +22,12 @@ int main(int argc, char **argv) {
// SelectionClass selection_object;
global_location target,self,M1,M2;
float distance,angle;
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>();
distance = config["formation"]["distance"].as<float>();
angle = config["formation"]["angle"].as<float>();
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
@@ -40,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){
@@ -51,12 +49,14 @@ int main(int argc, char **argv) {
while(true){
//
if(checkLeader == 1){
leader_object.leader(target);
}else{
//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();
return 0;
}
class_model/src/bio_main2.cpp
+62
View File
@@ -0,0 +1,62 @@
#include "class_model/FindLeader.h"
#include "class_model/Leader.h"
#include "class_model/follower.h"
#include "yaml-cpp/yaml.h"
int main(int argc, char **argv) {
// Init ROS node
ros::init(argc, argv, "drone2_node");
// reate Assync spiner
ros::AsyncSpinner spinner(0);
spinner.start();
// ros::Rate rate(5);
ModeClass mode_object;
ControlClass control_object;
RequestClass request_object;
FollowerClass follower_object;
LeaderClass leader_object;
// SelectionClass selection_object;
global_location target,self,M1,M2;
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>();
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
M2 = request_object.get_M2_GPS();
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);
checkLeader = check_leader(pos,index).is_leader;
if(checkLeader != 1){
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
}
while(true){
//
// if(checkLeader == 1){
// leader_object.leader(target);
// }else{
// follower_object.follower(member,m_index,ref_ID); //follow reference drone position
// }
}
ros::waitForShutdown();
return 0;
}
class_model/src/bio_main3.cpp
+62
View File
@@ -0,0 +1,62 @@
#include "class_model/FindLeader.h"
#include "class_model/Leader.h"
#include "class_model/follower.h"
#include "yaml-cpp/yaml.h"
int main(int argc, char **argv) {
// Init ROS node
ros::init(argc, argv, "drone3_node");
// reate Assync spiner
ros::AsyncSpinner spinner(0);
spinner.start();
// ros::Rate rate(5);
ModeClass mode_object;
ControlClass control_object;
RequestClass request_object;
FollowerClass follower_object;
LeaderClass leader_object;
// SelectionClass selection_object;
global_location target,self,M1,M2;
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>();
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
M2 = request_object.get_M2_GPS();
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);
checkLeader = check_leader(pos,index).is_leader;
if(checkLeader != 1){
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
}
while(true){
//
// if(checkLeader == 1){
// leader_object.leader(target);
// }else{
// follower_object.follower(member,m_index,ref_ID); //follow reference drone position
// }
}
ros::waitForShutdown();
return 0;
}
class_model/src/config.yaml
+2 -2
View File
@@ -1,8 +1,8 @@
DroneParam:
ID: 1
routh1:
x: 1.0
y: 1.5
x: 120.6743661 #(24.1218859, 120.6743161)
y: 24.1219359
z: 0.0
formation:
distance: 4.0
class_model/src/config2.yaml
+10
View File
@@ -0,0 +1,10 @@
DroneParam:
ID: 2
routh1:
x: 120.6743661 #(24.1218859, 120.6743161)
y: 24.1219359
z: 0.0
formation:
distance: 4.0
angle: 45
class_model/src/config3.yaml
+10
View File
@@ -0,0 +1,10 @@
DroneParam:
ID: 3
routh1:
x: 120.6743661 #(24.1218859, 120.6743161)
y: 24.1219359
z: 0.0
formation:
distance: 4.0
angle: 45
class_model/src/follower.cpp
+97 -44
View File
@@ -14,7 +14,6 @@ FollowerClass::~FollowerClass() { ros::shutdown(); }
void FollowerClass::follower(global_location data[], size_t index, int refID){
global_location (RequestClass::*ref)();
if(refID == data[0].ID){
ref = &RequestClass::get_M1_GPS;
}else if (refID == data[1].ID)
@@ -22,12 +21,18 @@ void FollowerClass::follower(global_location data[], size_t index, int refID){
ref = &RequestClass::get_M2_GPS;
}
std::string command = "";
std::string pre_command = receiver_object.check_command();
while(true){
refpos = (request_object.*ref)();
calculate_position(distance,angle,self.plane);
command = receiver_object.check_command();
if(command != pre_command ){
ROS_INFO("change formation");
break;
}
}
}
@@ -150,28 +155,28 @@ int FollowerClass::find_ref_drone(global_location data[], size_t index, int lead
}else{
memberDrone[j] = drone1;
j+=1;
std::cout << "drone1" <<std::endl;
// std::cout << "drone1" <<std::endl;
}
if(drone2.ID == data[1].ID){
self = drone2;
}else{
memberDrone[j] = drone2;
j+=1;
std::cout << "drone2" <<std::endl;
// std::cout << "drone2" <<std::endl;
}
if(drone3.ID == data[1].ID){
self = drone3;
}else{
memberDrone[j] = drone3;
j+=1;
std::cout << "drone3" <<std::endl;
// std::cout << "drone3" <<std::endl;
}
for(int i=0;i<index-2;i++){
if(memberDrone[i].plane == self.plane || memberDrone[i].plane == 0){ //load same plane member
samePlane[s_index] = memberDrone[i];
std::cout << samePlane[s_index].init_location.lat <<std::endl;
std::cout << memberDrone[i].ID <<std::endl;
// std::cout << samePlane[s_index].init_location.lat <<std::endl;
// std::cout << memberDrone[i].ID <<std::endl;
s_index++;
}
@@ -181,14 +186,14 @@ int FollowerClass::find_ref_drone(global_location data[], size_t index, int lead
// int s_index = sizeof(samePlane)/sizeof(samePlane[0]);
float d[s_index];
int dir,check_index,ref_ID;
std::cout <<"s_index " <<s_index <<std::endl;
// std::cout <<"s_index " <<s_index <<std::endl;
for(int i=0 ; i<s_index ; i++){ //caculate each same plane mamber's distance
dir = direct(data[0],leader_drone,self,samePlane[i]);
d[i] = dir * sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lon - samePlane[i].init_location.lon),2) );
std::cout << i<<"," <<d[i] <<std::endl;
std::cout << samePlane[i].ID <<std::endl;
// std::cout << i<<"," <<d[i] <<std::endl;
// std::cout << samePlane[i].ID <<std::endl;
}
std::vector<float> _d(d,d+s_index);
@@ -196,49 +201,97 @@ int FollowerClass::find_ref_drone(global_location data[], size_t index, int lead
check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end())); //get minimum value's index
ref_ID = samePlane[check_index].ID;
std::cout <<"refID " << ref_ID << std::endl;
std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " << ref_ID << std::endl;
return ref_ID;
}
// void FollowerClass::calculate_position(float k,float theta){
// theta = theta*3.14/180;
// float phi = request_object.get_leader_heading()/100;
// phi = phi*3.14/180;
// leader_location=request_object.get_leader_GPS();
// follower_location=GPS_object.gps_position();
void FollowerClass::calculate_position(float k,float theta,int plane){
// double Pf[]={},Pl[]={leader_location.lon,leader_location.lat};
// float transfer[2][2]={
// cos(phi),-sin(phi),
// sin(phi),cos(phi)
// };
// float Q[2]={k*sin(theta),k*cos(theta)};
// float T[2]={1,-1};
refpos = (request_object.*ref)(); //get reference drone data
float deg_phi = refpos.heading/100;
// 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;
float heading = GPS_object.get_heading();
// float error_x = Pf[0] - (follower_location.lon*1e+5);
// float error_y = Pf[1] - (follower_location.lat*1e+5);
float phi = deg_phi*3.14/180; //degree-->radian
theta = theta*3.14/180;
// if (error_x < 0.3 & error_x > -0.3){
// error_x = 0;
// }
// if (error_y < 0.3 & error_y > -0.3){
// error_y = 0;
// }
leader_location=request_object.get_leader_GPS(); //get Leader/Follower 's GPS
refpos = (request_object.*ref)();
// // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
// 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);
// ROS_INFO("************************************");
// command_object.fix_velocity(error_x,error_y,0,0,0.1);
// // sleep(0.5);
double Pf[]={},Pl[]={leader_location.lon,leader_location.lat}; //transfer maxtrix
float transfer[2][2]={
cos(phi),-sin(phi),
sin(phi),cos(phi)
};
float Q[2]={k*sin(theta),k*cos(theta)};
float T[2]={1,-1};
// }
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);
// 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);
float error_degree = deg_phi - heading + plane;
float error_yaw = 0.2; //CCW
if (error_degree >= 360){
error_degree -= 360;
}
if (error_degree <= -360){
error_degree += 360;
}
error_yaw = check_direction(error_degree)*error_yaw; //check yaw direction
if (error_lon < ignore_small && error_lon > -ignore_small){ //ignore small errors
error_lon = 0;
}
if (error_lat < ignore_small && error_lat > -ignore_small){
error_lat = 0;
}
if (error_degree < 5 && error_degree > -5 ){
error_yaw = 0;
}else if(error_degree >355 || error_degree < -355){
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);
// 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);
// ROS_INFO("%f,%f",error_lon,error_lat);
// // ROS_INFO("deg:%f",deg_phi);
// // ROS_INFO("heading:%f",heading);
// // 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);
}
class_model/src/mqttPubMain2.py
Executable
+103
View File
@@ -0,0 +1,103 @@
#!/usr/bin/env python3
#coding:utf-8
import paho.mqtt.client as mqtt
import os
import sys
import time
import utils
import proto.flight_information_pb2 as flight_information_pb2
import proto.flyformatioln_pb2 as flyformatioln_pb2
import logging
import rospy
from std_msgs.msg import String
from std_msgs.msg import Float64
from std_msgs.msg import Header
from mavros_msgs.srv import ParamGet
from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import Imu
from sensor_msgs.msg import Range
from geometry_msgs.msg import Vector3
def init_dataFormat(cfg:utils.Read_PUB_Config):
ros_namespace="/drone2"
if cfg.msg_format == "Proto":
utils.Proto_msg_from_ros.client = client
utils.Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()
utils.Proto_msg_from_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
utils.Proto_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Proto_msg_from_ros.callBack_gps)
rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Proto_msg_from_ros.callBack_compass_hdg)
elif cfg.msg_format == "Json":
utils.Json_msg_from_ros.client = client
utils.Json_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
utils.Json_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
utils.Json_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
else:
logging.debug("msg_format not found")
def on_connect(self, userdata, flags, rc):
logger.info("Connected with result code " + str(rc))
def on_publish(self, userdata, mid):
logger.debug("pub success")
def on_disconnect(client, userdata, rc):
logger.debug("disconnecting reason " +str(rc))
client.connected_flag=False
client.disconnect_flag=True
if __name__ == '__main__':
# Read Config
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB2.yml")
cfg = utils.Read_PUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNamePub)
# set log
log_format = "%(asctime)s - %(levelname)s - %(message)s"
formatter = logging.Formatter(log_format)
stream_handler = logging.StreamHandler()
stream_handler.setFormatter(formatter)
stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(cfg.logFileName)
file_handler.setFormatter(formatter)
file_handler.setLevel(logging.INFO)
logger = logging.getLogger(__name__)
logger.setLevel(logging.INFO)
logger.addHandler(file_handler)
logger.addHandler(stream_handler)
logger.info(cfg)
# Mqtt
client.on_connect = on_connect
client.on_publish = on_publish
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
client.loop_start()
# Ros
rosClient = cfg.ROSClientNamePub
rospy.init_node(rosClient)
# init data format
init_dataFormat(cfg)
try:
rospy.spin()
except BaseException as error:
client.disconnect()
logger.info("End of program")
sys.exit()
class_model/src/mqttPubMain3.py
Executable
+103
View File
@@ -0,0 +1,103 @@
#!/usr/bin/env python3
#coding:utf-8
import paho.mqtt.client as mqtt
import os
import sys
import time
import utils
import proto.flight_information_pb2 as flight_information_pb2
import proto.flyformatioln_pb2 as flyformatioln_pb2
import logging
import rospy
from std_msgs.msg import String
from std_msgs.msg import Float64
from std_msgs.msg import Header
from mavros_msgs.srv import ParamGet
from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import Imu
from sensor_msgs.msg import Range
from geometry_msgs.msg import Vector3
def init_dataFormat(cfg:utils.Read_PUB_Config):
ros_namespace="/drone3"
if cfg.msg_format == "Proto":
utils.Proto_msg_from_ros.client = client
utils.Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()
utils.Proto_msg_from_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
utils.Proto_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Proto_msg_from_ros.callBack_gps)
rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Proto_msg_from_ros.callBack_compass_hdg)
elif cfg.msg_format == "Json":
utils.Json_msg_from_ros.client = client
utils.Json_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
utils.Json_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
utils.Json_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
else:
logging.debug("msg_format not found")
def on_connect(self, userdata, flags, rc):
logger.info("Connected with result code " + str(rc))
def on_publish(self, userdata, mid):
logger.debug("pub success")
def on_disconnect(client, userdata, rc):
logger.debug("disconnecting reason " +str(rc))
client.connected_flag=False
client.disconnect_flag=True
if __name__ == '__main__':
# Read Config
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB3.yml")
cfg = utils.Read_PUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNamePub)
# set log
log_format = "%(asctime)s - %(levelname)s - %(message)s"
formatter = logging.Formatter(log_format)
stream_handler = logging.StreamHandler()
stream_handler.setFormatter(formatter)
stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(cfg.logFileName)
file_handler.setFormatter(formatter)
file_handler.setLevel(logging.INFO)
logger = logging.getLogger(__name__)
logger.setLevel(logging.INFO)
logger.addHandler(file_handler)
logger.addHandler(stream_handler)
logger.info(cfg)
# Mqtt
client.on_connect = on_connect
client.on_publish = on_publish
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
client.loop_start()
# Ros
rosClient = cfg.ROSClientNamePub
rospy.init_node(rosClient)
# init data format
init_dataFormat(cfg)
try:
rospy.spin()
except BaseException as error:
client.disconnect()
logger.info("End of program")
sys.exit()
class_model/src/mqttSubMain2.py
Executable
+107
View File
@@ -0,0 +1,107 @@
#!/usr/bin/env python3
#coding:utf-8
import paho.mqtt.client as mqtt
import os
import sys
import time
import utils
import proto.flight_information_pb2 as flight_information_pb2
import proto.flyformatioln_pb2 as flyformatioln_pb2
import rospy
from std_msgs.msg import String
import logging
def init_dataFormat(cfg:utils.Read_SUB_Config):
if cfg.msg_format == "Proto":
utils.Proto_msg_to_ros.rate = rospy.Rate(10)
utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
utils.Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
utils.Proto_msg_to_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
client.message_callback_add(cfg.Flight_Information_topicToMqtt, utils.Proto_msg_to_ros.on_message_Flight_Information)
client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Proto_msg_to_ros.on_message_Fly_Formation)
utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
elif cfg.msg_format == "Json":
# set ros publisher
utils.Json_msg_to_ros.rate = rospy.Rate(10)
utils.Json_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
utils.Json_msg_to_ros.Drone550_publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,String,queue_size=10)
utils.Json_msg_to_ros.Drone380_publisher_Flight_Information = rospy.Publisher(cfg.Dron380_ROStopicName_Flight_Information,String,queue_size=10)
utils.Json_msg_to_ros.Drone650_publisher_Flight_Information = rospy.Publisher(cfg.Dron650_ROStopicName_Flight_Information,String,queue_size=10)
# set callback to each topic
client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Json_msg_to_ros.on_message_Fly_Formation)
client.message_callback_add(cfg.Drone550_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone550_on_message_Flight_Information)
client.message_callback_add(cfg.Drone380_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone380_on_message_Flight_Information)
client.message_callback_add(cfg.Drone650_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone650_on_message_Flight_Information)
else:
logger.debug("msg_format not found")
def on_connect(self, userdata, flags, rc):
logger.info("Connected with result code " + str(rc))
client.subscribe(cfg.Fly_Formation_topicToMqtt)
client.subscribe(cfg.Drone550_Flight_Information_topicToMqtt)
client.subscribe(cfg.Drone380_Flight_Information_topicToMqtt)
client.subscribe(cfg.Drone650_Flight_Information_topicToMqtt)
def on_disconnect(client, userdata, rc):
logger.info("disconnecting reason " +str(rc))
client.connected_flag=False
client.disconnect_flag=True
def on_publish(self, userdata, mid):
pass
if __name__ == '__main__':
# Read Config
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_SUB2.yml")
cfg = utils.Read_SUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNameSub)
# set log
log_format = "%(asctime)s - %(levelname)s - %(message)s"
formatter = logging.Formatter(log_format)
stream_handler = logging.StreamHandler()
stream_handler.setFormatter(formatter)
stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(cfg.logFileName)
file_handler.setFormatter(formatter)
file_handler.setLevel(logging.INFO)
logger = logging.getLogger(__name__)
logger.setLevel(logging.DEBUG)
logger.addHandler(file_handler)
logger.addHandler(stream_handler)
logger.info(cfg)
# Mqtt
client = utils.MQTTClient(cfg.MQTTClientNameSub)
client.on_connect = on_connect
client.on_publish = on_publish
client.on_disconnect = on_disconnect
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
# Ros
rospy.init_node(cfg.ROSClientNameSub)
# initialize
init_dataFormat(cfg)
try:
client.loop_start()
rospy.spin()
except BaseException as error:
client.loop_stop()
client.disconnect()
logger.info("End of program")
sys.exit(0)
class_model/src/mqttSubMain3.py
Executable
+107
View File
@@ -0,0 +1,107 @@
#!/usr/bin/env python3
#coding:utf-8
import paho.mqtt.client as mqtt
import os
import sys
import time
import utils
import proto.flight_information_pb2 as flight_information_pb2
import proto.flyformatioln_pb2 as flyformatioln_pb2
import rospy
from std_msgs.msg import String
import logging
def init_dataFormat(cfg:utils.Read_SUB_Config):
if cfg.msg_format == "Proto":
utils.Proto_msg_to_ros.rate = rospy.Rate(10)
utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
utils.Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
utils.Proto_msg_to_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
client.message_callback_add(cfg.Flight_Information_topicToMqtt, utils.Proto_msg_to_ros.on_message_Flight_Information)
client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Proto_msg_to_ros.on_message_Fly_Formation)
utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
elif cfg.msg_format == "Json":
# set ros publisher
utils.Json_msg_to_ros.rate = rospy.Rate(10)
utils.Json_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
utils.Json_msg_to_ros.Drone550_publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,String,queue_size=10)
utils.Json_msg_to_ros.Drone380_publisher_Flight_Information = rospy.Publisher(cfg.Dron380_ROStopicName_Flight_Information,String,queue_size=10)
utils.Json_msg_to_ros.Drone650_publisher_Flight_Information = rospy.Publisher(cfg.Dron650_ROStopicName_Flight_Information,String,queue_size=10)
# set callback to each topic
client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Json_msg_to_ros.on_message_Fly_Formation)
client.message_callback_add(cfg.Drone550_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone550_on_message_Flight_Information)
client.message_callback_add(cfg.Drone380_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone380_on_message_Flight_Information)
client.message_callback_add(cfg.Drone650_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone650_on_message_Flight_Information)
else:
logger.debug("msg_format not found")
def on_connect(self, userdata, flags, rc):
logger.info("Connected with result code " + str(rc))
client.subscribe(cfg.Fly_Formation_topicToMqtt)
client.subscribe(cfg.Drone550_Flight_Information_topicToMqtt)
client.subscribe(cfg.Drone380_Flight_Information_topicToMqtt)
client.subscribe(cfg.Drone650_Flight_Information_topicToMqtt)
def on_disconnect(client, userdata, rc):
logger.info("disconnecting reason " +str(rc))
client.connected_flag=False
client.disconnect_flag=True
def on_publish(self, userdata, mid):
pass
if __name__ == '__main__':
# Read Config
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_SUB3.yml")
cfg = utils.Read_SUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNameSub)
# set log
log_format = "%(asctime)s - %(levelname)s - %(message)s"
formatter = logging.Formatter(log_format)
stream_handler = logging.StreamHandler()
stream_handler.setFormatter(formatter)
stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(cfg.logFileName)
file_handler.setFormatter(formatter)
file_handler.setLevel(logging.INFO)
logger = logging.getLogger(__name__)
logger.setLevel(logging.DEBUG)
logger.addHandler(file_handler)
logger.addHandler(stream_handler)
logger.info(cfg)
# Mqtt
client = utils.MQTTClient(cfg.MQTTClientNameSub)
client.on_connect = on_connect
client.on_publish = on_publish
client.on_disconnect = on_disconnect
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
# Ros
rospy.init_node(cfg.ROSClientNameSub)
# initialize
init_dataFormat(cfg)
try:
client.loop_start()
rospy.spin()
except BaseException as error:
client.loop_stop()
client.disconnect()
logger.info("End of program")
sys.exit(0)
class_model/src/requestData.cpp
+45 -40
View File
@@ -1,25 +1,25 @@
#include"class_model/requestData.h"
#include <cstdlib>
#include <iostream>
int command;
// json j_data; //Don't set json as global variable
RequestClass::RequestClass() : node_handle_(""){
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);
// mqtt_data = node_handle_.subscribe("/uav_message", 10,
// &RequestClass::Data_callback, this);
self_data = node_handle_.subscribe("/self_data_message", 100,
&RequestClass::self_callback, this); //test bionic
member1_data = node_handle_.subscribe("/member1_data_message", 100,
&RequestClass::M1_callback, this);
member2_data = node_handle_.subscribe("/member2_data_message", 100,
&RequestClass::M2_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(); }
@@ -79,12 +79,12 @@ void RequestClass::L_INFO(std::string data){
// alt = j_data["alt"];
// degree = j_data["heading"];
// leader_position.lat=j_data["lat"]; //type error ,need number
// leader_position.lon=j_data["lon"];
// leader_position.alt=j_data["alt"];
// heading = 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 <<"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;
@@ -92,38 +92,43 @@ void RequestClass::L_INFO(std::string data){
}
/*test biomic*/
void RequestClass::self_callback(const std_msgs::String::ConstPtr &sensor) {
Bio_INFO(sensor->data);
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::M1_callback(const std_msgs::String::ConstPtr &sensor) {
Bio_INFO(sensor->data);
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::M2_callback(const std_msgs::String::ConstPtr &sensor) {
Bio_INFO(sensor->data);
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(std::string data){
void RequestClass::Bio_INFO(json json_data,int drone_ID){
j_data = json::parse(data); //open source
drone_ID = j_data["ID"];
std::remove(ID.begin(),ID.end(),self_ID);
std::remove(ID_array.begin(),ID_array.end(),self_ID);
if (drone_ID == self_ID){
self_position.lat=j_data["lat"];
self_position.lon=j_data["lon"];
self_position.alt=j_data["alt"];
self_position.heading = j_data["heading"];
}else if (drone_ID == ID[0]){
M1_position.lat=j_data["lat"];
M1_position.lon=j_data["lon"];
M1_position.alt=j_data["alt"];
M1_position.heading = j_data["heading"];
}else if (drone_ID == ID[1]){
M2_position.lat=j_data["lat"];
M2_position.lon=j_data["lon"];
M2_position.alt=j_data["alt"];
M2_position.heading = j_data["heading"];
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"];
}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"];
}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"];
}
}
class_model/src/test_lib.cpp
+11 -9
View File
@@ -1,3 +1,6 @@
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Imu.h>
#include <iostream>
#include "yaml-cpp/yaml.h"
#include "class_model/gps_location.h"
@@ -220,19 +223,18 @@ void find_ref_drone(global_location data[], size_t index, int leaderID){
}
int main(int argc,char** argv){
// YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");
YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");
// cout <<"x:"<<config["routh1"]["x"].as<float>()<<endl;
// cout <<"y:"<<config["routh1"]["y"].as<float>()<<endl;
cout <<"x:"<<config["routh1"]["x"].as<float>()<<endl;
cout <<"y:"<<config["routh1"]["y"].as<float>()<<endl;
// int ID[3] = {1,2,3};
// int b = 1;
// std::array <int,3> a {1,2,3};
// remove(a.begin(),a.end(),b);
// std::cout << a[0]<<a[1] <<std::endl;
int ID[3] = {1,2,3};
int b = 1;
std::array <int,3> a {1,2,3};
remove(a.begin(),a.end(),b);
std::cout << a[0]<<a[1] <<std::endl;
global_location self;
global_location M1;
class_model/src/utils/mqttConfig_PUB.yml
+6 -6
View File
@@ -1,22 +1,22 @@
MQTT:
msg_format: Json
MQTTClientNamePub: Drone380Pub
MQTTClientNamePub: Drone650Pub
host: 140.120.31.133
port: 1883
keepalive: 60
willTopic: CheckDoneConnect
willTopicQOS: 1
lwt: Dron550 Gone Offline
lwt: Dron650 Gone Offline
willRetain: False
# Mqtt topic
Flight_Information_topicToMqtt: Drone380/Flight_Information
Fly_Formation_topicToMqtt: Drone380/Formation
Flight_Information_topicToMqtt: Drone650/Flight_Information
Fly_Formation_topicToMqtt: Drone650/Formation
# Change formate qos
Fly_Formation_topicToMqtt_QOS: 2
#ROS
ROS:
ROSClientNamePub: Drone380Pub
ROStopicName_Flight_Information: Flight_Information_reciver
ROSClientNamePub: Drone650Pub
ROStopicName_Flight_Information: Dron650_Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log
class_model/src/utils/mqttConfig_PUB2.yml
+22
View File
@@ -0,0 +1,22 @@
MQTT:
msg_format: Json
MQTTClientNamePub: Drone550Pub
host: 140.120.31.133
port: 1883
keepalive: 60
willTopic: CheckDoneConnect
willTopicQOS: 1
lwt: Drone550 Gone Offline
willRetain: False
# Mqtt topic
Flight_Information_topicToMqtt: Drone550/Flight_Information
Fly_Formation_topicToMqtt: Drone550/Formation
# Change formate qos
Fly_Formation_topicToMqtt_QOS: 2
#ROS
ROS:
ROSClientNamePub: Drone550Pub
ROStopicName_Flight_Information: Dron550_Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log
class_model/src/utils/mqttConfig_PUB3.yml
+22
View File
@@ -0,0 +1,22 @@
MQTT:
msg_format: Json
MQTTClientNamePub: Drone380Pub
host: 140.120.31.133
port: 1883
keepalive: 60
willTopic: CheckDoneConnect
willTopicQOS: 1
lwt: Dron380 Gone Offline
willRetain: False
# Mqtt topic
Flight_Information_topicToMqtt: Drone380/Flight_Information
Fly_Formation_topicToMqtt: Drone380/Formation
# Change formate qos
Fly_Formation_topicToMqtt_QOS: 2
#ROS
ROS:
ROSClientNamePub: Drone380Pub
ROStopicName_Flight_Information: Dron380_Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log
class_model/src/utils/mqttConfig_SUB2.yml
+30
View File
@@ -0,0 +1,30 @@
MQTT:
msg_format: Json
MQTTClientNameSub: Drone650Sub
host: 140.120.31.133
port: 1883
keepalive: 60
willTopic: CheckDoneConnect
willTopicQOS: 1
lwt: Dron550 Gone Offline
willRetain: False
# Mqtt topic
Fly_Formation_topicToMqtt: Drone650/Formation
Drone550_Flight_Information_topicToMqtt: Drone550/Flight_Information
Drone380_Flight_Information_topicToMqtt: Drone380/Flight_Information
Drone650_Flight_Information_topicToMqtt: Drone650/Flight_Information
# Change formate qos
Fly_Formation_topicToMqtt_QOS: 2
ROS:
ROSClientNameSub: Drone650Sub
ROStopicName_Fly_Formation: Fly_Formation_reciver
Dron550_ROStopicName_Flight_Information: Dron550_Flight_Information_reciver
Dron380_ROStopicName_Flight_Information: Dron380_Flight_Information_reciver
Dron650_ROStopicName_Flight_Information: Dron650_Flight_Information_reciver
LOG:
logFileName: sub.log
class_model/src/utils/mqttConfig_SUB3.yml
+30
View File
@@ -0,0 +1,30 @@
MQTT:
msg_format: Json
MQTTClientNameSub: Drone380Sub
host: 140.120.31.133
port: 1883
keepalive: 60
willTopic: CheckDoneConnect
willTopicQOS: 1
lwt: Dron550 Gone Offline
willRetain: False
# Mqtt topic
Fly_Formation_topicToMqtt: Drone380/Formation
Drone550_Flight_Information_topicToMqtt: Drone550/Flight_Information
Drone380_Flight_Information_topicToMqtt: Drone380/Flight_Information
Drone650_Flight_Information_topicToMqtt: Drone650/Flight_Information
# Change formate qos
Fly_Formation_topicToMqtt_QOS: 2
ROS:
ROSClientNameSub: Drone380Sub
ROStopicName_Fly_Formation: Fly_Formation_reciver
Dron550_ROStopicName_Flight_Information: Dron550_Flight_Information_reciver
Dron380_ROStopicName_Flight_Information: Dron380_Flight_Information_reciver
Dron650_ROStopicName_Flight_Information: Dron650_Flight_Information_reciver
LOG:
logFileName: sub.log