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

add calculate data timer test

Browse Source
protobuf
Xuan0319 3 years ago
parent 1c48cb94eb
commit ec4b10b723
27 changed files with 244 additions and 140 deletions
Show Stats Download Patch File Download Diff File

2
class_model/CMakeLists.txt
Unescape Escape View File

@ -232,7 +232,7 @@ add_dependencies(control_lib main_generate_messages_cpp)
add_library(command_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/command.cpp) add_library(command_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/command.cpp)
target_include_directories(command_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) target_include_directories(command_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(command_lib ${catkin_LIBRARIES}) target_link_libraries(command_lib ${catkin_LIBRARIES} param_lib)
add_dependencies(command_lib main_generate_messages_cpp) add_dependencies(command_lib main_generate_messages_cpp)
add_library(receiver_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/receiver.cpp) add_library(receiver_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/receiver.cpp)

13
class_model/include/class_model/command.h
Unescape Escape View File

@ -1,11 +1,16 @@
/*Drone Control Command*/ /*Drone Control Command*/
#ifndef COMMAND_H #ifndef COMMAND_H
#define COMMAND_H #define COMMAND_H
#define DEBUG
#include <ros/ros.h> #include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h> #include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/GlobalPositionTarget.h> #include <mavros_msgs/GlobalPositionTarget.h>
#include <class_model/sensor.h> #include <class_model/sensor.h>
#include <class_model/Param.h>
#include <fstream>
#include <iostream>
#include <string>
// #include <geographic_msgs/GeoPoseStamped.h> // #include <geographic_msgs/GeoPoseStamped.h>
class CommandClass { class CommandClass {
@ -13,7 +18,13 @@ public:
CommandClass(); CommandClass();
virtual ~CommandClass(); virtual ~CommandClass();
void set_global_position(double lon,double lat,float alt,float yaw); void set_global_position(double lon,double lat,float alt,float yaw);
#ifdef DEBUG
ParamClass param_object;
void set_velocity(float x,float y,float alt,float yaw,float second,uint64_t get_t,uint64_t cal_t);
#endif
#ifndef DEBUG
void set_velocity(float x,float y,float alt,float yaw,float second); void set_velocity(float x,float y,float alt,float yaw,float second);
#endif
void fix_velocity(float x,float y,float alt,float yaw,float second); void fix_velocity(float x,float y,float alt,float yaw,float second);
void velocity_init(); void velocity_init();
void set_target_position(float x,float y); void set_target_position(float x,float y);

2
class_model/include/class_model/convert_degree.h
Unescape Escape View File

@ -8,7 +8,7 @@ float ConvertDeg(float x ,float y){
degree = atan(y/x)*180/3.14 + 360; degree = atan(y/x)*180/3.14 + 360;
} }
if(x<0 && y<0){ if(x<0 && y<0){
degree = (atan(y/x)*180/3.14) + 180; degree = (atan(x/y)*180/3.14) + 180;
} }
if(x==0 && y>=0){ if(x==0 && y>=0){
degree = 0; degree = 0;

2
class_model/include/class_model/formation.h
Unescape Escape View File

@ -1,7 +1,7 @@
/*Follow the leader in a fixed formation */ /*Follow the leader in a fixed formation */
#ifndef FORMATION_H #ifndef FORMATION_H
#define FORMATION_H #define FORMATION_H
#define DEBUG
#include <ros/ros.h> #include <ros/ros.h>
#include "class_model/sensor.h" #include "class_model/sensor.h"
#include "class_model/mode.h" #include "class_model/mode.h"

4
class_model/src/Leader.cpp
Unescape Escape View File

@ -100,7 +100,7 @@ void LeaderClass::go2target(float target_lon,float target_lat){
} }
// PubData_object.publishData(drone_speed); // PubData_object.publishData(drone_speed);
command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1); // 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("slope:%f,%f,%f",slope,x,y);
// ROS_INFO("%f,%f",drone_speed.x,drone_speed.y); // ROS_INFO("%f,%f",drone_speed.x,drone_speed.y);
@ -125,6 +125,6 @@ void LeaderClass::face2target(float target_heading){
error_yaw = 0; error_yaw = 0;
heading_status = 1; heading_status = 1;
} }
command_object.set_velocity(0 ,0 ,0,error_yaw,0.1); // command_object.set_velocity(0 ,0 ,0,error_yaw,0.1);
// ROS_INFO("face2target...%f",target_heading); // ROS_INFO("face2target...%f",target_heading);
} }

