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

ver2

Browse Source
master
dodo 4 years ago
parent 62bea117ee
commit b11cdcb859
9 changed files with 254 additions and 84 deletions
Show Stats Download Patch File Download Diff File

70
class_model/.vscode/settings.json vendored
Unescape Escape View File

@ -4,5 +4,73 @@
], ],
"python.analysis.extraPaths": [ "python.analysis.extraPaths": [
"/opt/ros/noetic/lib/python3/dist-packages" "/opt/ros/noetic/lib/python3/dist-packages"
] ],
"files.associations": {
"cctype": "cpp",
"clocale": "cpp",
"cmath": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"array": "cpp",
"atomic": "cpp",
"hash_map": "cpp",
"strstream": "cpp",
"bit": "cpp",
"*.tcc": "cpp",
"bitset": "cpp",
"chrono": "cpp",
"complex": "cpp",
"condition_variable": "cpp",
"cstdint": "cpp",
"deque": "cpp",
"list": "cpp",
"map": "cpp",
"set": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"vector": "cpp",
"exception": "cpp",
"algorithm": "cpp",
"functional": "cpp",
"iterator": "cpp",
"memory": "cpp",
"memory_resource": "cpp",
"numeric": "cpp",
"optional": "cpp",
"random": "cpp",
"ratio": "cpp",
"string": "cpp",
"string_view": "cpp",
"system_error": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"utility": "cpp",
"hash_set": "cpp",
"fstream": "cpp",
"initializer_list": "cpp",
"iomanip": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"istream": "cpp",
"limits": "cpp",
"mutex": "cpp",
"new": "cpp",
"ostream": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"streambuf": "cpp",
"thread": "cpp",
"cfenv": "cpp",
"cinttypes": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
"variant": "cpp",
"*.ipp": "cpp"
}
} }

5
class_model/include/class_model/requestData.h
Unescape Escape View File

@ -13,17 +13,20 @@ public:
virtual ~RequestClass(); virtual ~RequestClass();
global_location get_leader_GPS(); global_location get_leader_GPS();
float get_leader_heading(); float get_leader_heading();
std::string get_formation_message();
private: private:
// ROS NodeHandle // ROS NodeHandle
ros::NodeHandle node_handle_; ros::NodeHandle node_handle_;
void Data_callback(const std_msgs::String::ConstPtr &gps); void Data_callback(const std_msgs::String::ConstPtr &gps);
void Message_callback(const std_msgs::String::ConstPtr &message);
void jsonToInt(std::string data); void jsonToInt(std::string data);
void jsonToString(std::string data);
float heading; float heading;
// SUBSCRIBE // SUBSCRIBE
ros::Subscriber mqtt_data; ros::Subscriber mqtt_data;
ros::Subscriber formation_data;
}; };

2
class_model/include/class_model/select.h
Unescape Escape View File

@ -17,7 +17,7 @@ public:
void init_formation(float x=0,float y=0); void init_formation(float x=0,float y=0);
void line_formation(float x=0,float y=0); void line_formation(float x=0,float y=0);
void row_formation(); void row_formation(float x=0,float y=0);
void circle_formation(); void circle_formation();
void goose_formation(float x=0,float y=0); void goose_formation(float x=0,float y=0);
void protect_formation(float x=0,float y=0); void protect_formation(float x=0,float y=0);

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

