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

sub 3 drone work, but too slow

Browse Source
protobuf
Xuan0319 3 years ago
parent 58a2f3e7c1
commit 02bff86b0e
11 changed files with 280 additions and 137 deletions
Show Stats Download Patch File Download Diff File

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

@ -0,0 +1,123 @@
#include "class_model/Leader.h"
#include "class_model/convert_degree.h"
LeaderClass::LeaderClass() : node_handle_(""){
}
LeaderClass::~LeaderClass() { ros::shutdown(); }
void LeaderClass::leader(global_location target){
std::string command = "",pre_command = receiver_object.check_command();;
std_msgs::String message;
target_location.lon = target.lon*1e5;
target_location.lat = target.lat*1e5;
sleep(3);
flag = 0;
heading_status = 0;
while(true){
if(flag==0){
go2target(target.lon,target.lat);
}else{
// next_command.publish(message);
ROS_INFO("break");
break;
}
command = receiver_object.check_command();
while(command =="stop" || command == "land"){
// command_object.set_velocity(0,0,0,0,1);
// command = receiver_object.check_command();
if(command == "land"){
mode_object.set_Mode("LAND");
}else if(command != "stop"){
break;
}
}
if(command != pre_command){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void LeaderClass::go2target(float x,float y){
current_location=GPS_object.gps_position();
float target_heading,error_heading;
float heading = GPS_object.get_heading();
target_heading = ConvertDeg(x,y);
// float error_yaw = 0.2;
// error_heading = target_heading - heading;
// error_yaw = check_direction(error_heading)*error_yaw; //check yaw direction
while(heading_status != 1){
face2target(target_heading);
}
drone_speed.x = PID_x.update(current_location.lon*1e5 , target_location.lon ,leader_pid); //leader_pid defined in header file
drone_speed.y = PID_y.update(current_location.lat*1e5 , target_location.lat ,leader_pid);
// float error_x = PID_x.update1(current_location.lon*1e5 , target_location.lon );
// float error_y = PID_y.update1(current_location.lat*1e5 , target_location.lat );
if (drone_speed.x < ignore_small && drone_speed.x > -ignore_small){ //ignore small errors
drone_speed.x = 0;
}
if (drone_speed.y < ignore_small && drone_speed.y > -ignore_small){
drone_speed.y = 0;
}
if (drone_speed.x < -1){ //max velociy
drone_speed.x = -1;
}
if (drone_speed.x > 1){
drone_speed.x = 1;
}
if (drone_speed.y < -1){
drone_speed.y = -1;
}
if (drone_speed.y > 1){
drone_speed.y = 1;
}
if (drone_speed.x == 0 && drone_speed.y == 0){
flag = 1;
}
PubData_object.publishData(drone_speed);
command_object.set_velocity(drone_speed.x ,drone_speed.y ,0,0,0.1);
// ROS_INFO("slope:%f,%f,%f",slope,x,y);
ROS_INFO("%f,%f",drone_speed.x,drone_speed.y);
// ROS_INFO("heading:%f,target_heading:%f",heading,target_heading);
// ROS_INFO("target:%f,%f",target_location.lon ,target_location.lat);
// ROS_INFO("current:%f,%f",current_location.lon*1e5 ,current_location.lat*1e5);
ROS_INFO("************************************");
}
void LeaderClass::face2target(float target_heading){
float error_heading;
float heading = GPS_object.get_heading();
float error_yaw = 0.2;
error_heading = target_heading - heading;
error_yaw = check_direction(error_heading)*error_yaw;
if (error_heading < 5 && error_heading > -5 ){
error_yaw = 0;
heading_status = 1;
}
command_object.set_velocity(0 ,0 ,0,error_yaw,0.1);
ROS_INFO("face2target...");
}

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

@ -1,7 +1,6 @@
#include "class_model/mission.h"
#include "class_model/choose_leader.h"
#include "class_model/PubInfo.h"
#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) {
@ -16,38 +15,48 @@ int main(int argc, char **argv) {
ModeClass mode_object;
ControlClass control_object;
ReceiverClass receiver_object;
RequestClass request_object;
MissionClass mission_object;
CommandClass command_object;
SelectClass select_formation;
PubInfoClass PubInfo_object;
FollowerClass follower_object;
LeaderClass leader_object;
// SelectionClass selection_object;
global_location target,self,M1,M2;
float distance,angle;
int ref_ID,checkLeader;
YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point)
target.lon = config["routh1"]["x"].as<float>();
target.lat = config["routh1"]["y"].as<float>();
distance = config["formation"]["distance"].as<float>();
angle = config["formation"]["angle"].as<float>();
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
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]);
if(check_leader(pos,index).is_leader == 1){
//fly2target
}else{
//find reference drone(pos,index,leader_ID);
}
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{
//follow reference drone position
}
}
}