23
class_model/src/bio_main.cpp
Unescape Escape View File

@ -73,21 +73,20 @@ int main(int argc, char **argv) {
M4 = request_object.get_M4_GPS(); M4 = request_object.get_M4_GPS();
if(checkLeader == 1){ if(checkLeader == 1){
target.lon = config["routh1"]["x"].as<float>()*1e7 ;
target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
leader_object.leader(target); leader_object.leader(target);
// for(int i=0;i<=360;i++){
// target.lon = config["routh1"]["x"].as<float>()*1e7 + 300*sin(i*0.2); target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
// target.lat = config["routh1"]["y"].as<float>()*1e7 + 300*cos(i*0.2); target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
// leader_object.leader(target);
// std::cout<< "radin: " << i*3.14/180 <<std::endl;
// }
target.lon = config["routh1"]["x"].as<float>()*1e7 + 500;
target.lat = config["routh1"]["y"].as<float>()*1e7 ;
leader_object.leader(target); leader_object.leader(target);
target.lon = config["routh1"]["x"].as<float>()*1e7 ;
target.lat = config["routh1"]["y"].as<float>()*1e7 -500; target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
leader_object.leader(target); leader_object.leader(target);
target.lon = config["routh1"]["x"].as<float>()*1e7 -500;
target.lat = config["routh1"]["y"].as<float>()*1e7 ; target.lon = config["routh1"]["x"].as<float>()*1e7 ;
target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
leader_object.leader(target); leader_object.leader(target);
}else{ }else{

2
class_model/src/bio_main2.cpp
Unescape Escape View File

@ -44,7 +44,7 @@ int main(int argc, char **argv) {
mode_object.set_Mode("GUIDED"); mode_object.set_Mode("GUIDED");
control_object.arm(); control_object.arm();
control_object.takeoff(2); control_object.takeoff(3);
// while(flag!=1){ // while(flag!=1){
// type = receiver_object.check_command(); // type = receiver_object.check_command();
// if(type == "go"){ // if(type == "go"){

2
class_model/src/bio_main3.cpp
Unescape Escape View File

@ -44,7 +44,7 @@ int main(int argc, char **argv) {
mode_object.set_Mode("GUIDED"); mode_object.set_Mode("GUIDED");
control_object.arm(); control_object.arm();
control_object.takeoff(2); control_object.takeoff(4);
// while(flag!=1){ // while(flag!=1){
// type = receiver_object.check_command(); // type = receiver_object.check_command();
// if(type == "go"){ // if(type == "go"){

2
class_model/src/bio_main4.cpp
Unescape Escape View File

@ -44,7 +44,7 @@ int main(int argc, char **argv) {
mode_object.set_Mode("GUIDED"); mode_object.set_Mode("GUIDED");
control_object.arm(); control_object.arm();
control_object.takeoff(2); control_object.takeoff(3);
// while(flag!=1){ // while(flag!=1){
// type = receiver_object.check_command(); // type = receiver_object.check_command();
// if(type == "go"){ // if(type == "go"){

2
class_model/src/bio_main5.cpp
Unescape Escape View File

@ -44,7 +44,7 @@ int main(int argc, char **argv) {
mode_object.set_Mode("GUIDED"); mode_object.set_Mode("GUIDED");
control_object.arm(); control_object.arm();
control_object.takeoff(2); control_object.takeoff(4);
// while(flag!=1){ // while(flag!=1){
// type = receiver_object.check_command(); // type = receiver_object.check_command();
// if(type == "go"){ // if(type == "go"){

62
class_model/src/command.cpp
Unescape Escape View File

@ -153,6 +153,67 @@ int CommandClass::fix_position(global_location pre_location,float x,float y,floa
return 1; return 1;
} }
} }
#ifdef DEBUG
void CommandClass::set_velocity(float x,float y,float alt,float yaw,float second,uint64_t get_t,uint64_t cal_t){
uint64_t fun_t = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
msg.twist.linear.x = x;
msg.twist.angular.x = x;
msg.twist.linear.y = y;
msg.twist.angular.y = y;
msg.twist.linear.z = alt;
msg.twist.angular.z = yaw;
// ROS_INFO("set_velocity: x:%f, y:%f, alt:%f, yaw:%f",x,y,alt,yaw);
// global_location pre_location=gps_object.gps_position();
int flag = 1;
double begin = ros::Time::now().toSec();
while(true){
velocity_command.publish(msg);
uint64_t pub_t = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
int ID = param_object.getID();
std::string path;
if(flag == 1){
switch (ID)
{
case 1:
path = "/home/dodo/Desktop/TimerTest/drone1.csv";
break;
case 2:
path = "/home/dodo/Desktop/TimerTest/drone2.csv";
break;
case 3:
path = "/home/dodo/Desktop/TimerTest/drone3.csv";
break;
}
std::fstream file(path,std::ios::app);
file << get_t << " " << cal_t << " " << fun_t <<" "<<pub_t<< "\n";
file.close();
flag = 0;
}
double now = ros::Time::now().toSec();
if((now-begin)>second){
velocity_init();
break;
}
// uint64_t last_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
// while(true){
// velocity_command.publish(msg);
// uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
// if((now_ms-last_ms)>second*1000){
// // ROS_INFO("%ld",now_ms-last_ms);
// //velocity_init();
// break;
// }
}
}
#endif
#ifndef DEBUG
void CommandClass::set_velocity(float x,float y,float alt,float yaw,float second){ void CommandClass::set_velocity(float x,float y,float alt,float yaw,float second){
msg.twist.linear.x = x; msg.twist.linear.x = x;
@ -186,6 +247,7 @@ void CommandClass::set_velocity(float x,float y,float alt,float yaw,float second
// } // }
} }
} }
#endif
void CommandClass::set_target_position(float x ,float y ){ void CommandClass::set_target_position(float x ,float y ){

19
class_model/src/drone2.cpp
Unescape Escape View File

@ -1,19 +0,0 @@
#include "class_model/follower.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();
FollowerClass follower_object;
follower_object.follower();
// RequestClass test_object;
ros::waitForShutdown();
return 0;
}

2
class_model/src/follower.cpp
Unescape Escape View File

@ -422,5 +422,5 @@ void FollowerClass::calculate_position(float k,float theta,int plane){
// ROS_INFO("drone%d,plane%d",self.ID,plane); // ROS_INFO("drone%d,plane%d",self.ID,plane);
command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.1); // command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.1);
} }

123
class_model/src/formation.cpp
Unescape Escape View File

@ -20,49 +20,49 @@ FormationClass::~FormationClass() { ros::shutdown(); }
void FormationClass::leader(){ void FormationClass::leader(){
int counter=0; // int counter=0;
std::string command = "",pre_command = ""; // std::string command = "",pre_command = "";
while(true){ // while(true){
if(flag==0){ // if(flag==0){
command_object.set_velocity(0,0,0,0.1,1); // command_object.set_velocity(0,0,0,0.1,1);
sleep(5); // sleep(5);
while(counter<=5){ // while(counter<=5){
command_object.set_velocity(-1,1,0,0,1); // command_object.set_velocity(-1,1,0,0,1);
command = receiver_object.check_command(); // command = receiver_object.check_command();
while(command =="stop" || command == "land"){ // while(command =="stop" || command == "land"){
command_object.set_velocity(0,0,0,0,1); // command_object.set_velocity(0,0,0,0,1);
command = receiver_object.check_command(); // command = receiver_object.check_command();
if(command == "land"){ // if(command == "land"){
mode_object.set_Mode("LAND"); // mode_object.set_Mode("LAND");
}else if(command != "stop"){ // }else if(command != "stop"){
break; // break;
} // }
} // }
counter++; // counter++;
std::cout << command <<std::endl; // std::cout << command <<std::endl;
} // }
if(counter>=5){ // if(counter>=5){
flag = 1; // flag = 1;
} // }
command = receiver_object.check_command(); // command = receiver_object.check_command();
// command_object.set_velocity(-1,1,0,0,10); // // command_object.set_velocity(-1,1,0,0,10);
// // sleep(5); // // // sleep(5);
// command_object.set_velocity(0,0,0,0,10); // // command_object.set_velocity(0,0,0,0,10);
// command_object.set_velocity(1,-1,0,0,10); // // command_object.set_velocity(1,-1,0,0,10);
// command_object.set_velocity(1,-1,0,0,10); // // command_object.set_velocity(1,-1,0,0,10);
// sleep(2); // // sleep(2);
} // }
if(command == "land"){ // if(command == "land"){
mode_object.set_Mode("LAND"); // mode_object.set_Mode("LAND");
ROS_INFO("xxxx"); // ROS_INFO("xxxx");
} // }
} // }
} }
void FormationClass::leader1(float x,float y){ void FormationClass::leader1(float x,float y){
@ -479,6 +479,10 @@ void FormationClass::calculate_position(float k,float theta,int direction){
theta = theta*3.14/180; theta = theta*3.14/180;
leader_location=request_object.get_leader_GPS(); //get Leader/Follower 's GPS leader_location=request_object.get_leader_GPS(); //get Leader/Follower 's GPS
#ifdef DEBUG
uint64_t get_data_t = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
#endif
follower_location=GPS_object.gps_position(); follower_location=GPS_object.gps_position();
@ -551,9 +555,17 @@ void FormationClass::calculate_position(float k,float theta,int direction){
// // ROS_INFO("error_degree:%f",error_degree); // // ROS_INFO("error_degree:%f",error_degree);
// // ROS_INFO("error_yaw:%f",error_yaw); // // ROS_INFO("error_yaw:%f",error_yaw);
// ROS_INFO("************************************"); // ROS_INFO("************************************");
#ifdef DEBUG
// command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.3); uint64_t cal_data_t = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
command_object.set_global_position(Pf[0]/1e5,Pf[1]/1e5,2,deg_phi); #endif
#ifndef DEBUG
command_object.set_velocity(error_lon ,error_lat ,0,0,0.1);
#endif
#ifdef DEBUG
command_object.set_velocity(error_lon ,error_lat ,0,0,0.1,get_data_t,cal_data_t);
#endif
// command_object.set_global_position(Pf[0]/1e5,Pf[1]/1e5,2,deg_phi);
// command_object.set_velocity(0,0,0,error_yaw,0.1); // command_object.set_velocity(0,0,0,error_yaw,0.1);
} }
@ -636,13 +648,17 @@ void FormationClass::spherical_coordinate(float k,float theta,float psi){
ROS_INFO("Q[2]:%f ,Pf[2]:%f ,error_z:%f",k*sin(psi),Pf[2],error_z); ROS_INFO("Q[2]:%f ,Pf[2]:%f ,error_z:%f",k*sin(psi),Pf[2],error_z);
// ROS_INFO("************************************"); // ROS_INFO("************************************");
command_object.set_velocity(error_x,error_y,error_z,error_yaw,0.1); // command_object.set_velocity(error_x,error_y,error_z,error_yaw,0.1);
} }
void FormationClass::go2target(float x,float y){ void FormationClass::go2target(float x,float y){
leader_location=request_object.get_leader_GPS(); leader_location=request_object.get_leader_GPS();
#ifdef DEBUG
uint64_t get_data_t = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
#endif
current_location=GPS_object.gps_position(); current_location=GPS_object.gps_position();
float target_heading,error_heading; float target_heading,error_heading;
@ -686,8 +702,17 @@ void FormationClass::go2target(float x,float y){
flag = 1; flag = 1;
} }
#ifdef DEBUG
uint64_t cal_data_t = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
#endif
PubData_object.publishData(drone_speed); PubData_object.publishData(drone_speed);
#ifndef DEBUG
command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1); command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1);
#endif
#ifdef DEBUG
command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1,get_data_t,cal_data_t);
#endif
// command_object.set_global_position(target_location.lon/1e5,target_location.lat/1e5,2,target_heading); // command_object.set_global_position(target_location.lon/1e5,target_location.lat/1e5,2,target_heading);
// ROS_INFO("slope:%f,%f,%f",slope,x,y); // ROS_INFO("slope:%f,%f,%f",slope,x,y);
@ -713,7 +738,13 @@ void FormationClass::face2target(float target_heading){
error_yaw = 0; error_yaw = 0;
heading_status = 1; heading_status = 1;
} }
#ifndef DEBUG
command_object.set_velocity(0 ,0 ,0,error_yaw,0.1); command_object.set_velocity(0 ,0 ,0,error_yaw,0.1);
#endif
#ifdef DEBUG
command_object.set_velocity(0 ,0 ,0,error_yaw,0.1,0,0);
#endif
ROS_INFO("face2target..."); ROS_INFO("face2target...");
} }

