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

sub 3 drone work, but too slow

Browse Source
This commit is contained in:
Xuan0319
2023-03-03 00:10:00 +08:00
parent 58a2f3e7c1
commit 02bff86b0e
11 changed files with 272 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files
class_model/src/Leader.cpp
+123
View File
@@ -0,0 +1,123 @@
#include "class_model/Leader.h"
#include "class_model/convert_degree.h"
LeaderClass::LeaderClass() : node_handle_(""){
}
LeaderClass::~LeaderClass() { ros::shutdown(); }
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){
if(flag==0){
go2target(target.lon,target.lat);
}else{
// next_command.publish(message);
ROS_INFO("break");
break;
}
command = receiver_object.check_command();
while(command =="stop" || command == "land"){
// command_object.set_velocity(0,0,0,0,1);
// command = receiver_object.check_command();
if(command == "land"){
mode_object.set_Mode("LAND");
}else if(command != "stop"){
break;
}
}
if(command != pre_command){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void LeaderClass::go2target(float x,float y){
current_location=GPS_object.gps_position();
float target_heading,error_heading;
float heading = GPS_object.get_heading();
target_heading = ConvertDeg(x,y);
// float error_yaw = 0.2;
// error_heading = target_heading - heading;
// error_yaw = check_direction(error_heading)*error_yaw; //check yaw direction
while(heading_status != 1){
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);
// 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 );
if (drone_speed.x < ignore_small && drone_speed.x > -ignore_small){ //ignore small errors
drone_speed.x = 0;
}
if (drone_speed.y < ignore_small && drone_speed.y > -ignore_small){
drone_speed.y = 0;
}
if (drone_speed.x < -1){ //max velociy
drone_speed.x = -1;
}
if (drone_speed.x > 1){
drone_speed.x = 1;
}
if (drone_speed.y < -1){
drone_speed.y = -1;
}
if (drone_speed.y > 1){
drone_speed.y = 1;
}
if (drone_speed.x == 0 && drone_speed.y == 0){
flag = 1;
}
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("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("************************************");
}
void LeaderClass::face2target(float target_heading){
float error_heading;
float heading = GPS_object.get_heading();
float error_yaw = 0.2;
error_heading = target_heading - heading;
error_yaw = check_direction(error_heading)*error_yaw;
if (error_heading < 5 && error_heading > -5 ){
error_yaw = 0;
heading_status = 1;
}
command_object.set_velocity(0 ,0 ,0,error_yaw,0.1);
ROS_INFO("face2target...");
}
class_model/src/bio_main.cpp
+25 -16
View File
@@ -1,14 +1,13 @@
#include "class_model/mission.h"
#include "class_model/choose_leader.h"
#include "class_model/PubInfo.h"
#include "class_model/FindLeader.h" #include "class_model/FindLeader.h"
#include "class_model/Leader.h"
#include "class_model/follower.h"
#include "yaml-cpp/yaml.h" #include "yaml-cpp/yaml.h"
int main(int argc, char **argv) { int main(int argc, char **argv) {
// Init ROS node // Init ROS node
ros::init(argc, argv, "drone1_node"); ros::init(argc, argv, "drone1_node");
// reate Assync spiner // reate Assync spiner
ros::AsyncSpinner spinner(0); ros::AsyncSpinner spinner(0);
spinner.start(); spinner.start();
@@ -16,38 +15,48 @@ int main(int argc, char **argv) {
ModeClass mode_object; ModeClass mode_object;
ControlClass control_object; ControlClass control_object;
ReceiverClass receiver_object;
RequestClass request_object; RequestClass request_object;
MissionClass mission_object; FollowerClass follower_object;
CommandClass command_object; LeaderClass leader_object;
SelectClass select_formation;
PubInfoClass PubInfo_object;
// SelectionClass selection_object; // SelectionClass selection_object;
global_location target,self,M1,M2; 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) 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.lon = config["routh1"]["x"].as<float>();
target.lat = config["routh1"]["y"].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(); self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS(); M1 = request_object.get_M1_GPS();
M2 = request_object.get_M2_GPS(); M2 = request_object.get_M2_GPS();
global_location pos[] = {target,self,M1,M2}; global_location pos[] = {target,self,M1,M2};
global_location member[] = {M1,M2};
size_t index = sizeof(pos)/sizeof(pos[0]); size_t index = sizeof(pos)/sizeof(pos[0]);
size_t m_index = sizeof(member)/sizeof(member[0]);
if(check_leader(pos,index).is_leader == 1){
//fly2target
}else{
//find reference drone(pos,index,leader_ID);
}
mode_object.set_Mode("GUIDED"); mode_object.set_Mode("GUIDED");
control_object.arm(); control_object.arm();
control_object.takeoff(2); 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{
//follow reference drone position
}
}
} }
class_model/src/config.yaml
+1 -1
View File
@@ -6,5 +6,5 @@ routh1:
z: 0.0 z: 0.0
formation: formation:
distance: 4.0 distance: 4.0
angel: 45 angle: 45
class_model/src/follower.cpp
+27 -13
View File
@@ -2,16 +2,34 @@
FollowerClass::FollowerClass() : node_handle_(""){ FollowerClass::FollowerClass() : node_handle_(""){
std::string ros_namespace;
if (!node_handle_.hasParam("namespace"))
{
}else{
node_handle_.getParam("namespace", ros_namespace);
}
} }
FollowerClass::~FollowerClass() { ros::shutdown(); } FollowerClass::~FollowerClass() { ros::shutdown(); }
void FollowerClass::follower(){ void FollowerClass::follower(global_location data[], size_t index, int refID){
//plane() global_location (RequestClass::*ref)();
//follow() if(refID == data[0].ID){
ref = &RequestClass::get_M1_GPS;
}else if (refID == data[1].ID)
{
ref = &RequestClass::get_M2_GPS;
}
while(true){
refpos = (request_object.*ref)();
}
} }
void FollowerClass::plane(global_location target, global_location leader, global_location follower){ void FollowerClass::plane(global_location target, global_location leader, global_location follower){
@@ -69,7 +87,7 @@ void FollowerClass::plane(global_location target, global_location leader, global
} }
int direct(global_location target, global_location leader, DroneStatus self, DroneStatus member){ int FollowerClass::direct(global_location target, global_location leader, DroneStatus self, DroneStatus member){
global_location vector_LT,vector_SM; global_location vector_LT,vector_SM;
float m,n,k,cosTheta; float m,n,k,cosTheta;
@@ -90,7 +108,7 @@ int direct(global_location target, global_location leader, DroneStatus self, Dro
} }
void FollowerClass::find_ref_drone(global_location data[], size_t index, int leaderID){ int FollowerClass::find_ref_drone(global_location data[], size_t index, int leaderID){
for(int i=1 ; i<index ; i++){ //check leader drone for(int i=1 ; i<index ; i++){ //check leader drone
if(leaderID == data[i].ID){ if(leaderID == data[i].ID){
@@ -178,16 +196,12 @@ void FollowerClass::find_ref_drone(global_location data[], size_t index, int lea
check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end())); //get minimum value's index check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end())); //get minimum value's index
ref_ID = samePlane[check_index].ID; ref_ID = samePlane[check_index].ID;
std::cout <<"refID " << ref_ID << std::endl; std::cout <<"refID " << ref_ID << std::endl;
return ref_ID;
} }
void FollowerClass::get_location(){
// lon[0]
}
// void FollowerClass::calculate_position(float k,float theta){ // void FollowerClass::calculate_position(float k,float theta){
class_model/src/json.cpp
+9 -9
View File
@@ -11,13 +11,13 @@ void callBack(const std_msgs::String::ConstPtr &data){
json j_data = json::parse(data->data); json j_data = json::parse(data->data);
std::cout <<"original message"<< data->data << std::endl; std::cout <<"original message"<< data->data << std::endl;
std::cout <<"parse message"<< j_data << std::endl; // std::cout <<"parse message"<< j_data << std::endl;
std::cout <<"parse message"<< j_data["lat"] << std::endl; // std::cout <<"parse message"<< j_data["lat"] << std::endl;
std::string k = j_data["lat"] , q = j_data["lon"]; std::string k = j_data["lat"] , q = j_data["lon"];
int lat = std::stoi(k) ,lon = std::stoi(q); int lat = std::stoi(k) ,lon = std::stoi(q);
std::cout <<"lat:"<< k << std::endl; std::cout <<"lat:"<< j_data["lat"] << std::endl;
std::cout <<"lon:"<< q << std::endl; std::cout <<"lon:"<< j_data["lon"] << std::endl;
} }
@@ -37,15 +37,15 @@ int main(int argc ,char **argv)
ros::AsyncSpinner spinner(0); ros::AsyncSpinner spinner(0);
spinner.start(); spinner.start();
std::string rostopicName = "jsonData"; std::string rostopicName = "/uav_message";
ros::Publisher data_pub = json_node.advertise<std_msgs::String>(rostopicName,100); // ros::Publisher data_pub = json_node.advertise<std_msgs::String>(rostopicName,100);
ros::Subscriber data_receive = json_node.subscribe<std_msgs::String>(rostopicName,100,callBack); ros::Subscriber data_receive = json_node.subscribe<std_msgs::String>(rostopicName,100,callBack);
//ros::Subscriber data_receive1 = json_node.subscribe<std_msgs::String>("/uav_message",100,callBack); //ros::Subscriber data_receive1 = json_node.subscribe<std_msgs::String>("/uav_message",100,callBack);
mes.data = data; mes.data = data;
while(ros::ok()){ // while(ros::ok()){
data_pub.publish(mes); // data_pub.publish(mes);
} // }
// json j_no_init_list = json::array(); // json j_no_init_list = json::array();
// json j_empty_init_list = json::array({}); // json j_empty_init_list = json::array({});
// json j_nonempty_init_list = json::array({1, 2, 3, 4}); // json j_nonempty_init_list = json::array({1, 2, 3, 4});
class_model/src/mqttSubMain.py
-3
View File
@@ -38,9 +38,6 @@ def init_dataFormat(cfg:utils.Read_SUB_Config):
client.message_callback_add(cfg.Drone550_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone550_on_message_Flight_Information) 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.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) client.message_callback_add(cfg.Drone650_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone650_on_message_Flight_Information)
utils.Json_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
utils.Json_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
else: else:
logger.debug("msg_format not found") logger.debug("msg_format not found")
class_model/src/requestData.cpp
+54 -54
View File
@@ -7,19 +7,19 @@ int command;
RequestClass::RequestClass() : node_handle_(""){ RequestClass::RequestClass() : node_handle_(""){
mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100, // mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100,
&RequestClass::Data_callback, this); // &RequestClass::Data_callback, this);
formation_data = node_handle_.subscribe("/Fly_Formation_reciver", 10, formation_data = node_handle_.subscribe("/Fly_Formation_reciver", 10,
&RequestClass::Message_callback, this); &RequestClass::Message_callback, this);
// mqtt_data = node_handle_.subscribe("/uav_message", 100, mqtt_data = node_handle_.subscribe("/uav_message", 10,
// &RequestClass::Data_callback, this); &RequestClass::Data_callback, this);
// self_data = node_handle_.subscribe("/self_data_message", 100, self_data = node_handle_.subscribe("/self_data_message", 100,
// &RequestClass::self_callback, this); //test bionic &RequestClass::self_callback, this); //test bionic
// member1_data = node_handle_.subscribe("/member1_data_message", 100, member1_data = node_handle_.subscribe("/member1_data_message", 100,
// &RequestClass::M1_callback, this); &RequestClass::M1_callback, this);
// member2_data = node_handle_.subscribe("/member2_data_message", 100, member2_data = node_handle_.subscribe("/member2_data_message", 100,
// &RequestClass::M2_callback, this); &RequestClass::M2_callback, this);
} }
RequestClass::~RequestClass() { ros::shutdown(); } RequestClass::~RequestClass() { ros::shutdown(); }
@@ -79,12 +79,12 @@ void RequestClass::L_INFO(std::string data){
// alt = j_data["alt"]; // alt = j_data["alt"];
// degree = j_data["heading"]; // degree = j_data["heading"];
leader_position.lat=j_data["lat"]; // leader_position.lat=j_data["lat"]; //type error ,need number
leader_position.lon=j_data["lon"]; // leader_position.lon=j_data["lon"];
leader_position.alt=j_data["alt"]; // leader_position.alt=j_data["alt"];
heading = j_data["heading"]; // heading = j_data["heading"];
// 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.lat << std::endl;
// std::cout << leader_position.lon << std::endl; // std::cout << leader_position.lon << std::endl;
// std::cout << leader_position.alt << std::endl; // std::cout << leader_position.alt << std::endl;
@@ -92,50 +92,50 @@ void RequestClass::L_INFO(std::string data){
} }
/*test biomic*/ /*test biomic*/
// void RequestClass::self_callback(const std_msgs::String::ConstPtr &sensor) { void RequestClass::self_callback(const std_msgs::String::ConstPtr &sensor) {
// Bio_INFO(sensor->data); Bio_INFO(sensor->data);
// } }
// void RequestClass::M1_callback(const std_msgs::String::ConstPtr &sensor) { void RequestClass::M1_callback(const std_msgs::String::ConstPtr &sensor) {
// Bio_INFO(sensor->data); Bio_INFO(sensor->data);
// } }
// void RequestClass::M2_callback(const std_msgs::String::ConstPtr &sensor) { void RequestClass::M2_callback(const std_msgs::String::ConstPtr &sensor) {
// Bio_INFO(sensor->data); Bio_INFO(sensor->data);
// } }
// void RequestClass::Bio_INFO(std::string data){ void RequestClass::Bio_INFO(std::string data){
// j_data = json::parse(data); //open source j_data = json::parse(data); //open source
// drone_ID = j_data["ID"]; drone_ID = j_data["ID"];
// std::remove(ID.begin(),ID.end(),self_ID); std::remove(ID.begin(),ID.end(),self_ID);
// if (drone_ID == self_ID){ if (drone_ID == self_ID){
// self_position.lat=j_data["lat"]; self_position.lat=j_data["lat"];
// self_position.lon=j_data["lon"]; self_position.lon=j_data["lon"];
// self_position.alt=j_data["alt"]; self_position.alt=j_data["alt"];
// self_position.heading = j_data["heading"]; self_position.heading = j_data["heading"];
// }else if (drone_ID == ID[0]){ }else if (drone_ID == ID[0]){
// M1_position.lat=j_data["lat"]; M1_position.lat=j_data["lat"];
// M1_position.lon=j_data["lon"]; M1_position.lon=j_data["lon"];
// M1_position.alt=j_data["alt"]; M1_position.alt=j_data["alt"];
// M1_position.heading = j_data["heading"]; M1_position.heading = j_data["heading"];
// }else if (drone_ID == ID[1]){ }else if (drone_ID == ID[1]){
// M2_position.lat=j_data["lat"]; M2_position.lat=j_data["lat"];
// M2_position.lon=j_data["lon"]; M2_position.lon=j_data["lon"];
// M2_position.alt=j_data["alt"]; M2_position.alt=j_data["alt"];
// M2_position.heading = j_data["heading"]; M2_position.heading = j_data["heading"];
// } }
// } }
// global_location RequestClass::get_self_GPS(){ global_location RequestClass::get_self_GPS(){
// return self_position; return self_position;
// } }
// global_location RequestClass::get_M1_GPS(){ global_location RequestClass::get_M1_GPS(){
// return M1_position; return M1_position;
// } }
// global_location RequestClass::get_M2_GPS(){ global_location RequestClass::get_M2_GPS(){
// return M2_position; return M2_position;
// } }
class_model/src/test_lib.cpp
+21 -21
View File
@@ -7,12 +7,12 @@
using namespace std; using namespace std;
global_location leader_drone; global_location leader_drone;
DroneStatus memberDrone[3]; DroneStatus memberDrone[5];
DroneStatus drone1; DroneStatus drone1;
DroneStatus drone2; DroneStatus drone2;
DroneStatus drone3; DroneStatus drone3;
DroneStatus self; DroneStatus self;
DroneStatus samePlane[3]; DroneStatus samePlane[5];
global_location follower_location; global_location follower_location;
typedef struct result{ typedef struct result{
@@ -31,7 +31,7 @@ result check_leader(global_location data[], size_t index){
d[data[i].ID] = sqrt( pow((data[0].lat - data[i].lat),2) + pow((data[0].lon - data[i].lon),2) ); d[data[i].ID] = sqrt( pow((data[0].lat - data[i].lat),2) + pow((data[0].lon - data[i].lon),2) );
std::cout << i<<"," <<d[data[i].ID] <<std::endl; // std::cout << i<<"," <<d[data[i].ID] <<std::endl;
} }
std::vector<float> _d(d+1,d+index); std::vector<float> _d(d+1,d+index);
@@ -169,28 +169,28 @@ void find_ref_drone(global_location data[], size_t index, int leaderID){
}else{ }else{
memberDrone[j] = drone1; memberDrone[j] = drone1;
j+=1; j+=1;
std::cout << "drone1" <<std::endl; // std::cout << "drone1" <<std::endl;
} }
if(drone2.ID == data[1].ID){ if(drone2.ID == data[1].ID){
self = drone2; self = drone2;
}else{ }else{
memberDrone[j] = drone2; memberDrone[j] = drone2;
j+=1; j+=1;
std::cout << "drone2" <<std::endl; // std::cout << "drone2" <<std::endl;
} }
if(drone3.ID == data[1].ID){ if(drone3.ID == data[1].ID){
self = drone3; self = drone3;
}else{ }else{
memberDrone[j] = drone3; memberDrone[j] = drone3;
j+=1; j+=1;
std::cout << "drone3" <<std::endl; // std::cout << "drone3" <<std::endl;
} }
for(int i=0;i<index-2;i++){ for(int i=0;i<index-2;i++){
if(memberDrone[i].plane == self.plane || memberDrone[i].plane == 0){ //load same plane member if(memberDrone[i].plane == self.plane || memberDrone[i].plane == 0){ //load same plane member
samePlane[s_index] = memberDrone[i]; samePlane[s_index] = memberDrone[i];
std::cout << samePlane[s_index].init_location.lat <<std::endl; // std::cout << samePlane[s_index].init_location.lat <<std::endl;
std::cout << memberDrone[i].ID <<std::endl; // std::cout << memberDrone[i].ID <<std::endl;
s_index++; s_index++;
} }
@@ -200,14 +200,14 @@ void find_ref_drone(global_location data[], size_t index, int leaderID){
// int s_index = sizeof(samePlane)/sizeof(samePlane[0]); // int s_index = sizeof(samePlane)/sizeof(samePlane[0]);
float d[s_index]; float d[s_index];
int dir,check_index,ref_ID; 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 for(int i=0 ; i<s_index ; i++){ //caculate each same plane mamber's distance
dir = direct(data[0],leader_drone,self,samePlane[i]); 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) ); 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 << i<<"," <<d[i] <<std::endl;
std::cout << samePlane[i].ID <<std::endl; // std::cout << samePlane[i].ID <<std::endl;
} }
std::vector<float> _d(d,d+s_index); std::vector<float> _d(d,d+s_index);
@@ -239,21 +239,21 @@ int main(int argc,char** argv){
global_location M2; global_location M2;
global_location target; global_location target;
target.lon = 0; target.lon = 3;
target.lat = 0; target.lat = -3;
target.heading = 0; target.heading = 0;
self.lat = 2; self.lon = -2;
self.lon = 2; self.lat = -2;
self.ID = 1; self.ID = 3;
M1.lat = 1; M1.lon = 2;
M1.lon = 1; M1.lat = -3;
M1.ID = 2; M1.ID = 2;
M2.lat = 5; M2.lon = 0;
M2.lon = 5; M2.lat = 0;
M2.ID = 3; M2.ID = 1;
global_location data[]={target,self,M1,M2}; global_location data[]={target,self,M1,M2};
size_t index = sizeof(data)/sizeof(data[0]); size_t index = sizeof(data)/sizeof(data[0]);
class_model/src/utils/mqttConfig_PUB.yml
+4 -4
View File
@@ -1,6 +1,6 @@
MQTT: MQTT:
msg_format: Json msg_format: Json
MQTTClientNamePub: Drone550Pub MQTTClientNamePub: Drone380Pub
host: 140.120.31.133 host: 140.120.31.133
port: 1883 port: 1883
keepalive: 60 keepalive: 60
@@ -9,13 +9,13 @@ MQTT:
lwt: Dron550 Gone Offline lwt: Dron550 Gone Offline
willRetain: False willRetain: False
# Mqtt topic # Mqtt topic
Flight_Information_topicToMqtt: Drone550/Flight_Information Flight_Information_topicToMqtt: Drone380/Flight_Information
Fly_Formation_topicToMqtt: Drone550/Formation Fly_Formation_topicToMqtt: Drone380/Formation
# Change formate qos # Change formate qos
Fly_Formation_topicToMqtt_QOS: 2 Fly_Formation_topicToMqtt_QOS: 2
#ROS #ROS
ROS: ROS:
ROSClientNamePub: Drone550Pub ROSClientNamePub: Drone380Pub
ROStopicName_Flight_Information: Flight_Information_reciver ROStopicName_Flight_Information: Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG: LOG:
class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py
+1 -6
View File
@@ -15,9 +15,6 @@ class Proto_msg_to_ros:
publisher_Flight_Information = None publisher_Flight_Information = None
publisher_Fly_Formation = None publisher_Fly_Formation = None
#Mqtt topic: check data from which topic
Flight_Information_topicToMqtt = None
Fly_Formation_topicToMqtt = None
#Proto #Proto
@classmethod @classmethod
@@ -44,8 +41,6 @@ class Json_msg_to_ros:
Drone380_publisher_Flight_Information = None Drone380_publisher_Flight_Information = None
Drone650_publisher_Flight_Information = None Drone650_publisher_Flight_Information = None
#Mqtt topic: check data from which topic #Mqtt topic: check data from which topic
Flight_Information_topicToMqtt = None
Fly_Formation_topicToMqtt = None
@classmethod @classmethod
@@ -66,4 +61,4 @@ class Json_msg_to_ros:
@classmethod @classmethod
def Drone650_on_message_Flight_Information(cls, client, userdata, msg): def Drone650_on_message_Flight_Information(cls, client, userdata, msg):
cls.Drone650_publisher_Flight_Information.publish(msg.payload.decode("UTF-8")) cls.Drone650_publisher_Flight_Information.publish(msg.payload.decode("UTF-8"))
cls.rate.sleep() cls.rate.sleep()
class_model/src/utils/readConfig.py
+7 -2
View File
@@ -83,12 +83,17 @@ class Read_SUB_Config(Config):
"lwt":(str, True), "lwt":(str, True),
"willRetain":(bool,False), "willRetain":(bool,False),
"willTopicQOS":(int,True), "willTopicQOS":(int,True),
"Flight_Information_topicToMqtt": (str,False), "Drone550_Flight_Information_topicToMqtt": (str,False),
"Drone380_Flight_Information_topicToMqtt":(str,False),
"Drone650_Flight_Information_topicToMqtt":(str,False),
"Fly_Formation_topicToMqtt": (str,False), "Fly_Formation_topicToMqtt": (str,False),
"Fly_Formation_topicToMqtt_QOS":(int,False)}, "Fly_Formation_topicToMqtt_QOS":(int,False)},
self.sectionNames[1]:{ self.sectionNames[1]:{
"ROSClientNameSub": (str,False), "ROSClientNameSub": (str,False),
"ROStopicName_Flight_Information": (str,False), "Dron550_ROStopicName_Flight_Information": (str,False),
"Dron380_ROStopicName_Flight_Information": (str,False),
"Dron380_ROStopicName_Flight_Information": (str,False),
"Dron650_ROStopicName_Flight_Information": (str,False),
"ROStopicName_Fly_Formation": (str,False)}, "ROStopicName_Fly_Formation": (str,False)},
self.sectionNames[2]:{ self.sectionNames[2]:{
"logFileName":(str,False)}} "logFileName":(str,False)}}