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

Fixed leader drone not responding in land mode

Browse Source
This commit is contained in:
Xuan0319
2023-02-08 21:53:01 +08:00
parent eec2609f88
commit d1280cf656
11 changed files with 145 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
class_model/include/class_model/formation.h
+4 -3
View File
@@ -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);
class_model/src/command.cpp
Executable → Regular
+47 -17
View File
@@ -9,7 +9,7 @@ CommandClass::CommandClass() : node_handle_("~"){
node_handle_.getParam("namespace", ros_namespace);
}
velocity_command=node_handle_.advertise<geometry_msgs::TwistStamped>(ros_namespace+"/mavros/setpoint_velocity/cmd_vel",100);
velocity_command=node_handle_.advertise<geometry_msgs::TwistStamped>(ros_namespace+"/mavros/setpoint_velocity/cmd_vel",10);
gps_command=node_handle_.advertise<mavros_msgs::GlobalPositionTarget>(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::milliseconds>(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::milliseconds>(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::milliseconds>(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::milliseconds>(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::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
//uint64_t last_ms = std::chrono::duration_cast<std::chrono::milliseconds>(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::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
if((now_ms-last_ms)>second*1000){
// ROS_INFO("%ld",now_ms-last_ms);
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::milliseconds>(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();
uint64_t last_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
// global_location pre_location=gps_object.gps_position();
double begin = ros::Time::now().toSec();
while(true){
velocity_command.publish(msg);
uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(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();
double now = ros::Time::now().toSec();
if((now-begin)>second){
velocity_init();
break;
}
// uint64_t last_ms = std::chrono::duration_cast<std::chrono::milliseconds>(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::milliseconds>(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;
// }
}
}
class_model/src/command.py
+1 -1
View File
@@ -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
class_model/src/formation.cpp
Executable → Regular
+46 -19
View File
@@ -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,8 +88,8 @@ 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");
@@ -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...");
}
class_model/src/local_mqtt_pub_data_to_ros.py
+1 -1
View File
@@ -38,7 +38,7 @@ def on_message(client, userdata, msg):
if __name__ == '__main__':
# Mqtt
mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor"}
mqtt_config = {"host": "140.120.31.133", "port": 1883, "topic": "data/sensor"}
client = initialise_clients("123456")
client.on_connect = on_connect
class_model/src/local_mqtt_sub_data_from_ros.py
+1 -1
View File
@@ -17,7 +17,7 @@ from sensor_msgs.msg import Range
from geometry_msgs.msg import Vector3
import time
mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor"}
mqtt_config = {"host": "140.120.31.133", "port": 1883, "topic": "data/sensor"}
data ={}
# Ros
def callBack_imu(IMU):
class_model/src/main.cpp
+21 -7
View File
@@ -11,6 +11,7 @@ int main(int argc, char **argv) {
// reate Assync spiner
ros::AsyncSpinner spinner(0);
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");
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");
}
class_model/src/mode.cpp
+3 -2
View File
@@ -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;
}
}
class_model/src/requestData.cpp
+4 -2
View File
@@ -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);
}
class_model/src/tower.py
+1 -1
View File
@@ -40,7 +40,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
class_model/src/utils/mqttConfig.yml
+1 -1
View File
@@ -2,7 +2,7 @@ MQTT_ROS:
msg_format: Json
MQTTClientNamePub: Drone650Pub
MQTTClientNameSub: Drone650Sub
host: 192.168.50.27
host: 140.120.31.133
port: 1883
keepalive: 60
willTopic: CheckDoneConnect