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

test bio drones

Browse Source
protobuf
Xuan0319 3 years ago
parent d40ddbe239
commit 4b8cc0278f
13 changed files with 196 additions and 100 deletions
Show Stats Download Patch File Download Diff File

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

@ -25,7 +25,7 @@ result check_leader(global_location* data, size_t index){
check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end())); //get minimum value's index
_result.leader_ID = check_ID + 1;
std::cout <<"Drone " << _result.leader_ID << " is leader " << std::endl;
// std::cout <<"Drone " << _result.leader_ID << " is leader " << std::endl;
if (data[1].ID == _result.leader_ID){
_result.is_leader = 1;

4
class_model/include/class_model/follower.h
Unescape Escape View File

@ -36,8 +36,8 @@ public:
DroneStatus self;
DroneStatus samePlane[5];
void follower(global_location* member_data, size_t index, int refID);
int find_ref_drone(global_location* data[], size_t index, int leaderID);
void follower(global_location** member_data, size_t index, int refID);
int find_ref_drone(global_location** data, size_t index, int leaderID);
float lon[3],lat[3];

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

@ -18,11 +18,12 @@ int main(int argc, char **argv) {
RequestClass request_object;
FollowerClass follower_object;
LeaderClass leader_object;
ReceiverClass receiver_object;
// SelectionClass selection_object;
global_location target,self,M1,M2,M3,M4;
int ref_ID,checkLeader;
int ref_ID,checkLeader,flag;
std::string type = "",pre_type ="x";
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>()*1e7;
@ -35,8 +36,7 @@ int main(int argc, char **argv) {
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
// global_location pos[] = {target,self,M1,M2};
// global_location member[] = {M1,M2};
global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
global_location* member[] = {&M1,&M2,&M3,&M4};
size_t index = sizeof(pos)/sizeof(pos[0]);
@ -45,28 +45,44 @@ int main(int argc, char **argv) {
mode_object.set_Mode("GUIDED");
control_object.arm();
control_object.takeoff(2);
// while(flag!=1){
// type = receiver_object.check_command();
// if(type == "go"){
// mode_object.set_Mode("GUIDED");
// control_object.arm();
// control_object.takeoff(2);
// sleep(2);
// flag = 1;
// type ="";
// }
// }
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){
while(ros::ok()){
// type = receiver_object.check_command();
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
M2 = request_object.get_M2_GPS();
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
// pos[] = {target,self,M1,M2,M3,M4};
// member[] = {M1,M2,M3,M4};
if(checkLeader == 1){
// leader_object.leader(target);
leader_object.leader(target);
}else{
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
follower_object.follower(*member,m_index,ref_ID); //follow reference drone position
follower_object.follower(member,m_index,ref_ID); //follow reference drone position
}
// if(type == "land"){
// mode_object.set_Mode("LAND");
// }
}
ros::waitForShutdown();

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

@ -18,11 +18,12 @@ int main(int argc, char **argv) {
RequestClass request_object;
FollowerClass follower_object;
LeaderClass leader_object;
ReceiverClass receiver_object;
// SelectionClass selection_object;
global_location target,self,M1,M2,M3,M4;
int ref_ID,checkLeader;
int ref_ID,checkLeader,flag;
std::string type = "",pre_type ="x";
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>()*1e7;
@ -35,8 +36,7 @@ int main(int argc, char **argv) {
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
// global_location pos[] = {target,self,M1,M2};
// global_location member[] = {M1,M2};
global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
global_location* member[] = {&M1,&M2,&M3,&M4};
size_t index = sizeof(pos)/sizeof(pos[0]);
@ -45,28 +45,44 @@ int main(int argc, char **argv) {
mode_object.set_Mode("GUIDED");
control_object.arm();
control_object.takeoff(2);
// while(flag!=1){
// type = receiver_object.check_command();
// if(type == "go"){
// mode_object.set_Mode("GUIDED");
// control_object.arm();
// control_object.takeoff(2);
// sleep(2);
// flag = 1;
// type ="";
// }
// }
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){
while(ros::ok()&& flag == 1){
// self = request_object.get_self_GPS();
// M1 = request_object.get_M1_GPS();
// M2 = request_object.get_M2_GPS();
// M3 = request_object.get_M3_GPS();
// M4 = request_object.get_M4_GPS();
// pos[] = {target,self,M1,M2,M3,M4};
// member[] = {M1,M2,M3,M4};
// type = receiver_object.check_command();
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
M2 = request_object.get_M2_GPS();
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
if(checkLeader == 1){
// leader_object.leader(target);
leader_object.leader(target);
}else{
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
follower_object.follower(*member,m_index,ref_ID); //follow reference drone position
follower_object.follower(member,m_index,ref_ID); //follow reference drone position
}
// if(type == "land"){
// mode_object.set_Mode("LAND");
// }
}
ros::waitForShutdown();

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

@ -18,11 +18,12 @@ int main(int argc, char **argv) {
RequestClass request_object;
FollowerClass follower_object;
LeaderClass leader_object;
ReceiverClass receiver_object;
// SelectionClass selection_object;
global_location target,self,M1,M2,M3,M4;
int ref_ID,checkLeader;
int ref_ID,checkLeader,flag;
std::string type = "",pre_type ="x";
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>()*1e7;
@ -35,8 +36,7 @@ int main(int argc, char **argv) {
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
// global_location pos[] = {target,self,M1,M2};
// global_location member[] = {M1,M2};
global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
global_location* member[] = {&M1,&M2,&M3,&M4};
size_t index = sizeof(pos)/sizeof(pos[0]);
@ -45,28 +45,44 @@ int main(int argc, char **argv) {
mode_object.set_Mode("GUIDED");
control_object.arm();
control_object.takeoff(2);
// while(flag!=1){
// type = receiver_object.check_command();
// if(type == "go"){
// mode_object.set_Mode("GUIDED");
// control_object.arm();
// control_object.takeoff(2);
// sleep(2);
// flag = 1;
// type ="";
// }
// }
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){
while(ros::ok()&& flag == 1){
// self = request_object.get_self_GPS();
// M1 = request_object.get_M1_GPS();
// M2 = request_object.get_M2_GPS();
// M3 = request_object.get_M3_GPS();
// M4 = request_object.get_M4_GPS();
// pos[] = {target,self,M1,M2,M3,M4};
// member[] = {M1,M2,M3,M4};
// type = receiver_object.check_command();
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
M2 = request_object.get_M2_GPS();
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
if(checkLeader == 1){
// leader_object.leader(target);
leader_object.leader(target);
}else{
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
follower_object.follower(*member,m_index,ref_ID); //follow reference drone position
follower_object.follower(member,m_index,ref_ID); //follow reference drone position
}
// if(type == "land"){
// mode_object.set_Mode("LAND");
// }
}
ros::waitForShutdown();

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

@ -18,11 +18,12 @@ int main(int argc, char **argv) {
RequestClass request_object;
FollowerClass follower_object;
LeaderClass leader_object;
ReceiverClass receiver_object;
// SelectionClass selection_object;
global_location target,self,M1,M2,M3,M4;
int ref_ID,checkLeader;
int ref_ID,checkLeader,flag;
std::string type = "",pre_type ="x";
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>()*1e7;
@ -35,8 +36,7 @@ int main(int argc, char **argv) {
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
// global_location pos[] = {target,self,M1,M2};
// global_location member[] = {M1,M2};
global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
global_location* member[] = {&M1,&M2,&M3,&M4};
size_t index = sizeof(pos)/sizeof(pos[0]);
@ -45,28 +45,44 @@ int main(int argc, char **argv) {
mode_object.set_Mode("GUIDED");
control_object.arm();
control_object.takeoff(2);
// while(flag!=1){
// type = receiver_object.check_command();
// if(type == "go"){
// mode_object.set_Mode("GUIDED");
// control_object.arm();
// control_object.takeoff(2);
// sleep(2);
// flag = 1;
// type ="";
// }
// }
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){
while(ros::ok()&& flag == 1){
// self = request_object.get_self_GPS();
// M1 = request_object.get_M1_GPS();
// M2 = request_object.get_M2_GPS();
// M3 = request_object.get_M3_GPS();
// M4 = request_object.get_M4_GPS();
// pos[] = {target,self,M1,M2,M3,M4};
// member[] = {M1,M2,M3,M4};
// type = receiver_object.check_command();
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
M2 = request_object.get_M2_GPS();
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
if(checkLeader == 1){
// leader_object.leader(target);
leader_object.leader(target);
}else{
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
follower_object.follower(*member,m_index,ref_ID); //follow reference drone position
follower_object.follower(member,m_index,ref_ID); //follow reference drone position
}
// if(type == "land"){
// mode_object.set_Mode("LAND");
// }
}
ros::waitForShutdown();

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

@ -18,11 +18,12 @@ int main(int argc, char **argv) {
RequestClass request_object;
FollowerClass follower_object;
LeaderClass leader_object;
ReceiverClass receiver_object;
// SelectionClass selection_object;
global_location target,self,M1,M2,M3,M4;
int ref_ID,checkLeader;
int ref_ID,checkLeader,flag;
std::string type = "",pre_type ="x";
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>()*1e7;
@ -35,8 +36,7 @@ int main(int argc, char **argv) {
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
// global_location pos[] = {target,self,M1,M2};
// global_location member[] = {M1,M2};
global_location* pos[] = {&target,&self,&M1,&M2,&M3,&M4};
global_location* member[] = {&M1,&M2,&M3,&M4};
size_t index = sizeof(pos)/sizeof(pos[0]);
@ -45,28 +45,44 @@ int main(int argc, char **argv) {
mode_object.set_Mode("GUIDED");
control_object.arm();
control_object.takeoff(2);
// while(flag!=1){
// type = receiver_object.check_command();
// if(type == "go"){
// mode_object.set_Mode("GUIDED");
// control_object.arm();
// control_object.takeoff(2);
// sleep(2);
// flag = 1;
// type ="";
// }
// }
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){
while(ros::ok()&& flag == 1){
// self = request_object.get_self_GPS();
// M1 = request_object.get_M1_GPS();
// M2 = request_object.get_M2_GPS();
// M3 = request_object.get_M3_GPS();
// M4 = request_object.get_M4_GPS();
// pos[] = {target,self,M1,M2,M3,M4};
// member[] = {M1,M2,M3,M4};
// type = receiver_object.check_command();
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
M2 = request_object.get_M2_GPS();
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
if(checkLeader == 1){
// leader_object.leader(target);
leader_object.leader(target);
}else{
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
follower_object.follower(*member,m_index,ref_ID); //follow reference drone position
follower_object.follower(member,m_index,ref_ID); //follow reference drone position
}
// if(type == "land"){
// mode_object.set_Mode("LAND");
// }
}
ros::waitForShutdown();

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

@ -12,17 +12,17 @@ FollowerClass::FollowerClass() : node_handle_(""){
FollowerClass::~FollowerClass() { ros::shutdown(); }
void FollowerClass::follower(global_location* member_data, size_t index, int refID){
void FollowerClass::follower(global_location** member_data, size_t index, int refID){
if(refID == member_data[0].ID){
if(refID == member_data[0]->ID){
ref = &RequestClass::get_M1_GPS;
}else if (refID == member_data[1].ID)
}else if (refID == member_data[1]->ID)
{
ref = &RequestClass::get_M2_GPS;
}else if (refID == member_data[2].ID)
}else if (refID == member_data[2]->ID)
{
ref = &RequestClass::get_M3_GPS;
}else if (refID == member_data[3].ID)
}else if (refID == member_data[3]->ID)
{
ref = &RequestClass::get_M4_GPS;
}
@ -146,7 +146,7 @@ int FollowerClass::direct(global_location* target, global_location* leader, Dron
}
int 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){
@ -261,7 +261,7 @@ int FollowerClass::find_ref_drone(global_location* data[], size_t index, int lea
check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end())); //get minimum value's index
ref_ID = samePlane[check_index].ID;
std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " << ref_ID << std::endl;
// std::cout <<"drone "<< self.ID <<",plane: "<< self.plane <<",refID: " << ref_ID << std::endl;
return ref_ID;

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

@ -260,8 +260,13 @@ int main(int argc,char** argv){
global_location data[]={target,self,M1,M2};
size_t index = sizeof(data)/sizeof(data[0]);
M2.lon = 1;
M2.lat = 1;
M2.ID = 3;
std::cout<< data[3].lon<<std::endl;
find_ref_drone(data,index,check_leader(data,index).leader_ID);
// find_ref_drone(data,index,check_leader(data,index).leader_ID);

17
class_model/src/uavlinkSubMain.py
Unescape Escape View File

@ -30,7 +30,7 @@ def init_dataFormat(cfg:Read_SUB_Config):
Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
Proto_msg_to_ros.rate = rospy.Rate(10)
Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,String,queue_size=10)
if __name__ == '__main__':
@ -56,14 +56,27 @@ if __name__ == '__main__':
logger.addHandler(stream_handler)
logger.info(cfg)
rospy.init_node(cfg.ROSClientNameSub)
init_dataFormat(cfg)
flight_information_msg = flight_information_pb2.flight_information_message()
flight_information_msg.gps.LAT = 34123.1231515
flight_information_msg.gps.LON = 23423.1231515
flight_information_msg.gps.ALT = 12123.1231515
flight_information_msg.heading = 155.12215
proto = flight_information_msg.SerializeToString()
readTenByte = b'\xf1' + proto + b'\r\x1a'
while True:
try:
# test json
readTenByte = sel.read_until(expected= b'\x01\x01', size=25)
# readTenByte = sel.read_until(expected= b'\x01\x01', size=25)
Proto_msg_to_ros.on_message_Flight_Information(readTenByte)
except KeyboardInterrupt as e:
print("End of program")
sys.exit()

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

@ -24,9 +24,8 @@ class Proto_msg_to_ros:
proto_msg = cls.flight_information_msg.FromString(proto)
protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True)
logger.info(protoTOJson_msg)
print(protoTOJson_msg)
cls.publisher_Flight_Information.publish(protoTOJson_msg)
cls.rate.sleep()

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

@ -79,15 +79,15 @@ class Read_SUB_Config(Config):
super().__init__(inFileName)
self.options = {
self.sectionNames[0]:{
"msg_format": (str,True),
"MQTTClientNameSub": (str,True),
"host": (str,True),
"port": (int,True),
"keepalive": (int,True),
"willTopic":(str,True),
"lwt":(str, True),
"msg_format": (str,False),
"MQTTClientNameSub": (str,False),
"host": (str,False),
"port": (int,False),
"keepalive": (int,False),
"willTopic":(str,False),
"lwt":(str, False),
"willRetain":(bool,False),
"willTopicQOS":(int,True),
"willTopicQOS":(int,False),
"Drone550_Flight_Information_topicToMqtt": (str,False),
"Drone380_Flight_Information_topicToMqtt":(str,False),
"Drone650_Flight_Information_topicToMqtt":(str,False),
@ -96,7 +96,7 @@ class Read_SUB_Config(Config):
"Fly_Formation_topicToMqtt": (str,False),
"Fly_Formation_topicToMqtt_QOS":(int,False)},
self.sectionNames[1]:{
"ROSClientNameSub": (str,False),
"ROSClientNameSub": (str,True),
"Dron550_ROStopicName_Flight_Information": (str,False),
"Dron380_ROStopicName_Flight_Information": (str,False),
"Dron380_ROStopicName_Flight_Information": (str,False),

5
class_model/src/utils/uavlinkConfig_SUB.yml
Unescape Escape View File

@ -2,11 +2,10 @@ UAVLINK:
uavlink_msg_format: Proto
uav_id: \x01\x01
baudrate: 250000
ttyport: /dev/ttyUSB2
ttyport: /dev/ttyUSB0
MQTT: "None"
ROS:
ROSClientNameSub: Drone550UAVLINKSub
ROStopicName_Flight_Information: Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver
Dron550_ROStopicName_Flight_Information: Flight_Information_reciver
LOG:
logFileName: UAVLINKsub.log

Write Preview
Loading…
Cancel
Save