diff --git a/class_model/src/Leader.cpp b/class_model/src/Leader.cpp new file mode 100644 index 0000000..03d9f06 --- /dev/null +++ b/class_model/src/Leader.cpp @@ -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..."); +} \ No newline at end of file diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp index 441b2ea..fc18bc5 100644 --- a/class_model/src/bio_main.cpp +++ b/class_model/src/bio_main.cpp @@ -1,14 +1,13 @@ -#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) { // Init ROS node ros::init(argc, argv, "drone1_node"); - + // reate Assync spiner ros::AsyncSpinner spinner(0); spinner.start(); @@ -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(); target.lat = config["routh1"]["y"].as(); + distance = config["formation"]["distance"].as(); + angle = config["formation"]["angle"].as(); self = request_object.get_self_GPS(); M1 = request_object.get_M1_GPS(); 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 + } + } + } \ No newline at end of file diff --git a/class_model/src/config.yaml b/class_model/src/config.yaml index 4f54336..01bfd98 100644 --- a/class_model/src/config.yaml +++ b/class_model/src/config.yaml @@ -6,5 +6,5 @@ routh1: z: 0.0 formation: distance: 4.0 - angel: 45 + angle: 45 diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp index 7240d76..c0ce1b7 100755 --- a/class_model/src/follower.cpp +++ b/class_model/src/follower.cpp @@ -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 ; idata); 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(rostopicName,100); + std::string rostopicName = "/uav_message"; + // ros::Publisher data_pub = json_node.advertise(rostopicName,100); ros::Subscriber data_receive = json_node.subscribe(rostopicName,100,callBack); //ros::Subscriber data_receive1 = json_node.subscribe("/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}); diff --git a/class_model/src/mqttSubMain.py b/class_model/src/mqttSubMain.py index eb8e697..bf11096 100755 --- a/class_model/src/mqttSubMain.py +++ b/class_model/src/mqttSubMain.py @@ -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") diff --git a/class_model/src/requestData.cpp b/class_model/src/requestData.cpp index 1778bcf..0f12a3b 100755 --- a/class_model/src/requestData.cpp +++ b/class_model/src/requestData.cpp @@ -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: " <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; -// } \ No newline at end of file +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; +} \ No newline at end of file diff --git a/class_model/src/test_lib.cpp b/class_model/src/test_lib.cpp index b1926bc..81cae94 100644 --- a/class_model/src/test_lib.cpp +++ b/class_model/src/test_lib.cpp @@ -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(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" < _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]); diff --git a/class_model/src/utils/mqttConfig_PUB.yml b/class_model/src/utils/mqttConfig_PUB.yml index 45f9d60..4d07a13 100644 --- a/class_model/src/utils/mqttConfig_PUB.yml +++ b/class_model/src/utils/mqttConfig_PUB.yml @@ -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: diff --git a/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py b/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py index 22ad2cf..35c08bc 100644 --- a/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py +++ b/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py @@ -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 @@ -66,4 +61,4 @@ class Json_msg_to_ros: @classmethod def Drone650_on_message_Flight_Information(cls, client, userdata, msg): cls.Drone650_publisher_Flight_Information.publish(msg.payload.decode("UTF-8")) - cls.rate.sleep() \ No newline at end of file + cls.rate.sleep() diff --git a/class_model/src/utils/readConfig.py b/class_model/src/utils/readConfig.py index 8df612c..65ca5e7 100644 --- a/class_model/src/utils/readConfig.py +++ b/class_model/src/utils/readConfig.py @@ -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)}}