3
class_model/src/main.cpp
Unescape Escape View File

@ -58,7 +58,6 @@ int main(int argc, char **argv) {
// ROS_INFO("set"); // ROS_INFO("set");
} }
// TODO: switch case
if(type == "init"){ if(type == "init"){
select_formation.goose_formation(); select_formation.goose_formation();
ROS_INFO("init formation"); ROS_INFO("init formation");
@ -69,7 +68,7 @@ int main(int argc, char **argv) {
}else if(type == "row"){ }else if(type == "row"){
select_formation.row_formation(); select_formation.row_formation();
pre_type ="row"; pre_type ="row";
}else if(type == "v1"){ //PID error }else if(type == "v1"){
select_formation.goose_formation(0,5); select_formation.goose_formation(0,5);
pre_type ="v1"; pre_type ="v1";
}else if(type == "v2"){ }else if(type == "v2"){

12
class_model/src/mqttPubMain.py
Unescape Escape View File

@ -61,7 +61,6 @@ if __name__ == '__main__':
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB.yml") FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB.yml")
cfg = utils.Read_PUB_Config(FilePath) cfg = utils.Read_PUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNamePub) client = utils.MQTTClient(cfg.MQTTClientNamePub)
# set log # set log
stream_log_format = "%(asctime)s - %(levelname)s - %(message)s" stream_log_format = "%(asctime)s - %(levelname)s - %(message)s"
file_log_format = "%(message)s" file_log_format = "%(message)s"
@ -72,19 +71,22 @@ if __name__ == '__main__':
stream_handler.setFormatter(stream_formatter) stream_handler.setFormatter(stream_formatter)
stream_handler.setLevel(logging.DEBUG) stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(cfg.logFileName) logpath = os.path.join(os.path.dirname(__file__),"log", cfg.logFileName)
file_handler = logging.FileHandler(logpath, mode='w')
file_handler.setFormatter(file_formatter) file_handler.setFormatter(file_formatter)
file_handler.setLevel(logging.INFO) file_handler.setLevel(logging.INFO)
logger = logging.getLogger(__name__)
logger.setLevel(logging.INFO) logger = logging.getLogger("__mqttPubMain__")
logger.setLevel(logging.DEBUG)
logger.addHandler(file_handler) logger.addHandler(file_handler)
logger.addHandler(stream_handler) logger.addHandler(stream_handler)
logger.debug(cfg) logger.debug(cfg)
# Mqtt # Mqtt
client.on_connect = on_connect client.on_connect = on_connect
client.on_publish = on_publish # client.on_publish = on_publish
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain) client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
client.loop_start() client.loop_start()

