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

reslove conflict

Browse Source
protobuf
RangeOfGlitching 3 years ago
parent 434d048547 a10950df10
commit 27945fa239
12 changed files with 200 additions and 66 deletions
Show Stats Download Patch File Download Diff File

7
class_model/include/class_model/formation.h
Unescape Escape View File

@ -51,11 +51,12 @@ private:
global_location leader_location; global_location leader_location;
global_location current_location; global_location current_location;
velocity drone_speed; velocity drone_speed;
int flag = 0,heading_status = 0; int flag = 0,heading_status = 0,buf = 0;
float pre_alt,cur_alt; 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 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 calculate_position(float k,float theta,int direction=0);
void spherical_coordinate(float k,float theta,float psi); void spherical_coordinate(float k,float theta,float psi);

12
class_model/launch/mavlink.launch
Unescape Escape View File

@ -0,0 +1,12 @@
<launch>
<!-- vim: set ft=xml noet : -->
<!-- base node launch file-->
<arg name="fcu_url" default="/dev/ttyACM0:115200" />
<arg name="tgt_system" default="2"/>
<node pkg="mavros" type="mavros_node" name="mavros" >
<param name="fcu_url" value="$(arg fcu_url)" />
<param name="target_system_id" value="$(arg tgt_system)" />
</node>
</launch>

18
class_model/launch/mqtt.launch
Unescape Escape View File

@ -0,0 +1,18 @@
<launch>
<group>
<node pkg="class_model" type="mqttPubMain.py" name="mqttPubMain" output="screen">
</node>
</group>
<group>
<node pkg="class_model" type="mqttSubMain.py" name="mqttSubMain" output="screen">
</node>
</group>
<group>
<node pkg="class_model" type="command.py" name="command" output="screen">
</node>
</group>
</launch>

18
class_model/launch/mqtt_old.launch
Unescape Escape View File

@ -0,0 +1,18 @@
<launch>
<group>
<node pkg="class_model" type="local_mqtt_pub_data_to_ros.py" name="mqttPubMain" output="screen">
</node>
</group>
<group>
<node pkg="class_model" type="local_mqtt_sub_data_from_ros.py" name="mqttSubMain" output="screen">
</node>
</group>
<group>
<node pkg="class_model" type="command.py" name="command" output="screen">
</node>
</group>
</launch>

11
class_model/launch/one.launch
Unescape Escape View File

@ -0,0 +1,11 @@
<launch>
<group>
<node pkg="class_model" type="main" name="leader" output="screen">
<!-- <param name="namespace" value="/drone1"/> -->
<param name="droneID" value="1"/>
<param name="use_sim_time" value="true" />
</node>
</group>
</launch>

78
class_model/src/command.cpp
Unescape Escape View File

