You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
73 lines
1.7 KiB
C++
73 lines
1.7 KiB
C++
4 years ago
|
#include "class_model/mission.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();
|
||
|
|
|
||
|
|
ThreadGPSClass gps_object;
|
||
|
|
ModeClass mode_object;
|
||
|
|
ControlClass control_object;
|
||
|
|
SelectClass select_formation;
|
||
|
|
ReceiverClass receiver_object;
|
||
|
|
MissionClass mission_object;
|
||
|
|
CommandClass command_object;
|
||
|
|
std::string type = "";
|
||
|
|
|
||
|
|
mode_object.set_Mode("GUIDED");
|
||
|
|
control_object.arm();
|
||
|
|
control_object.takeoff(4.5);
|
||
|
|
sleep(5);
|
||
|
|
|
||
|
|
while(ros::ok()){
|
||
|
|
|
||
|
|
type = receiver_object.check_command();
|
||
|
|
// // ROS_INFO("%s",type.c_str());
|
||
|
|
|
||
|
|
if(type == "" || type == "init"){
|
||
|
|
select_formation.goose_formation();
|
||
|
|
ROS_INFO("init formation");
|
||
|
|
}else if(type == "line"){
|
||
|
|
select_formation.line_formation();
|
||
|
|
// ROS_INFO("line foemation");
|
||
|
|
}else if(type == "row"){
|
||
|
|
select_formation.row_formation();
|
||
|
|
}else if(type == "circle"){
|
||
|
|
select_formation.circle_formation();
|
||
|
|
}else if(type == "stop"){
|
||
|
|
select_formation.stop();
|
||
|
|
}else if(type == "land"){
|
||
|
|
mode_object.set_Mode("LAND");
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//mission_object.cruise(20,20);
|
||
|
|
// select_formation.goose_formation();
|
||
|
|
// }else if (type == "hex"){
|
||
|
|
// select_formation.Hex_formation();
|
||
|
|
// }
|
||
|
|
// select_formation.Hex_formation();
|
||
|
|
sleep(0.5);
|
||
|
|
// command_object.set_velocity(0 ,0 ,0,0,1);
|
||
|
|
|
||
|
|
if(type == "stop"){
|
||
|
|
select_formation.stop();
|
||
|
|
}else if(type == "land"){
|
||
|
|
mode_object.set_Mode("LAND");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// select_formation.square();
|
||
|
|
|
||
|
|
// RequestClass test_object;
|
||
|
|
|
||
|
|
ros::waitForShutdown();
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|