23
class_model/src/mqttPubMain2.py
Unescape Escape View File

@ -61,28 +61,31 @@ if __name__ == '__main__':
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB2.yml") FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB2.yml")
cfg = utils.Read_PUB_Config(FilePath) cfg = utils.Read_PUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNamePub) client = utils.MQTTClient(cfg.MQTTClientNamePub)
# set log # set log
log_format = "%(asctime)s - %(levelname)s - %(message)s" stream_log_format = "%(asctime)s - %(levelname)s - %(message)s"
formatter = logging.Formatter(log_format) file_log_format = "%(message)s"
file_formatter = logging.Formatter(file_log_format)
stream_formatter = logging.Formatter(stream_log_format)
stream_handler = logging.StreamHandler() stream_handler = logging.StreamHandler()
stream_handler.setFormatter(formatter) stream_handler.setFormatter(stream_formatter)
stream_handler.setLevel(logging.DEBUG) stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(cfg.logFileName) logpath = os.path.join(os.path.dirname(__file__),"log", cfg.logFileName)
file_handler.setFormatter(formatter) file_handler = logging.FileHandler(logpath, mode='w')
file_handler.setFormatter(file_formatter)
file_handler.setLevel(logging.INFO) file_handler.setLevel(logging.INFO)
logger = logging.getLogger(__name__)
logger.setLevel(logging.INFO) logger = logging.getLogger("__mqttPubMain__")
logger.setLevel(logging.DEBUG)
logger.addHandler(file_handler) logger.addHandler(file_handler)
logger.addHandler(stream_handler) logger.addHandler(stream_handler)
logger.info(cfg) logger.debug(cfg)
# Mqtt # Mqtt
client.on_connect = on_connect client.on_connect = on_connect
client.on_publish = on_publish # client.on_publish = on_publish
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain) client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
client.loop_start() client.loop_start()

