diff --git a/class_model/.vscode/launch.json b/class_model/.vscode/launch.json index d87a6ba..b9471b3 100644 --- a/class_model/.vscode/launch.json +++ b/class_model/.vscode/launch.json @@ -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", diff --git a/class_model/.vscode/settings.json b/class_model/.vscode/settings.json index c50191d..d8bbb64 100644 --- a/class_model/.vscode/settings.json +++ b/class_model/.vscode/settings.json @@ -76,6 +76,7 @@ "valarray": "cpp", "any": "cpp", "codecvt": "cpp", - "forward_list": "cpp" + "forward_list": "cpp", + "shared_mutex": "cpp" } } \ No newline at end of file diff --git a/class_model/CMakeLists.txt b/class_model/CMakeLists.txt index ca39419..bfde0d3 100755 --- a/class_model/CMakeLists.txt +++ b/class_model/CMakeLists.txt @@ -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} ) diff --git a/class_model/include/class_model/DroneStatus.h b/class_model/include/class_model/DroneStatus.h index 274f089..86b80de 100644 --- a/class_model/include/class_model/DroneStatus.h +++ b/class_model/include/class_model/DroneStatus.h @@ -1,10 +1,14 @@ +#ifndef DRONESTATUS_H +#define DRONESTATUS_H + #include #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 diff --git a/class_model/include/class_model/Leader.h b/class_model/include/class_model/Leader.h new file mode 100644 index 0000000..434406f --- /dev/null +++ b/class_model/include/class_model/Leader.h @@ -0,0 +1,49 @@ +/*Follow the leader in a fixed formation */ +#ifndef Leader_H +#define Leader_H + +#include +#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 + +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 \ No newline at end of file diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h index 0817564..9fd5852 100755 --- a/class_model/include/class_model/follower.h +++ b/class_model/include/class_model/follower.h @@ -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 angle = config["formation"]["angle"].as(); + 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(); + }; diff --git a/class_model/include/class_model/requestData.h b/class_model/include/class_model/requestData.h index 13dcd7a..6856bc4 100755 --- a/class_model/include/class_model/requestData.h +++ b/class_model/include/class_model/requestData.h @@ -4,10 +4,14 @@ #include #include +#include #include #include #include #include +#include +#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 ID {1,2,3}; + std::array 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(),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 diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp index fc18bc5..4712e5c 100644 --- a/class_model/src/bio_main.cpp +++ b/class_model/src/bio_main.cpp @@ -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(); target.lat = config["routh1"]["y"].as(); - distance = config["formation"]["distance"].as(); - angle = config["formation"]["angle"].as(); + 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; } \ No newline at end of file diff --git a/class_model/src/bio_main2.cpp b/class_model/src/bio_main2.cpp new file mode 100644 index 0000000..67d0afa --- /dev/null +++ b/class_model/src/bio_main2.cpp @@ -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(); + target.lat = config["routh1"]["y"].as(); + + + 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; + +} \ No newline at end of file diff --git a/class_model/src/bio_main3.cpp b/class_model/src/bio_main3.cpp new file mode 100644 index 0000000..9af53cf --- /dev/null +++ b/class_model/src/bio_main3.cpp @@ -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(); + target.lat = config["routh1"]["y"].as(); + + + 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; + +} \ No newline at end of file diff --git a/class_model/src/config.yaml b/class_model/src/config.yaml index 01bfd98..d1cd7be 100644 --- a/class_model/src/config.yaml +++ b/class_model/src/config.yaml @@ -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 diff --git a/class_model/src/config2.yaml b/class_model/src/config2.yaml new file mode 100644 index 0000000..1462b45 --- /dev/null +++ b/class_model/src/config2.yaml @@ -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 + diff --git a/class_model/src/config3.yaml b/class_model/src/config3.yaml new file mode 100644 index 0000000..9d7b30d --- /dev/null +++ b/class_model/src/config3.yaml @@ -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 + diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp index c0ce1b7..2e9ac91 100755 --- a/class_model/src/follower.cpp +++ b/class_model/src/follower.cpp @@ -14,21 +14,26 @@ 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) { ref = &RequestClass::get_M2_GPS; } - - while(true){ - refpos = (request_object.*ref)(); + std::string command = ""; + std::string pre_command = receiver_object.check_command(); + while(true){ + 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" < _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){ + + refpos = (request_object.*ref)(); //get reference drone data + float deg_phi = refpos.heading/100; + + float heading = GPS_object.get_heading(); + + float phi = deg_phi*3.14/180; //degree-->radian + theta = theta*3.14/180; + + leader_location=request_object.get_leader_GPS(); //get Leader/Follower 's GPS + refpos = (request_object.*ref)(); + -// 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}; + 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; + 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 error_x = Pf[0] - (follower_location.lon*1e+5); -// float error_y = Pf[1] - (follower_location.lat*1e+5); - -// if (error_x < 0.3 & error_x > -0.3){ -// error_x = 0; -// } -// if (error_y < 0.3 & error_y > -0.3){ -// error_y = 0; -// } + 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); -// // 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("************************************"); + + // 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); + -// command_object.fix_velocity(error_x,error_y,0,0,0.1); -// // sleep(0.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); +} diff --git a/class_model/src/mqttPubMain2.py b/class_model/src/mqttPubMain2.py new file mode 100755 index 0000000..ca546bc --- /dev/null +++ b/class_model/src/mqttPubMain2.py @@ -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() + \ No newline at end of file diff --git a/class_model/src/mqttPubMain3.py b/class_model/src/mqttPubMain3.py new file mode 100755 index 0000000..e7f8c33 --- /dev/null +++ b/class_model/src/mqttPubMain3.py @@ -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() + \ No newline at end of file diff --git a/class_model/src/mqttSubMain2.py b/class_model/src/mqttSubMain2.py new file mode 100755 index 0000000..c544ae0 --- /dev/null +++ b/class_model/src/mqttSubMain2.py @@ -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) \ No newline at end of file diff --git a/class_model/src/mqttSubMain3.py b/class_model/src/mqttSubMain3.py new file mode 100755 index 0000000..331ba2f --- /dev/null +++ b/class_model/src/mqttSubMain3.py @@ -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) \ No newline at end of file diff --git a/class_model/src/requestData.cpp b/class_model/src/requestData.cpp index 0f12a3b..292a1a8 100755 --- a/class_model/src/requestData.cpp +++ b/class_model/src/requestData.cpp @@ -1,25 +1,25 @@ #include"class_model/requestData.h" #include -#include + 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); - - 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); + // 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(); } @@ -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()); + leader_position.lon = std::stoi(j_data["lon"].get()); + // leader_position.alt = std::stoi(j_data["alt"].get()); + heading = std::stoi(j_data["heading"].get()); - std::cout <<"Json: " <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: " <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: " <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){ - - j_data = json::parse(data); //open source +void RequestClass::Bio_INFO(json json_data,int drone_ID){ - 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()); + 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()); + 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()); + M2_position.heading = json_data["heading"]; } } diff --git a/class_model/src/test_lib.cpp b/class_model/src/test_lib.cpp index 81cae94..ca47d36 100644 --- a/class_model/src/test_lib.cpp +++ b/class_model/src/test_lib.cpp @@ -1,3 +1,6 @@ +#include +#include +#include #include #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:"<()<()<()<()< a {1,2,3}; - // remove(a.begin(),a.end(),b); - // std::cout << a[0]< a {1,2,3}; + remove(a.begin(),a.end(),b); + std::cout << a[0]<