@ -65,10 +65,50 @@ void FormationClass::leader(){
} }
} }
void FormationClass::leader1(float x,float y){
std::string command = "",pre_command = "";
std_msgs::String message;
leader_location=request_object.get_leader_GPS();
target_location.lon = leader_location.lon/100 + x;
target_location.lat = leader_location.lat/100 + y/1.1;
sleep(3);
flag = 0;
heading_status = 0;
message.data = "1";
while(true){
if(flag==0){
go2target(x,y);
}else{
next_command.publish(message);
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 FormationClass::follower1(int type){ void FormationClass::follower1(int type){
std::string command,pre_command = ""; std::string message,command,pre_command = "";
while(true){ while(true){
if(type == 0){ if(type == 0){
@ -86,7 +126,9 @@ void FormationClass::follower1(int type){
} }
command = receiver_object.check_command(); command = receiver_object.check_command();
if(command != pre_command){ message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation"); // ROS_INFO("change formation");
break; break;
} }
@ -103,7 +145,7 @@ void FormationClass::follower1(int type){
void FormationClass::follower2(int type){ void FormationClass::follower2(int type){
std::string command,pre_command = ""; std::string message,command,pre_command = "";
while(true){ while(true){
if(type == 0){ if(type == 0){
@ -121,7 +163,9 @@ void FormationClass::follower2(int type){
} }
command = receiver_object.check_command(); command = receiver_object.check_command();
if(command != pre_command){ message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation"); // ROS_INFO("change formation");
break; break;
} }
@ -134,7 +178,7 @@ void FormationClass::follower2(int type){
void FormationClass::follower3(int type){ void FormationClass::follower3(int type){
std::string command,pre_command = ""; std::string message,command,pre_command = "";
while(true){ while(true){
if(type == 0){ if(type == 0){
@ -153,7 +197,9 @@ void FormationClass::follower3(int type){
command = receiver_object.check_command(); command = receiver_object.check_command();
if(command != pre_command){ message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation"); // ROS_INFO("change formation");
break; break;
} }
@ -166,7 +212,7 @@ void FormationClass::follower3(int type){
void FormationClass::follower4(int type){ void FormationClass::follower4(int type){
std::string command,pre_command = ""; std::string message,command,pre_command = "";
while(true){ while(true){
if(type == 0){ if(type == 0){
@ -184,12 +230,13 @@ void FormationClass::follower4(int type){
} }
command = receiver_object.check_command(); command = receiver_object.check_command();
if(command != pre_command){ message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation"); // ROS_INFO("change formation");
break; break;
} }
pre_command = command; pre_command = command;
} }
@ -197,7 +244,7 @@ void FormationClass::follower4(int type){
void FormationClass::follower5(int type){ void FormationClass::follower5(int type){
std::string command,pre_command = ""; std::string message,command,pre_command = "";
while(true){ while(true){
if(type == 0){ if(type == 0){
@ -215,7 +262,9 @@ void FormationClass::follower5(int type){
} }
command = receiver_object.check_command(); command = receiver_object.check_command();
if(command != pre_command){ message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation"); // ROS_INFO("change formation");
break; break;
} }
@ -228,7 +277,7 @@ void FormationClass::follower5(int type){
void FormationClass::sph_follower1(int type){ void FormationClass::sph_follower1(int type){
std::string command,pre_command = ""; std::string message,command,pre_command = "";
while(true){ while(true){
if(type == 0){ if(type == 0){
@ -245,7 +294,9 @@ void FormationClass::sph_follower1(int type){
command = receiver_object.check_command(); command = receiver_object.check_command();
if(command != pre_command){ message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation"); // ROS_INFO("change formation");
break; break;
} }
@ -258,7 +309,7 @@ void FormationClass::sph_follower1(int type){
void FormationClass::sph_follower2(int type){ void FormationClass::sph_follower2(int type){
std::string command,pre_command = ""; std::string message,command,pre_command = "";
while(true){ while(true){
if(type == 0){ if(type == 0){
@ -275,12 +326,13 @@ void FormationClass::sph_follower2(int type){
command = receiver_object.check_command(); command = receiver_object.check_command();
if(command != pre_command){ message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation"); // ROS_INFO("change formation");
break; break;
} }
pre_command = command; pre_command = command;
} }
@ -288,7 +340,7 @@ void FormationClass::sph_follower2(int type){
void FormationClass::sph_follower3(int type){ void FormationClass::sph_follower3(int type){
std::string command,pre_command = ""; std::string message,command,pre_command = "";
while(true){ while(true){
if(type == 0){ if(type == 0){
@ -305,7 +357,9 @@ void FormationClass::sph_follower3(int type){
command = receiver_object.check_command(); command = receiver_object.check_command();
if(command != pre_command){ message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation"); // ROS_INFO("change formation");
break; break;
} }
@ -317,7 +371,7 @@ void FormationClass::sph_follower3(int type){
void FormationClass::sph_follower4(int type){ void FormationClass::sph_follower4(int type){
std::string command,pre_command = ""; std::string message,command,pre_command = "";
while(true){ while(true){
if(type == 0){ if(type == 0){
@ -334,7 +388,9 @@ void FormationClass::sph_follower4(int type){
command = receiver_object.check_command(); command = receiver_object.check_command();
if(command != pre_command){ message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation"); // ROS_INFO("change formation");
break; break;
} }
@ -345,43 +401,7 @@ void FormationClass::sph_follower4(int type){
} }
void FormationClass::leader1(float x,float y){
std::string command = "",pre_command = "";
leader_location=request_object.get_leader_GPS();
target_location.lon = leader_location.lon/100 + x;
target_location.lat = leader_location.lat/100 + y/1.1;
sleep(3);
flag = 0;
heading_status = 0;
while(true){
if(flag==0){
go2target(x,y);
}else{
next_command.publish("1");
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 FormationClass::calculate_position(float k,float theta,int direction){ void FormationClass::calculate_position(float k,float theta,int direction){

16
class_model/src/local_mqtt_pub_data_to_ros.py
Unescape Escape View File

@ -16,6 +16,12 @@ def ros_pub(dataJson):
rate.sleep() rate.sleep()
# print(f"publish data {data}") # print(f"publish data {data}")
def PubMessage(dataJson):
global publisher1
# data = json.loads(dataJson)
publisher1.publish(dataJson) #將date字串發布到topic
rate.sleep()
# MQTT # MQTT
def initialise_clients(clientname): def initialise_clients(clientname):
# callback assignment # callback assignment
@ -26,18 +32,21 @@ def initialise_clients(clientname):
def on_connect(client, userdata, flags, rc): def on_connect(client, userdata, flags, rc):
print("Connected with result code " + str(rc)) print("Connected with result code " + str(rc))
# client.subscribe(mqtt_config["topic"]) # client.subscribe(mqtt_config["topic"])
client.subscribe("data/imu") client.subscribe("data/sensor")
client.subscribe("data/message")
def on_message(client, userdata, msg): def on_message(client, userdata, msg):
# print(f"got {msg.payload.decode('utf-8')}") # print(f"got {msg.payload.decode('utf-8')}")
# print(f"msg.topic {msg.payload.decode('utf-8')}") # print(f"msg.topic {msg.payload.decode('utf-8')}")
ros_pub(msg.payload.decode('utf-8')) ros_pub(msg.payload.decode('utf-8'))
PubMessage(msg.payload.decode('utf-8'))
if __name__ == '__main__': if __name__ == '__main__':
# Mqtt # Mqtt
mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/imu"} mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor", "topic1":"data/message"}
client = initialise_clients("123456") client = initialise_clients("123456")
client.on_connect = on_connect client.on_connect = on_connect
client.on_message = on_message client.on_message = on_message
@ -50,7 +59,10 @@ if __name__ == '__main__':
rospy.init_node(Mqtt_Node) rospy.init_node(Mqtt_Node)
# initialize Ros node # initialize Ros node
topicName = 'uav_message' topicName = 'uav_message'
topicName1 = 'formation_message'
publisher = rospy.Publisher(topicName,String,queue_size=10) publisher = rospy.Publisher(topicName,String,queue_size=10)
publisher1 = rospy.Publisher(topicName1,String,queue_size=10)
rate = rospy.Rate(10) rate = rospy.Rate(10)

22
class_model/src/local_mqtt_sub_data_from_ros.py
Unescape Escape View File

@ -16,7 +16,7 @@ from sensor_msgs.msg import Imu
from sensor_msgs.msg import Range from sensor_msgs.msg import Range
from geometry_msgs.msg import Vector3 from geometry_msgs.msg import Vector3
mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/imu"} mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor", "topic1": "data/message"}
data ={} data ={}
# Ros # Ros
def callBack_imu(IMU): def callBack_imu(IMU):
@ -57,6 +57,15 @@ def callBack_state(state):
dataGpsUpdate = {"mode": mode} dataGpsUpdate = {"mode": mode}
data.update(dataGpsUpdate) data.update(dataGpsUpdate)
def callBack_message(data):
MessageUpdate = {"message": data}
data.update(MessageUpdate)
dataMessageFormate = json.dumps(data)
mqtt_Pub_message(message=dataMessageFormate)
# get parameter
def GetParam(self,param_name): def GetParam(self,param_name):
param = self.get_param_srv(param_name) param = self.get_param_srv(param_name)
if param.success: if param.success:
@ -90,6 +99,14 @@ def mqtt_Pub(message, topics = mqtt_config["topic"], waitForAck=False):
time.sleep(0.25) time.sleep(0.25)
client.topic_ack.remove(mid) client.topic_ack.remove(mid)
def mqtt_Pub_message(message, topics = mqtt_config["topic1"], waitForAck=False):
mid = client.publish(topics, message, 0)[1]
# print(f"just published {message} to topic")
if waitForAck:
while mid not in client.topic_ack:
print("wait for ack")
time.sleep(0.25)
client.topic_ack.remove(mid)
def on_publish(self, userdata, mid): def on_publish(self, userdata, mid):
client.topic_ack.append(mid) client.topic_ack.append(mid)
@ -122,8 +139,9 @@ if __name__ == '__main__':
# subscriber = rospy.Subscriber('/mavros/imu/data_raw',Imu,callBack_imu) #從topic獲取string再呼叫callback # subscriber = rospy.Subscriber('/mavros/imu/data_raw',Imu,callBack_imu) #從topic獲取string再呼叫callback
subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/global',NavSatFix,callBack_gps) #從topic獲取string再呼叫callback subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/global',NavSatFix,callBack_gps) #從topic獲取string再呼叫callback
subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg',Float64,callBack_compass_hdg) #從topic獲取string再呼叫callback subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg',Float64,callBack_compass_hdg) #從topic獲取string再呼叫callback
subscriber = rospy.Subscriber(ros_namespace+"/mavros/next_command",String,callBack_message)
#subscriber = rospy.Subscriber(ros_namespace+'/mavros/mavros_msgs/State',Header,callBack_state) #subscriber = rospy.Subscriber(ros_namespace+'/mavros/mavros_msgs/State',Header,callBack_state)
ID = rospy.get_param(ros_namespace+'/leader/droneID') # ID = rospy.get_param(ros_namespace+'/leader/droneID')
# subscriber = rospy.Subscriber('/mavros/rangefinder/rangefinder',Range,callBack_rng) #從topic獲取string再呼叫callback # subscriber = rospy.Subscriber('/mavros/rangefinder/rangefinder',Range,callBack_rng) #從topic獲取string再呼叫callback
# ID = rospy.GetParam("droneID") # ID = rospy.GetParam("droneID")

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

@ -28,29 +28,29 @@ int main(int argc, char **argv) {
type = receiver_object.check_command(); type = receiver_object.check_command();
// // ROS_INFO("%s",type.c_str()); // // ROS_INFO("%s",type.c_str());
if(type == "" || type == "init"){ // if(type == "" || 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();
// ROS_INFO("line foemation"); // // ROS_INFO("line foemation");
}else if(type == "row"){ // }else if(type == "row"){
select_formation.row_formation(); // select_formation.row_formation();
}else if(type == "circle"){ // }else if(type == "circle"){
select_formation.circle_formation(); // select_formation.circle_formation();
}else if(type == "stop"){ // }else if(type == "stop"){
select_formation.stop(); // select_formation.stop();
}else if(type == "land"){ // }else if(type == "land"){
mode_object.set_Mode("LAND"); // mode_object.set_Mode("LAND");
} // }
//mission_object.cruise(20,20); //mission_object.cruise(20,20);
// select_formation.goose_formation(); select_formation.goose_formation(5,0);
// }else if (type == "hex"){ // }else if (type == "hex"){
// select_formation.Hex_formation(); // select_formation.Hex_formation();
// } // }
// select_formation.Hex_formation(); select_formation.row_formation(-5,0);
sleep(0.5); sleep(0.5);
// command_object.set_velocity(0 ,0 ,0,0,1); // command_object.set_velocity(0 ,0 ,0,0,1);

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

@ -2,12 +2,15 @@
#include <cstdlib> #include <cstdlib>
#include <iostream> #include <iostream>
global_location leader_position; global_location leader_position;
std::string command;
RequestClass::RequestClass() : node_handle_(""){ RequestClass::RequestClass() : node_handle_(""){
mqtt_data = node_handle_.subscribe("/uav_message", 10, mqtt_data = node_handle_.subscribe("/uav_message", 10,
&RequestClass::Data_callback, this); &RequestClass::Data_callback, this);
formation_data = node_handle_.subscribe("/formation_message", 10,
&RequestClass::Message_callback, this);
} }
@ -20,6 +23,13 @@ void RequestClass::Data_callback(const std_msgs::String::ConstPtr &sensor) {
} }
void RequestClass::Message_callback(const std_msgs::String::ConstPtr &message) {
std::string data = message->data;
}
global_location RequestClass::get_leader_GPS(){ global_location RequestClass::get_leader_GPS(){
return leader_position; return leader_position;
@ -30,6 +40,11 @@ float RequestClass::get_leader_heading(){
return heading; return heading;
} }
std::string RequestClass::get_formation_message(){
return command;
}
void RequestClass::jsonToInt(std::string data){ void RequestClass::jsonToInt(std::string data){
std::string lat,lon,degree; std::string lat,lon,degree;
@ -68,6 +83,40 @@ void RequestClass::jsonToInt(std::string data){
// std::cout<<sizeof(data)<<std::endl;
//std::cout<<data<<std::endl;
// ROS_INFO("leader_GPS [%f,%f]", leader_position.lat, leader_position.lon);
// ROS_INFO("leader_heading: %f",heading);
}
void RequestClass::jsonToString(std::string data){
std::string list[1]={""};
int j = 0;
for(int i=0;i<data.length();i++){
if(data[i] == ':'){
i+=3;
while(data[i] != ','){
if(data[i] == ',' || data[i] == '}'){
j++;
break;
}
list[j]=list[j]+data[i];
i++;
if(j>sizeof(list)){
break;
}
}
}
}
command = list[1];
// std::cout<<sizeof(data)<<std::endl; // std::cout<<sizeof(data)<<std::endl;
//std::cout<<data<<std::endl; //std::cout<<data<<std::endl;
// ROS_INFO("leader_GPS [%f,%f]", leader_position.lat, leader_position.lon); // ROS_INFO("leader_GPS [%f,%f]", leader_position.lat, leader_position.lon);

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

@ -46,7 +46,7 @@ void SelectClass::goose_formation(float x ,float y){
counter = 0; counter = 0;
if(param_object.getID() == 1){ if(param_object.getID() == 1){
formation_object.leader1(); formation_object.leader1(x,y);
}else if(param_object.getID() == 2){ }else if(param_object.getID() == 2){
formation_object.follower1(counter); formation_object.follower1(counter);
}else if(param_object.getID() == 3){ }else if(param_object.getID() == 3){
@ -88,11 +88,11 @@ void SelectClass::line_formation(float x ,float y){
} }
void SelectClass::row_formation(){ void SelectClass::row_formation(float x ,float y){
counter = 2; counter = 2;
if(param_object.getID() == 1){ if(param_object.getID() == 1){
formation_object.leader1(); formation_object.leader1(x,y);
}else if(param_object.getID() == 2){ }else if(param_object.getID() == 2){
formation_object.follower1(counter); formation_object.follower1(counter);
}else if(param_object.getID() == 3){ }else if(param_object.getID() == 3){

Write Preview
Loading…
Cancel
Save