23
class_model/src/mqttPubMain3.py
Unescape Escape View File

@ -61,28 +61,31 @@ if __name__ == '__main__':
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB3.yml") FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB3.yml")
cfg = utils.Read_PUB_Config(FilePath) cfg = utils.Read_PUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNamePub) client = utils.MQTTClient(cfg.MQTTClientNamePub)
# set log # set log
log_format = "%(asctime)s - %(levelname)s - %(message)s" stream_log_format = "%(asctime)s - %(levelname)s - %(message)s"
formatter = logging.Formatter(log_format) file_log_format = "%(message)s"
file_formatter = logging.Formatter(file_log_format)
stream_formatter = logging.Formatter(stream_log_format)
stream_handler = logging.StreamHandler() stream_handler = logging.StreamHandler()
stream_handler.setFormatter(formatter) stream_handler.setFormatter(stream_formatter)
stream_handler.setLevel(logging.DEBUG) stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(cfg.logFileName) logpath = os.path.join(os.path.dirname(__file__),"log", cfg.logFileName)
file_handler.setFormatter(formatter) file_handler = logging.FileHandler(logpath, mode='w')
file_handler.setFormatter(file_formatter)
file_handler.setLevel(logging.INFO) file_handler.setLevel(logging.INFO)
logger = logging.getLogger(__name__)
logger.setLevel(logging.INFO) logger = logging.getLogger("__mqttPubMain__")
logger.setLevel(logging.DEBUG)
logger.addHandler(file_handler) logger.addHandler(file_handler)
logger.addHandler(stream_handler) logger.addHandler(stream_handler)
logger.info(cfg) logger.debug(cfg)
# Mqtt # Mqtt
client.on_connect = on_connect client.on_connect = on_connect
client.on_publish = on_publish # client.on_publish = on_publish
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain) client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
client.loop_start() client.loop_start()