2
class_model/src/config.yaml
Unescape Escape View File

@ -6,5 +6,5 @@ routh1:
z: 0.0
formation:
distance: 4.0
angel: 45
angle: 45

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

@ -2,16 +2,34 @@
FollowerClass::FollowerClass() : node_handle_(""){
std::string ros_namespace;
if (!node_handle_.hasParam("namespace"))
{
}else{
node_handle_.getParam("namespace", ros_namespace);
}
}
FollowerClass::~FollowerClass() { ros::shutdown(); }
void FollowerClass::follower(){
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)();
//plane()
//follow()
}
}
void FollowerClass::plane(global_location target, global_location leader, global_location follower){
@ -69,7 +87,7 @@ void FollowerClass::plane(global_location target, global_location leader, global
}
int direct(global_location target, global_location leader, DroneStatus self, DroneStatus member){
int FollowerClass::direct(global_location target, global_location leader, DroneStatus self, DroneStatus member){
global_location vector_LT,vector_SM;
float m,n,k,cosTheta;
@ -90,7 +108,7 @@ int direct(global_location target, global_location leader, DroneStatus self, Dro
}
void FollowerClass::find_ref_drone(global_location data[], size_t index, int leaderID){
int FollowerClass::find_ref_drone(global_location data[], size_t index, int leaderID){
for(int i=1 ; i<index ; i++){ //check leader drone
if(leaderID == data[i].ID){
@ -180,15 +198,11 @@ void FollowerClass::find_ref_drone(global_location data[], size_t index, int lea
std::cout <<"refID " << ref_ID << std::endl;
}
void FollowerClass::get_location(){
// lon[0]
return ref_ID;
}
// void FollowerClass::calculate_position(float k,float theta){
// theta = theta*3.14/180;

18
class_model/src/json.cpp
Unescape Escape View File

@ -11,13 +11,13 @@ void callBack(const std_msgs::String::ConstPtr &data){
json j_data = json::parse(data->data);
std::cout <<"original message"<< data->data << std::endl;
std::cout <<"parse message"<< j_data << std::endl;
std::cout <<"parse message"<< j_data["lat"] << std::endl;
// std::cout <<"parse message"<< j_data << std::endl;
// std::cout <<"parse message"<< j_data["lat"] << std::endl;
std::string k = j_data["lat"] , q = j_data["lon"];
int lat = std::stoi(k) ,lon = std::stoi(q);
std::cout <<"lat:"<< k << std::endl;
std::cout <<"lon:"<< q << std::endl;
std::cout <<"lat:"<< j_data["lat"] << std::endl;
std::cout <<"lon:"<< j_data["lon"] << std::endl;
}
@ -37,15 +37,15 @@ int main(int argc ,char **argv)
ros::AsyncSpinner spinner(0);
spinner.start();
std::string rostopicName = "jsonData";
ros::Publisher data_pub = json_node.advertise<std_msgs::String>(rostopicName,100);
std::string rostopicName = "/uav_message";
// ros::Publisher data_pub = json_node.advertise<std_msgs::String>(rostopicName,100);
ros::Subscriber data_receive = json_node.subscribe<std_msgs::String>(rostopicName,100,callBack);
//ros::Subscriber data_receive1 = json_node.subscribe<std_msgs::String>("/uav_message",100,callBack);
mes.data = data;
while(ros::ok()){
data_pub.publish(mes);
}
// while(ros::ok()){
// data_pub.publish(mes);
// }
// json j_no_init_list = json::array();
// json j_empty_init_list = json::array({});
// json j_nonempty_init_list = json::array({1, 2, 3, 4});

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

@ -38,9 +38,6 @@ def init_dataFormat(cfg:utils.Read_SUB_Config):
client.message_callback_add(cfg.Drone550_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone550_on_message_Flight_Information)
client.message_callback_add(cfg.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)
utils.Json_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
utils.Json_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
else:
logger.debug("msg_format not found")

124
class_model/src/requestData.cpp
Unescape Escape View File

@ -7,19 +7,19 @@ int command;
RequestClass::RequestClass() : node_handle_(""){
mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100,
&RequestClass::Data_callback, this);
// 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", 100,
// &RequestClass::Data_callback, this);
mqtt_data = node_handle_.subscribe("/uav_message", 10,
&RequestClass::Data_callback, this);
// self_data = node_handle_.subscribe("/self_data_message", 100,
// &RequestClass::self_callback, this); //test bionic
// member1_data = node_handle_.subscribe("/member1_data_message", 100,
// &RequestClass::M1_callback, this);
// member2_data = node_handle_.subscribe("/member2_data_message", 100,
// &RequestClass::M2_callback, this);
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);
}
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"];
leader_position.lon=j_data["lon"];
leader_position.alt=j_data["alt"];
heading = 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"];
// std::cout <<"Json: " <<j_data << std::endl;
std::cout <<"Json: " <<j_data << std::endl;
// std::cout << leader_position.lat << std::endl;
// std::cout << leader_position.lon << std::endl;
// std::cout << leader_position.alt << std::endl;
@ -92,50 +92,50 @@ void RequestClass::L_INFO(std::string data){
}
/*test biomic*/
// void RequestClass::self_callback(const std_msgs::String::ConstPtr &sensor) {
// Bio_INFO(sensor->data);
// }
// void RequestClass::M1_callback(const std_msgs::String::ConstPtr &sensor) {
// Bio_INFO(sensor->data);
// }
// void RequestClass::M2_callback(const std_msgs::String::ConstPtr &sensor) {
// Bio_INFO(sensor->data);
// }
// void RequestClass::Bio_INFO(std::string data){
// j_data = json::parse(data); //open source
// drone_ID = j_data["ID"];
// std::remove(ID.begin(),ID.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"];
// }
// }
// global_location RequestClass::get_self_GPS(){
// return self_position;
// }
// global_location RequestClass::get_M1_GPS(){
// return M1_position;
// }
// global_location RequestClass::get_M2_GPS(){
// return M2_position;
// }
void RequestClass::self_callback(const std_msgs::String::ConstPtr &sensor) {
Bio_INFO(sensor->data);
}
void RequestClass::M1_callback(const std_msgs::String::ConstPtr &sensor) {
Bio_INFO(sensor->data);
}
void RequestClass::M2_callback(const std_msgs::String::ConstPtr &sensor) {
Bio_INFO(sensor->data);
}
void RequestClass::Bio_INFO(std::string data){
j_data = json::parse(data); //open source
drone_ID = j_data["ID"];
std::remove(ID.begin(),ID.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"];
}
}
global_location RequestClass::get_self_GPS(){
return self_position;
}
global_location RequestClass::get_M1_GPS(){
return M1_position;
}
global_location RequestClass::get_M2_GPS(){
return M2_position;
}

42
class_model/src/test_lib.cpp
Unescape Escape View File

@ -7,12 +7,12 @@
using namespace std;
global_location leader_drone;
DroneStatus memberDrone[3];
DroneStatus memberDrone[5];
DroneStatus drone1;
DroneStatus drone2;
DroneStatus drone3;
DroneStatus self;
DroneStatus samePlane[3];
DroneStatus samePlane[5];
global_location follower_location;
typedef struct result{
@ -31,7 +31,7 @@ result check_leader(global_location data[], size_t index){
d[data[i].ID] = sqrt( pow((data[0].lat - data[i].lat),2) + pow((data[0].lon - data[i].lon),2) );
std::cout << i<<"," <<d[data[i].ID] <<std::endl;
// std::cout << i<<"," <<d[data[i].ID] <<std::endl;
}
std::vector<float> _d(d+1,d+index);
@ -169,28 +169,28 @@ void find_ref_drone(global_location data[], size_t index, int leaderID){
}else{
memberDrone[j] = drone1;
j+=1;
std::cout << "drone1" <<std::endl;
// std::cout << "drone1" <<std::endl;
}
if(drone2.ID == data[1].ID){
self = drone2;
}else{
memberDrone[j] = drone2;
j+=1;
std::cout << "drone2" <<std::endl;
// std::cout << "drone2" <<std::endl;
}
if(drone3.ID == data[1].ID){
self = drone3;
}else{
memberDrone[j] = drone3;
j+=1;
std::cout << "drone3" <<std::endl;
// std::cout << "drone3" <<std::endl;
}
for(int i=0;i<index-2;i++){
if(memberDrone[i].plane == self.plane || memberDrone[i].plane == 0){ //load same plane member
samePlane[s_index] = memberDrone[i];
std::cout << samePlane[s_index].init_location.lat <<std::endl;
std::cout << memberDrone[i].ID <<std::endl;
// std::cout << samePlane[s_index].init_location.lat <<std::endl;
// std::cout << memberDrone[i].ID <<std::endl;
s_index++;
}
@ -200,14 +200,14 @@ void find_ref_drone(global_location data[], size_t index, int leaderID){
// int s_index = sizeof(samePlane)/sizeof(samePlane[0]);
float d[s_index];
int dir,check_index,ref_ID;
std::cout <<"s_index " <<s_index <<std::endl;
// std::cout <<"s_index " <<s_index <<std::endl;
for(int i=0 ; i<s_index ; i++){ //caculate each same plane mamber's distance
dir = direct(data[0],leader_drone,self,samePlane[i]);
d[i] = dir * sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lon - samePlane[i].init_location.lon),2) );
std::cout << i<<"," <<d[i] <<std::endl;
std::cout << samePlane[i].ID <<std::endl;
// std::cout << i<<"," <<d[i] <<std::endl;
// std::cout << samePlane[i].ID <<std::endl;
}
std::vector<float> _d(d,d+s_index);
@ -239,21 +239,21 @@ int main(int argc,char** argv){
global_location M2;
global_location target;
target.lon = 0;
target.lat = 0;
target.lon = 3;
target.lat = -3;
target.heading = 0;
self.lat = 2;
self.lon = 2;
self.ID = 1;
self.lon = -2;
self.lat = -2;
self.ID = 3;
M1.lat = 1;
M1.lon = 1;
M1.lon = 2;
M1.lat = -3;
M1.ID = 2;
M2.lat = 5;
M2.lon = 5;
M2.ID = 3;
M2.lon = 0;
M2.lat = 0;
M2.ID = 1;
global_location data[]={target,self,M1,M2};
size_t index = sizeof(data)/sizeof(data[0]);

8
class_model/src/utils/mqttConfig_PUB.yml
Unescape Escape View File

@ -1,6 +1,6 @@
MQTT:
msg_format: Json
MQTTClientNamePub: Drone550Pub
MQTTClientNamePub: Drone380Pub
host: 140.120.31.133
port: 1883
keepalive: 60
@ -9,13 +9,13 @@ MQTT:
lwt: Dron550 Gone Offline
willRetain: False
# Mqtt topic
Flight_Information_topicToMqtt: Drone550/Flight_Information
Fly_Formation_topicToMqtt: Drone550/Formation
Flight_Information_topicToMqtt: Drone380/Flight_Information
Fly_Formation_topicToMqtt: Drone380/Formation
# Change formate qos
Fly_Formation_topicToMqtt_QOS: 2
#ROS
ROS:
ROSClientNamePub: Drone550Pub
ROSClientNamePub: Drone380Pub
ROStopicName_Flight_Information: Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:

5
class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py
Unescape Escape View File

@ -15,9 +15,6 @@ class Proto_msg_to_ros:
publisher_Flight_Information = None
publisher_Fly_Formation = None
#Mqtt topic: check data from which topic
Flight_Information_topicToMqtt = None
Fly_Formation_topicToMqtt = None
#Proto
@classmethod
@ -44,8 +41,6 @@ class Json_msg_to_ros:
Drone380_publisher_Flight_Information = None
Drone650_publisher_Flight_Information = None
#Mqtt topic: check data from which topic
Flight_Information_topicToMqtt = None
Fly_Formation_topicToMqtt = None
@classmethod

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

@ -83,12 +83,17 @@ class Read_SUB_Config(Config):
"lwt":(str, True),
"willRetain":(bool,False),
"willTopicQOS":(int,True),
"Flight_Information_topicToMqtt": (str,False),
"Drone550_Flight_Information_topicToMqtt": (str,False),
"Drone380_Flight_Information_topicToMqtt":(str,False),
"Drone650_Flight_Information_topicToMqtt":(str,False),
"Fly_Formation_topicToMqtt": (str,False),
"Fly_Formation_topicToMqtt_QOS":(int,False)},
self.sectionNames[1]:{
"ROSClientNameSub": (str,False),
"ROStopicName_Flight_Information": (str,False),
"Dron550_ROStopicName_Flight_Information": (str,False),
"Dron380_ROStopicName_Flight_Information": (str,False),
"Dron380_ROStopicName_Flight_Information": (str,False),
"Dron650_ROStopicName_Flight_Information": (str,False),
"ROStopicName_Fly_Formation": (str,False)},
self.sectionNames[2]:{
"logFileName":(str,False)}}

Write Preview
Loading…
Cancel
Save