diff --git a/class_model/include/class_model/formation.h b/class_model/include/class_model/formation.h
index c898752..3e647a9 100755
--- a/class_model/include/class_model/formation.h
+++ b/class_model/include/class_model/formation.h
@@ -51,11 +51,12 @@ private:
global_location leader_location;
global_location current_location;
velocity drone_speed;
- int flag = 0,heading_status = 0;
+ int flag = 0,heading_status = 0,buf = 0;
float pre_alt,cur_alt;
- float leader_pid[3] = {0.5 , 0.000000000001 ,0.5};
+ float error_lon,error_lat;
+ float leader_pid[3] = {1 , 0.000000000001 ,0.5};
float follower_pid[3] = {1 , 0.00000000001 ,0.5};
- float ignore_small = 0.20;
+ float ignore_small = 0.50;
void calculate_position(float k,float theta,int direction=0);
void spherical_coordinate(float k,float theta,float psi);
diff --git a/class_model/launch/mavlink.launch b/class_model/launch/mavlink.launch
new file mode 100644
index 0000000..7f71bb3
--- /dev/null
+++ b/class_model/launch/mavlink.launch
@@ -0,0 +1,12 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/class_model/launch/mqtt.launch b/class_model/launch/mqtt.launch
new file mode 100755
index 0000000..029ad84
--- /dev/null
+++ b/class_model/launch/mqtt.launch
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/class_model/launch/mqtt_old.launch b/class_model/launch/mqtt_old.launch
new file mode 100755
index 0000000..f078a75
--- /dev/null
+++ b/class_model/launch/mqtt_old.launch
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/class_model/launch/one.launch b/class_model/launch/one.launch
new file mode 100644
index 0000000..bd03748
--- /dev/null
+++ b/class_model/launch/one.launch
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/class_model/src/command.cpp b/class_model/src/command.cpp
old mode 100755
new mode 100644
index e615a0a..c8dd142
--- a/class_model/src/command.cpp
+++ b/class_model/src/command.cpp
@@ -9,7 +9,7 @@ CommandClass::CommandClass() : node_handle_("~"){
node_handle_.getParam("namespace", ros_namespace);
}
- velocity_command=node_handle_.advertise(ros_namespace+"/mavros/setpoint_velocity/cmd_vel",100);
+ velocity_command=node_handle_.advertise(ros_namespace+"/mavros/setpoint_velocity/cmd_vel",10);
gps_command=node_handle_.advertise(ros_namespace+"/mavros/setpoint_raw/global",10);
}
@@ -26,13 +26,21 @@ void CommandClass::velocity_init(){
velocity_command.publish(msg);
- uint64_t last_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count();
+ double begin = ros::Time::now().toSec();
while(true){
velocity_command.publish(msg);
- uint64_t now_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count();
- if((now_ms-last_ms)>10){
+ double now = ros::Time::now().toSec();
+ if((now-begin)>0.01){
break;
}
+
+ // uint64_t last_ms = std::chrono::duration_cast(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::system_clock::now().time_since_epoch()).count();
+ // if((now_ms-last_ms)>10){
+ // break;
+ // }
}
}
@@ -49,19 +57,32 @@ void CommandClass::fix_velocity(float x,float y,float alt,float yaw,float second
pre_location=gps_object.gps_position();
ROS_INFO("%f,%f",pre_location.lat,pre_location.lon);
- uint64_t last_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count();
+ //uint64_t last_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count();
int flag = 0;
+
+ double begin = ros::Time::now().toSec();
while(true){
- velocity_command.publish(msg);
-
- uint64_t now_ms = std::chrono::duration_cast(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();
- flag=1;
- ROS_INFO("fin");
- break;
+ velocity_command.publish(msg);
+ double now = ros::Time::now().toSec();
+ if((now-begin)>second){
+ //ROS_INFO("%ld",now-begin);
+ velocity_init();
+ flag=1;
+ ROS_INFO("fin");
+ break;
}
+
+ // while(true){
+ // velocity_command.publish(msg);
+
+ // uint64_t now_ms = std::chrono::duration_cast(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();
+ // flag=1;
+ // ROS_INFO("fin");
+ // break;
+ // }
}
while(flag==1){
@@ -113,19 +134,28 @@ void CommandClass::set_velocity(float x,float y,float alt,float yaw,float second
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();
+ // global_location pre_location=gps_object.gps_position();
+
+ double begin = ros::Time::now().toSec();
+ while(true){
+ velocity_command.publish(msg);
+ double now = ros::Time::now().toSec();
+ if((now-begin)>second){
+ velocity_init();
+ break;
+ }
- uint64_t last_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count();
- while(true){
- velocity_command.publish(msg);
+ // uint64_t last_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count();
- uint64_t now_ms = std::chrono::duration_cast(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;
- }
+ // while(true){
+ // velocity_command.publish(msg);
+ // uint64_t now_ms = std::chrono::duration_cast(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;
+ // }
}
}
diff --git a/class_model/src/command.py b/class_model/src/command.py
index ca73e08..7bc88f7 100755
--- a/class_model/src/command.py
+++ b/class_model/src/command.py
@@ -43,7 +43,7 @@ def initialise_clients(clientName):
if __name__ == '__main__':
# Mqtt
- mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "cmd/broadcast"}
+ mqtt_config = {"host": "140.120.31.133", "port": 1883, "topic": "cmd/broadcast"}
client = initialise_clients("receiver")
client.on_connect = on_connect
client.on_message = on_message
diff --git a/class_model/src/formation.cpp b/class_model/src/formation.cpp
old mode 100755
new mode 100644
index 2a5b106..6db646c
--- a/class_model/src/formation.cpp
+++ b/class_model/src/formation.cpp
@@ -67,7 +67,7 @@ void FormationClass::leader(){
void FormationClass::leader1(float x,float y){
- std::string command = "",pre_command = "";
+ std::string command = "",pre_command = receiver_object.check_command();;
std_msgs::String message;
leader_location=request_object.get_leader_GPS();
@@ -88,9 +88,9 @@ void FormationClass::leader1(float x,float y){
command = receiver_object.check_command();
while(command =="stop" || command == "land"){
- command_object.set_velocity(0,0,0,0,1);
- command = receiver_object.check_command();
-
+ // 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"){
@@ -150,7 +150,8 @@ void FormationClass::follower1(int type){
void FormationClass::follower2(int type){
- std::string message,command,pre_command = "";
+ std::string message,command;
+ std::string pre_command = receiver_object.check_command();
while(true){
if(type == 0){
@@ -174,7 +175,6 @@ void FormationClass::follower2(int type){
// ROS_INFO("change formation");
break;
}
- pre_command = command;
}
@@ -413,6 +413,7 @@ void FormationClass::calculate_position(float k,float theta,int direction){
float deg_phi = request_object.get_leader_heading()/100;
float heading = GPS_object.get_heading();
+ std::string type = receiver_object.check_command();
float phi = deg_phi*3.14/180; //degree-->radian
theta = theta*3.14/180;
@@ -432,15 +433,16 @@ void FormationClass::calculate_position(float k,float theta,int direction){
Pf[0]=(transfer[0][0]*Q[0]+transfer[0][1]*Q[1])*T[0]+Pl[0]/100;
Pf[1]=(transfer[1][0]*Q[0]+transfer[1][1]*Q[1])*T[1]+Pl[1]/100;
+ error_lon = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid); //caculate error (PID)
+ error_lat = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid);
- float error_x = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid); //caculate error
- float error_y = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid);
-
+
// float error_x = PID_x.update1(follower_location.lon*1e5 , Pf[0] ); //caculate error
// float error_y = PID_y.update1(follower_location.lat*1e5 , Pf[1] );
- float error_lon = Pf[0] - (follower_location.lon*1e+5); //caculate error
- float error_lat = Pf[1] - (follower_location.lat*1e+5);
+ // error_lon = Pf[0] - (follower_location.lon*1e+5); //caculate error
+ // error_lat = Pf[1] - (follower_location.lat*1e+5);
+
float error_degree = deg_phi - heading + direction;
float error_yaw = 0.2; //CCW
@@ -466,19 +468,31 @@ void FormationClass::calculate_position(float k,float theta,int direction){
error_yaw = 0;
}
+ // if (error_lon < -1){
+ // error_lon = -1;
+ // }
+ // if (error_lon > 1){
+ // error_lon = 1;
+ // }
+ // if (error_lat < -1){
+ // error_lat = -1;
+ // }
+ // if (error_lat > 1){
+ // error_lat = 1;
+ // }
+
// ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
// ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
// ROS_INFO("%f,%f",Pf[0],Pf[1]);
- // ROS_INFO("%f,%f",error_x,error_y);
+ // // ROS_INFO("%f,%f",error_x,error_y);
// ROS_INFO("%f,%f",error_lon,error_lat);
- // ROS_INFO("deg:%f",deg_phi);
- // ROS_INFO("heading:%f",heading);
- // ROS_INFO("error_degree:%f",error_degree);
- // ROS_INFO("error_yaw:%f",error_yaw);
+ // // ROS_INFO("deg:%f",deg_phi);
+ // // ROS_INFO("heading:%f",heading);
+ // // ROS_INFO("error_degree:%f",error_degree);
+ // // ROS_INFO("error_yaw:%f",error_yaw);
// ROS_INFO("************************************");
-
- 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.3);
}
void FormationClass::spherical_coordinate(float k,float theta,float psi){
@@ -587,12 +601,24 @@ void FormationClass::go2target(float x,float y){
// 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 < 0.3 && drone_speed.x > -0.3){ //ignore small errors
+ if (drone_speed.x < ignore_small && drone_speed.x > -ignore_small){ //ignore small errors
drone_speed.x = 0;
}
- if (drone_speed.y < 0.3 && drone_speed.y > -0.3){
+ 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;
@@ -602,7 +628,7 @@ void FormationClass::go2target(float x,float y){
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",error_x,error_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);
@@ -625,6 +651,7 @@ void FormationClass::face2target(float target_heading){
heading_status = 1;
}
command_object.set_velocity(0 ,0 ,0,error_yaw,0.1);
+ ROS_INFO("face2target...");
}
diff --git a/class_model/src/main.cpp b/class_model/src/main.cpp
index 999ab6b..7282521 100755
--- a/class_model/src/main.cpp
+++ b/class_model/src/main.cpp
@@ -10,7 +10,8 @@ int main(int argc, char **argv) {
// reate Assync spiner
ros::AsyncSpinner spinner(0);
- spinner.start();
+ spinner.start();
+ // ros::Rate rate(5);
ModeClass mode_object;
ControlClass control_object;
@@ -23,11 +24,22 @@ int main(int argc, char **argv) {
// SelectionClass selection_object;
// YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point)
+ std::string type = "",pre_type ="x";
+ int flag = 0;
mode_object.set_Mode("GUIDED");
- control_object.arm();
- control_object.takeoff(2);
- sleep(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 ="";
+ }
+ }
+
// selection_object.choose_leader();
// mission_object.start(); //default hover in goose formation
@@ -36,28 +48,29 @@ int main(int argc, char **argv) {
// msg.type = "V_formation";
// msg.mission = "fly2target";
- std::string type = "",pre_type ="x";
-
+
- while(ros::ok()){
+ while(ros::ok() && flag == 1){
type = receiver_object.check_command();
if(pre_type == type){
type = "alreadly_set";
- ROS_INFO("set");
+ // ROS_INFO("set");
}
-
- if(type == ""){
+// TODO: switch case
+ if(type == "init"){
select_formation.goose_formation();
ROS_INFO("init formation");
}else if(type == "line"){
select_formation.line_formation();
+ pre_type ="line";
}else if(type == "row"){
select_formation.row_formation();
- }else if(type == "v"){
+ pre_type ="row";
+ }else if(type == "v1"){ //PID error
select_formation.goose_formation(0,5);
- pre_type ="v";
+ pre_type ="v1";
}else if(type == "v2"){
select_formation.line_formation(-5,0);
pre_type ="v2";
@@ -70,6 +83,7 @@ int main(int argc, char **argv) {
}else if(type == "stop"){
select_formation.stop();
}else if(type == "land"){
+ // ROS_INFO("Land");
mode_object.set_Mode("LAND");
}
diff --git a/class_model/src/mode.cpp b/class_model/src/mode.cpp
index 2ca8e80..c97b02d 100755
--- a/class_model/src/mode.cpp
+++ b/class_model/src/mode.cpp
@@ -26,8 +26,9 @@ int ModeClass::set_Mode(std::string mode) {
// ROS_INFO("setmode %s ok",mode.c_str());
return 0;
}else{
- ROS_ERROR("Failed SetMode %s",mode.c_str());
- return -1;
+ ROS_ERROR("Failed SetMode %s,retry...",mode.c_str());
+ // set_Mode(mode);
+ return 0;
}
}
\ No newline at end of file
diff --git a/class_model/src/requestData.cpp b/class_model/src/requestData.cpp
index 6e3f53d..fd7e0f2 100755
--- a/class_model/src/requestData.cpp
+++ b/class_model/src/requestData.cpp
@@ -7,10 +7,12 @@ 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);
}
diff --git a/class_model/src/tower.py b/class_model/src/tower.py
index c755852..254f93d 100755
--- a/class_model/src/tower.py
+++ b/class_model/src/tower.py
@@ -35,7 +35,7 @@ def on_publish(self, userdata, mid):
connect_flag = False
-mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "cmd/broadcast", "name": "Tower"}
+mqtt_config = {"host": "140.120.31.133", "port": 1883, "topic": "cmd/broadcast", "name": "Tower"}
client = initialise_clients(mqtt_config["name"])
client.on_publish = on_publish
client.on_connect = on_connect