23
class_model/src/mqttPubMain4.py
Unescape Escape View File

@ -61,28 +61,31 @@ if __name__ == '__main__':
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB4.yml") FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB4.yml")
cfg = utils.Read_PUB_Config(FilePath) cfg = utils.Read_PUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNamePub) client = utils.MQTTClient(cfg.MQTTClientNamePub)
# set log # set log
log_format = "%(asctime)s - %(levelname)s - %(message)s" stream_log_format = "%(asctime)s - %(levelname)s - %(message)s"
formatter = logging.Formatter(log_format) file_log_format = "%(message)s"
file_formatter = logging.Formatter(file_log_format)
stream_formatter = logging.Formatter(stream_log_format)
stream_handler = logging.StreamHandler() stream_handler = logging.StreamHandler()
stream_handler.setFormatter(formatter) stream_handler.setFormatter(stream_formatter)
stream_handler.setLevel(logging.DEBUG) stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(cfg.logFileName) logpath = os.path.join(os.path.dirname(__file__),"log", cfg.logFileName)
file_handler.setFormatter(formatter) file_handler = logging.FileHandler(logpath, mode='w')
file_handler.setFormatter(file_formatter)
file_handler.setLevel(logging.INFO) file_handler.setLevel(logging.INFO)
logger = logging.getLogger(__name__)
logger.setLevel(logging.INFO) logger = logging.getLogger("__mqttPubMain__")
logger.setLevel(logging.DEBUG)
logger.addHandler(file_handler) logger.addHandler(file_handler)
logger.addHandler(stream_handler) logger.addHandler(stream_handler)
logger.info(cfg) logger.debug(cfg)
# Mqtt # Mqtt
client.on_connect = on_connect client.on_connect = on_connect
client.on_publish = on_publish # client.on_publish = on_publish
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain) client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
client.loop_start() client.loop_start()