@ -9,7 +9,7 @@ CommandClass::CommandClass() : node_handle_("~"){
node_handle_.getParam("namespace", ros_namespace); 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); 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); 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){ while(true){
velocity_command.publish(msg); velocity_command.publish(msg);
uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count(); double now = ros::Time::now().toSec();
if((now_ms-last_ms)>10){ if((now-begin)>0.01){
break; 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(); pre_location=gps_object.gps_position();
ROS_INFO("%f,%f",pre_location.lat,pre_location.lon); 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; int flag = 0;
double begin = ros::Time::now().toSec();
while(true){ while(true){
velocity_command.publish(msg); velocity_command.publish(msg);
double now = ros::Time::now().toSec();
uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count(); if((now-begin)>second){
if((now_ms-last_ms)>second*1000){ //ROS_INFO("%ld",now-begin);
// ROS_INFO("%ld",now_ms-last_ms); velocity_init();
velocity_init(); flag=1;
flag=1; ROS_INFO("fin");
ROS_INFO("fin"); break;
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){ 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.linear.z = alt;
msg.twist.angular.z = yaw; msg.twist.angular.z = yaw;
// ROS_INFO("set_velocity: x:%f, y:%f, alt:%f, yaw:%f",x,y,alt,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::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
while(true){ // uint64_t last_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
velocity_command.publish(msg);
uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count(); // while(true){
if((now_ms-last_ms)>second*1000){ // velocity_command.publish(msg);
// ROS_INFO("%ld",now_ms-last_ms); // uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
//velocity_init(); // if((now_ms-last_ms)>second*1000){
break; // // ROS_INFO("%ld",now_ms-last_ms);
} // //velocity_init();
// break;
// }
} }
} }

2
class_model/src/command.py
Unescape Escape View File

@ -43,7 +43,7 @@ def initialise_clients(clientName):
if __name__ == '__main__': if __name__ == '__main__':
# Mqtt # 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 = initialise_clients("receiver")
client.on_connect = on_connect client.on_connect = on_connect
client.on_message = on_message client.on_message = on_message

69
class_model/src/formation.cpp
Unescape Escape View File

@ -67,7 +67,7 @@ void FormationClass::leader(){
void FormationClass::leader1(float x,float y){ 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; std_msgs::String message;
leader_location=request_object.get_leader_GPS(); leader_location=request_object.get_leader_GPS();
@ -88,9 +88,9 @@ void FormationClass::leader1(float x,float y){
command = receiver_object.check_command(); command = receiver_object.check_command();
while(command =="stop" || command == "land"){ while(command =="stop" || command == "land"){
command_object.set_velocity(0,0,0,0,1); // command_object.set_velocity(0,0,0,0,1);
command = receiver_object.check_command(); // command = receiver_object.check_command();
if(command == "land"){ if(command == "land"){
mode_object.set_Mode("LAND"); mode_object.set_Mode("LAND");
}else if(command != "stop"){ }else if(command != "stop"){
@ -150,7 +150,8 @@ void FormationClass::follower1(int type){
void FormationClass::follower2(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){ while(true){
if(type == 0){ if(type == 0){
@ -174,7 +175,6 @@ void FormationClass::follower2(int type){
// ROS_INFO("change formation"); // ROS_INFO("change formation");
break; 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 deg_phi = request_object.get_leader_heading()/100;
float heading = GPS_object.get_heading(); float heading = GPS_object.get_heading();
std::string type = receiver_object.check_command();
float phi = deg_phi*3.14/180; //degree-->radian float phi = deg_phi*3.14/180; //degree-->radian
theta = theta*3.14/180; 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[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; 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_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_y = PID_y.update1(follower_location.lat*1e5 , Pf[1] );
float error_lon = Pf[0] - (follower_location.lon*1e+5); //caculate error // error_lon = Pf[0] - (follower_location.lon*1e+5); //caculate error
float error_lat = Pf[1] - (follower_location.lat*1e+5); // error_lat = Pf[1] - (follower_location.lat*1e+5);
float error_degree = deg_phi - heading + direction; float error_degree = deg_phi - heading + direction;
float error_yaw = 0.2; //CCW float error_yaw = 0.2; //CCW
@ -466,19 +468,31 @@ void FormationClass::calculate_position(float k,float theta,int direction){
error_yaw = 0; 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",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",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
// ROS_INFO("%f,%f",Pf[0],Pf[1]); // 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("%f,%f",error_lon,error_lat);
// ROS_INFO("deg:%f",deg_phi); // // ROS_INFO("deg:%f",deg_phi);
// ROS_INFO("heading:%f",heading); // // ROS_INFO("heading:%f",heading);
// ROS_INFO("error_degree:%f",error_degree); // // ROS_INFO("error_degree:%f",error_degree);
// ROS_INFO("error_yaw:%f",error_yaw); // // ROS_INFO("error_yaw:%f",error_yaw);
// ROS_INFO("************************************"); // 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){ 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_x = PID_x.update1(current_location.lon*1e5 , target_location.lon );
// float error_y = PID_y.update1(current_location.lat*1e5 , target_location.lat ); // 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; 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; 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){ if (drone_speed.x == 0 && drone_speed.y == 0){
flag = 1; 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); 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("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("heading:%f,target_heading:%f",heading,target_heading);
// ROS_INFO("target:%f,%f",target_location.lon ,target_location.lat); // 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("current:%f,%f",current_location.lon*1e5 ,current_location.lat*1e5);
@ -625,6 +651,7 @@ void FormationClass::face2target(float target_heading){
heading_status = 1; heading_status = 1;
} }
command_object.set_velocity(0 ,0 ,0,error_yaw,0.1); command_object.set_velocity(0 ,0 ,0,error_yaw,0.1);
ROS_INFO("face2target...");
} }

38
class_model/src/main.cpp
Unescape Escape View File

@ -10,7 +10,8 @@ int main(int argc, char **argv) {
// reate Assync spiner // reate Assync spiner
ros::AsyncSpinner spinner(0); ros::AsyncSpinner spinner(0);
spinner.start(); spinner.start();
// ros::Rate rate(5);
ModeClass mode_object; ModeClass mode_object;
ControlClass control_object; ControlClass control_object;
@ -23,11 +24,22 @@ int main(int argc, char **argv) {
// SelectionClass selection_object; // SelectionClass selection_object;
// YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml"); //reading yaml parameter(target point) // 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"); mode_object.set_Mode("GUIDED");
control_object.arm(); while(flag!=1){
control_object.takeoff(2); type = receiver_object.check_command();
sleep(2); if(type == "go"){
mode_object.set_Mode("GUIDED");
control_object.arm();
control_object.takeoff(2);
sleep(2);
flag = 1;
type ="";
}
}
// selection_object.choose_leader(); // selection_object.choose_leader();
// mission_object.start(); //default hover in goose formation // mission_object.start(); //default hover in goose formation
@ -36,28 +48,29 @@ int main(int argc, char **argv) {
// msg.type = "V_formation"; // msg.type = "V_formation";
// msg.mission = "fly2target"; // msg.mission = "fly2target";
std::string type = "",pre_type ="x";
while(ros::ok()){ while(ros::ok() && flag == 1){
type = receiver_object.check_command(); type = receiver_object.check_command();
if(pre_type == type){ if(pre_type == type){
type = "alreadly_set"; type = "alreadly_set";
ROS_INFO("set"); // ROS_INFO("set");
} }
// TODO: switch case
if(type == ""){ if(type == "init"){
select_formation.goose_formation(); select_formation.goose_formation();
ROS_INFO("init formation"); ROS_INFO("init formation");
}else if(type == "line"){ }else if(type == "line"){
select_formation.line_formation(); select_formation.line_formation();
pre_type ="line";
}else if(type == "row"){ }else if(type == "row"){
select_formation.row_formation(); select_formation.row_formation();
}else if(type == "v"){ pre_type ="row";
}else if(type == "v1"){ //PID error
select_formation.goose_formation(0,5); select_formation.goose_formation(0,5);
pre_type ="v"; pre_type ="v1";
}else if(type == "v2"){ }else if(type == "v2"){
select_formation.line_formation(-5,0); select_formation.line_formation(-5,0);
pre_type ="v2"; pre_type ="v2";
@ -70,6 +83,7 @@ int main(int argc, char **argv) {
}else if(type == "stop"){ }else if(type == "stop"){
select_formation.stop(); select_formation.stop();
}else if(type == "land"){ }else if(type == "land"){
// ROS_INFO("Land");
mode_object.set_Mode("LAND"); mode_object.set_Mode("LAND");
} }

5
class_model/src/mode.cpp
Unescape Escape View File

@ -26,8 +26,9 @@ int ModeClass::set_Mode(std::string mode) {
// ROS_INFO("setmode %s ok",mode.c_str()); // ROS_INFO("setmode %s ok",mode.c_str());
return 0; return 0;
}else{ }else{
ROS_ERROR("Failed SetMode %s",mode.c_str()); ROS_ERROR("Failed SetMode %s,retry...",mode.c_str());
return -1; // set_Mode(mode);
return 0;
} }
} }

6
class_model/src/requestData.cpp
Unescape Escape View File

@ -7,10 +7,12 @@ int command;
RequestClass::RequestClass() : node_handle_(""){ RequestClass::RequestClass() : node_handle_(""){
mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100, // mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100,
&RequestClass::Data_callback, this); // &RequestClass::Data_callback, this);
formation_data = node_handle_.subscribe("/Fly_Formation_reciver", 10, formation_data = node_handle_.subscribe("/Fly_Formation_reciver", 10,
&RequestClass::Message_callback, this); &RequestClass::Message_callback, this);
mqtt_data = node_handle_.subscribe("/uav_message", 100,
&RequestClass::Data_callback, this);
} }

2
class_model/src/tower.py
Unescape Escape View File

@ -35,7 +35,7 @@ def on_publish(self, userdata, mid):
connect_flag = False 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 = initialise_clients(mqtt_config["name"])
client.on_publish = on_publish client.on_publish = on_publish
client.on_connect = on_connect client.on_connect = on_connect

Write Preview
Loading…
Cancel
Save