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

add calculate data timer test

Browse Source
This commit is contained in:
Xuan0319
2023-05-02 00:56:54 +08:00
parent 1c48cb94eb
commit ec4b10b723
27 changed files with 244 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files
class_model/CMakeLists.txt
+1 -1
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)
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_library(receiver_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/receiver.cpp)
class_model/include/class_model/command.h
+12 -1
View File
@@ -1,11 +1,16 @@
/*Drone Control Command*/
#ifndef COMMAND_H
#define COMMAND_H
#define DEBUG
#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/GlobalPositionTarget.h>
#include <class_model/sensor.h>
#include <class_model/Param.h>
#include <fstream>
#include <iostream>
#include <string>
// #include <geographic_msgs/GeoPoseStamped.h>
class CommandClass {
@@ -13,7 +18,13 @@ public:
CommandClass();
virtual ~CommandClass();
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);
#endif
void fix_velocity(float x,float y,float alt,float yaw,float second);
void velocity_init();
void set_target_position(float x,float y);
class_model/include/class_model/convert_degree.h
+1 -1
View File
@@ -8,7 +8,7 @@ float ConvertDeg(float x ,float y){
degree = atan(y/x)*180/3.14 + 360;
}
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){
degree = 0;
class_model/include/class_model/formation.h
+1 -1
View File
@@ -1,7 +1,7 @@
/*Follow the leader in a fixed formation */
#ifndef FORMATION_H
#define FORMATION_H
#define DEBUG
#include <ros/ros.h>
#include "class_model/sensor.h"
#include "class_model/mode.h"
class_model/src/Leader.cpp
+2 -2
View File
@@ -100,7 +100,7 @@ void LeaderClass::go2target(float target_lon,float target_lat){
}
// 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("%f,%f",drone_speed.x,drone_speed.y);
@@ -125,6 +125,6 @@ void LeaderClass::face2target(float target_heading){
error_yaw = 0;
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);
}
class_model/src/bio_main.cpp
+11 -12
View File
@@ -73,21 +73,20 @@ int main(int argc, char **argv) {
M4 = request_object.get_M4_GPS();
if(checkLeader == 1){
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.lat = config["routh1"]["y"].as<float>()*1e7 + 300*cos(i*0.2);
// 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);
target.lon = config["routh1"]["x"].as<float>()*1e7 ;
target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
leader_object.leader(target);
target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
target.lat = config["routh1"]["y"].as<float>()*1e7 + 500;
leader_object.leader(target);
target.lon = config["routh1"]["x"].as<float>()*1e7 + 1000;
target.lat = config["routh1"]["y"].as<float>()*1e7 - 500;
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);
}else{
class_model/src/bio_main2.cpp
+1 -1
View File
@@ -44,7 +44,7 @@ int main(int argc, char **argv) {
mode_object.set_Mode("GUIDED");
control_object.arm();
control_object.takeoff(2);
control_object.takeoff(3);
// while(flag!=1){
// type = receiver_object.check_command();
// if(type == "go"){
class_model/src/bio_main3.cpp
+1 -1
View File
@@ -44,7 +44,7 @@ int main(int argc, char **argv) {
mode_object.set_Mode("GUIDED");
control_object.arm();
control_object.takeoff(2);
control_object.takeoff(4);
// while(flag!=1){
// type = receiver_object.check_command();
// if(type == "go"){
class_model/src/bio_main4.cpp
+1 -1
View File
@@ -44,7 +44,7 @@ int main(int argc, char **argv) {
mode_object.set_Mode("GUIDED");
control_object.arm();
control_object.takeoff(2);
control_object.takeoff(3);
// while(flag!=1){
// type = receiver_object.check_command();
// if(type == "go"){
class_model/src/bio_main5.cpp
+1 -1
View File
@@ -44,7 +44,7 @@ int main(int argc, char **argv) {
mode_object.set_Mode("GUIDED");
control_object.arm();
control_object.takeoff(2);
control_object.takeoff(4);
// while(flag!=1){
// type = receiver_object.check_command();
// if(type == "go"){
class_model/src/command.cpp
+62
View File
@@ -153,6 +153,67 @@ int CommandClass::fix_position(global_location pre_location,float x,float y,floa
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){
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 ){
class_model/src/drone2.cpp
-19
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;
}
class_model/src/follower.cpp
+1 -1
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);
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);
}
class_model/src/formation.cpp
+71 -40
View File
@@ -20,49 +20,49 @@ FormationClass::~FormationClass() { ros::shutdown(); }
void FormationClass::leader(){
int counter=0;
std::string command = "",pre_command = "";
// int counter=0;
// std::string command = "",pre_command = "";
while(true){
if(flag==0){
command_object.set_velocity(0,0,0,0.1,1);
sleep(5);
while(counter<=5){
command_object.set_velocity(-1,1,0,0,1);
command = receiver_object.check_command();
// while(true){
// if(flag==0){
// command_object.set_velocity(0,0,0,0.1,1);
// sleep(5);
// while(counter<=5){
// command_object.set_velocity(-1,1,0,0,1);
// command = receiver_object.check_command();
while(command =="stop" || command == "land"){
command_object.set_velocity(0,0,0,0,1);
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 == "land"){
// mode_object.set_Mode("LAND");
// }else if(command != "stop"){
// break;
// }
}
// }
counter++;
std::cout << command <<std::endl;
}
if(counter>=5){
flag = 1;
}
command = receiver_object.check_command();
// command_object.set_velocity(-1,1,0,0,10);
// // sleep(5);
// 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);
// sleep(2);
}
if(command == "land"){
mode_object.set_Mode("LAND");
ROS_INFO("xxxx");
}
// counter++;
// std::cout << command <<std::endl;
// }
// if(counter>=5){
// flag = 1;
// }
// command = receiver_object.check_command();
// // command_object.set_velocity(-1,1,0,0,10);
// // // sleep(5);
// // 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);
// // sleep(2);
// }
// if(command == "land"){
// mode_object.set_Mode("LAND");
// ROS_INFO("xxxx");
// }
}
// }
}
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;
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();
@@ -551,9 +555,17 @@ void FormationClass::calculate_position(float k,float theta,int direction){
// // ROS_INFO("error_degree:%f",error_degree);
// // ROS_INFO("error_yaw:%f",error_yaw);
// ROS_INFO("************************************");
#ifdef DEBUG
uint64_t cal_data_t = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
#endif
// command_object.set_velocity(error_lon,error_lat,0,error_yaw,0.3);
command_object.set_global_position(Pf[0]/1e5,Pf[1]/1e5,2,deg_phi);
#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);
}
@@ -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("************************************");
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){
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();
float target_heading,error_heading;
@@ -686,8 +702,17 @@ void FormationClass::go2target(float x,float y){
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);
#ifndef DEBUG
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);
// ROS_INFO("slope:%f,%f,%f",slope,x,y);
@@ -713,7 +738,13 @@ void FormationClass::face2target(float target_heading){
error_yaw = 0;
heading_status = 1;
}
#ifndef DEBUG
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...");
}
class_model/src/main.cpp
+1 -2
View File
@@ -58,7 +58,6 @@ int main(int argc, char **argv) {
// ROS_INFO("set");
}
// TODO: switch case
if(type == "init"){
select_formation.goose_formation();
ROS_INFO("init formation");
@@ -69,7 +68,7 @@ int main(int argc, char **argv) {
}else if(type == "row"){
select_formation.row_formation();
pre_type ="row";
}else if(type == "v1"){ //PID error
}else if(type == "v1"){
select_formation.goose_formation(0,5);
pre_type ="v1";
}else if(type == "v2"){
class_model/src/mqttPubMain.py
+7 -5
View File
@@ -61,7 +61,6 @@ if __name__ == '__main__':
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB.yml")
cfg = utils.Read_PUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNamePub)
# set log
stream_log_format = "%(asctime)s - %(levelname)s - %(message)s"
file_log_format = "%(message)s"
@@ -72,19 +71,22 @@ if __name__ == '__main__':
stream_handler.setFormatter(stream_formatter)
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.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(stream_handler)
logger.debug(cfg)
# Mqtt
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.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
client.loop_start()
class_model/src/mqttPubMain2.py
+13 -10
View File
@@ -61,28 +61,31 @@ if __name__ == '__main__':
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_log_format = "%(asctime)s - %(levelname)s - %(message)s"
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.setFormatter(formatter)
stream_handler.setFormatter(stream_formatter)
stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(cfg.logFileName)
file_handler.setFormatter(formatter)
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.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(stream_handler)
logger.info(cfg)
logger.debug(cfg)
# Mqtt
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.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
client.loop_start()
class_model/src/mqttPubMain3.py
+13 -10
View File
@@ -61,28 +61,31 @@ if __name__ == '__main__':
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_log_format = "%(asctime)s - %(levelname)s - %(message)s"
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.setFormatter(formatter)
stream_handler.setFormatter(stream_formatter)
stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(cfg.logFileName)
file_handler.setFormatter(formatter)
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.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(stream_handler)
logger.info(cfg)
logger.debug(cfg)
# Mqtt
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.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
client.loop_start()
class_model/src/mqttPubMain4.py
+13 -10
View File
@@ -61,28 +61,31 @@ if __name__ == '__main__':
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB4.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_log_format = "%(asctime)s - %(levelname)s - %(message)s"
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.setFormatter(formatter)
stream_handler.setFormatter(stream_formatter)
stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(cfg.logFileName)
file_handler.setFormatter(formatter)
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.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(stream_handler)
logger.info(cfg)
logger.debug(cfg)
# Mqtt
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.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
client.loop_start()
class_model/src/mqttPubMain5.py
+13 -10
View File
@@ -62,27 +62,30 @@ if __name__ == '__main__':
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_log_format = "%(asctime)s - %(levelname)s - %(message)s"
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.setFormatter(formatter)
stream_handler.setFormatter(stream_formatter)
stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(cfg.logFileName)
file_handler.setFormatter(formatter)
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.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(stream_handler)
logger.info(cfg)
print(cfg.msg_format)
logger.debug(cfg)
# Mqtt
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.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
client.loop_start()
class_model/src/mqttSubMain.py
+2 -1
View File
@@ -71,7 +71,8 @@ if __name__ == '__main__':
# Read Config
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_SUB.yml")
cfg = utils.Read_SUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNameSub)
print(cfg)
# client = utils.MQTTClient(cfg.MQTTClientNameSub)
# set log
log_format = "%(asctime)s - %(levelname)s - %(message)s"
class_model/src/utils/mqttConfig_PUB2.yml
+1 -1
View File
@@ -19,5 +19,5 @@ ROS:
ROStopicName_Flight_Information: Dron550_Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log
logFileName: GPSDrone2.log
UAVLINK: "None"
class_model/src/utils/mqttConfig_PUB3.yml
+1 -1
View File
@@ -19,5 +19,5 @@ ROS:
ROStopicName_Flight_Information: Dron380_Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log
logFileName: GPSDrone3.log
UAVLINK: "None"
class_model/src/utils/mqttConfig_PUB4.yml
+1 -1
View File
@@ -19,5 +19,5 @@ ROS:
ROStopicName_Flight_Information: Drone888_Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log
logFileName: GPSDrone4.log
UAVLINK: "None"
class_model/src/utils/mqttConfig_PUB5.yml
+1 -1
View File
@@ -19,5 +19,5 @@ ROS:
ROStopicName_Flight_Information: Drone555_Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log
logFileName: GPSDrone5.log
UAVLINK: "None"
class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py
+2 -1
View File
@@ -2,7 +2,7 @@ import orjson
import time
import logging
logger = logging.getLogger(__name__)
logger = logging.getLogger("__mqttPubMain__")
class Proto_msg_from_ros:
#Protobuf
@@ -113,4 +113,5 @@ class Json_msg_from_ros:
@classmethod
def log(cls):
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))
class_model/src/utils/readConfig.py
+8 -3
View File
@@ -29,7 +29,8 @@ class Config:
if self.options[opts][opt][1]:
raise Exception("Missing mandatory parameter {}".format(opt))
else:
setattr(self,opt,None)
pass
# setattr(self,opt,None)
def __str__(self):
return str(yaml.dump(self.ymlcfg, default_flow_style=False))
@@ -111,6 +112,10 @@ class Read_SUB_Config(Config):
"uav_id": (str,False),
"baudrate": (int,False),
"ttyport": (str,False)}}
self.setAttribute()
def __str__(self):
return super().__str__()
class Read_CMD_Config(Config):
def setAttribute(self):
@@ -141,7 +146,7 @@ class Read_CMD_Config(Config):
return super().__str__()
if __name__ == "__main__":
cfg=Read_CMD_Config("uavlinkConfig_SUB.yml")
print(cfg)
cfg=Read_SUB_Config("mqttConfig_SUB.yml")
print(cfg.msg_format)