23
class_model/src/mqttPubMain5.py
Unescape Escape View File

@ -62,27 +62,30 @@ if __name__ == '__main__':
cfg = utils.Read_PUB_Config(FilePath) cfg = utils.Read_PUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNamePub) client = utils.MQTTClient(cfg.MQTTClientNamePub)
# set log # set log
log_format = "%(asctime)s - %(levelname)s - %(message)s" stream_log_format = "%(asctime)s - %(levelname)s - %(message)s"
formatter = logging.Formatter(log_format) file_log_format = "%(message)s"
file_formatter = logging.Formatter(file_log_format)
stream_formatter = logging.Formatter(stream_log_format)
stream_handler = logging.StreamHandler() stream_handler = logging.StreamHandler()
stream_handler.setFormatter(formatter) stream_handler.setFormatter(stream_formatter)
stream_handler.setLevel(logging.DEBUG) stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(cfg.logFileName) logpath = os.path.join(os.path.dirname(__file__),"log", cfg.logFileName)
file_handler.setFormatter(formatter) file_handler = logging.FileHandler(logpath, mode='w')
file_handler.setFormatter(file_formatter)
file_handler.setLevel(logging.INFO) file_handler.setLevel(logging.INFO)
logger = logging.getLogger(__name__)
logger.setLevel(logging.INFO) logger = logging.getLogger("__mqttPubMain__")
logger.setLevel(logging.DEBUG)
logger.addHandler(file_handler) logger.addHandler(file_handler)
logger.addHandler(stream_handler) logger.addHandler(stream_handler)
logger.info(cfg) logger.debug(cfg)
print(cfg.msg_format)
# Mqtt # Mqtt
client.on_connect = on_connect client.on_connect = on_connect
client.on_publish = on_publish # client.on_publish = on_publish
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain) client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
client.loop_start() client.loop_start()

3
class_model/src/mqttSubMain.py
Unescape Escape View File

@ -71,7 +71,8 @@ if __name__ == '__main__':
# Read Config # Read Config
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_SUB.yml") FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_SUB.yml")
cfg = utils.Read_SUB_Config(FilePath) cfg = utils.Read_SUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNameSub) print(cfg)
# client = utils.MQTTClient(cfg.MQTTClientNameSub)
# set log # set log
log_format = "%(asctime)s - %(levelname)s - %(message)s" log_format = "%(asctime)s - %(levelname)s - %(message)s"

2
class_model/src/utils/mqttConfig_PUB2.yml
Unescape Escape View File

@ -19,5 +19,5 @@ ROS:
ROStopicName_Flight_Information: Dron550_Flight_Information_reciver ROStopicName_Flight_Information: Dron550_Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG: LOG:
logFileName: pub.log logFileName: GPSDrone2.log
UAVLINK: "None" UAVLINK: "None"

2
class_model/src/utils/mqttConfig_PUB3.yml
Unescape Escape View File

@ -19,5 +19,5 @@ ROS:
ROStopicName_Flight_Information: Dron380_Flight_Information_reciver ROStopicName_Flight_Information: Dron380_Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG: LOG:
logFileName: pub.log logFileName: GPSDrone3.log
UAVLINK: "None" UAVLINK: "None"

2
class_model/src/utils/mqttConfig_PUB4.yml
Unescape Escape View File

@ -19,5 +19,5 @@ ROS:
ROStopicName_Flight_Information: Drone888_Flight_Information_reciver ROStopicName_Flight_Information: Drone888_Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG: LOG:
logFileName: pub.log logFileName: GPSDrone4.log
UAVLINK: "None" UAVLINK: "None"

2
class_model/src/utils/mqttConfig_PUB5.yml
Unescape Escape View File

@ -19,5 +19,5 @@ ROS:
ROStopicName_Flight_Information: Drone555_Flight_Information_reciver ROStopicName_Flight_Information: Drone555_Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG: LOG:
logFileName: pub.log logFileName: GPSDrone5.log
UAVLINK: "None" UAVLINK: "None"

3
class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py
Unescape Escape View File

@ -2,7 +2,7 @@ import orjson
import time import time
import logging import logging
logger = logging.getLogger(__name__) logger = logging.getLogger("__mqttPubMain__")
class Proto_msg_from_ros: class Proto_msg_from_ros:
#Protobuf #Protobuf
@ -113,4 +113,5 @@ class Json_msg_from_ros:
@classmethod @classmethod
def log(cls): def log(cls):
unit = 1e7 unit = 1e7
# print(cls.GPS_Data["lat"])
logger.info("{},{},{},{}".format(cls.GPS_Data["lat"]/1e7, cls.GPS_Data["lon"]/1e7, cls.GPS_Data["alt"]/100, cls.GPS_Data["heading"]/100)) logger.info("{},{},{},{}".format(cls.GPS_Data["lat"]/1e7, cls.GPS_Data["lon"]/1e7, cls.GPS_Data["alt"]/100, cls.GPS_Data["heading"]/100))

11
class_model/src/utils/readConfig.py
Unescape Escape View File

@ -29,7 +29,8 @@ class Config:
if self.options[opts][opt][1]: if self.options[opts][opt][1]:
raise Exception("Missing mandatory parameter {}".format(opt)) raise Exception("Missing mandatory parameter {}".format(opt))
else: else:
setattr(self,opt,None) pass
# setattr(self,opt,None)
def __str__(self): def __str__(self):
return str(yaml.dump(self.ymlcfg, default_flow_style=False)) return str(yaml.dump(self.ymlcfg, default_flow_style=False))
@ -111,6 +112,10 @@ class Read_SUB_Config(Config):
"uav_id": (str,False), "uav_id": (str,False),
"baudrate": (int,False), "baudrate": (int,False),
"ttyport": (str,False)}} "ttyport": (str,False)}}
self.setAttribute()
def __str__(self):
return super().__str__()
class Read_CMD_Config(Config): class Read_CMD_Config(Config):
def setAttribute(self): def setAttribute(self):
@ -141,7 +146,7 @@ class Read_CMD_Config(Config):
return super().__str__() return super().__str__()
if __name__ == "__main__": if __name__ == "__main__":
cfg=Read_CMD_Config("uavlinkConfig_SUB.yml") cfg=Read_SUB_Config("mqttConfig_SUB.yml")
print(cfg) print(cfg.msg_format)

Write Preview
Loading…
Cancel
Save