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

ver2

Browse Source
This commit is contained in:
Xuan0319
2022-11-15 21:18:37 +08:00
parent 62bea117ee
commit b11cdcb859
9 changed files with 479 additions and 309 deletions
Download Patch File Download Diff File Expand all files Collapse all files
class_model/.vscode/settings.json
Vendored
+69 -1
View File
@@ -4,5 +4,73 @@
],
"python.analysis.extraPaths": [
"/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"
}
}
class_model/include/class_model/requestData.h
+4 -1
View File
@@ -13,17 +13,20 @@ public:
virtual ~RequestClass();
global_location get_leader_GPS();
float get_leader_heading();
std::string get_formation_message();
private:
// ROS NodeHandle
ros::NodeHandle node_handle_;
void Data_callback(const std_msgs::String::ConstPtr &gps);
void Message_callback(const std_msgs::String::ConstPtr &message);
void jsonToInt(std::string data);
void jsonToString(std::string data);
float heading;
// SUBSCRIBE
ros::Subscriber mqtt_data;
ros::Subscriber formation_data;
};
class_model/include/class_model/select.h
+1 -1
View File
@@ -17,7 +17,7 @@ public:
void init_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 goose_formation(float x=0,float y=0);
void protect_formation(float x=0,float y=0);
class_model/src/formation.cpp
+301 -281
View File
@@ -65,300 +65,23 @@ void FormationClass::leader(){
}
}
void FormationClass::follower1(int type){
std::string command,pre_command = "";
while(true){
if(type == 0){
calculate_position(4,30);
}else if(type == 1){
calculate_position(4,0);
}else if(type == 2){
calculate_position(4,90);
}else if(type == 3){
calculate_position(4,120);
}else if(type == 4){
calculate_position(2.5,120);
}else if(type == 5){
calculate_position(4,60,60);
}
command = receiver_object.check_command();
if(command != pre_command){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
// sleep(2);
// mode_object.set_Mode("LAND");
}
void FormationClass::follower2(int type){
std::string command,pre_command = "";
while(true){
if(type == 0){
calculate_position(4,-30);
}else if(type == 1){
calculate_position(8,0);
}else if(type == 2){
calculate_position(4,-90);
}else if(type == 3){
calculate_position(4,-120);
}else if(type == 4){
calculate_position(2.5,-120);
}else if(type == 5){
calculate_position(4,-60,-60);
}
command = receiver_object.check_command();
if(command != pre_command){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::follower3(int type){
std::string command,pre_command = "";
while(true){
if(type == 0){
calculate_position(8,30);
}else if(type == 1){
calculate_position(12,0);
}else if(type == 2){
calculate_position(8,90);
}else if(type == 3){
calculate_position(6,160);
}else if(type == 4){
calculate_position(2.5,60);
}else if(type == 5){
calculate_position(6.93,30,120);
}
command = receiver_object.check_command();
if(command != pre_command){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::follower4(int type){
std::string command,pre_command = "";
while(true){
if(type == 0){
calculate_position(8,-30);
}else if(type == 1){
calculate_position(16,0);
}else if(type == 2){
calculate_position(8,-90);
}else if(type == 3){
calculate_position(6,-160);
}else if(type == 4){
calculate_position(2.5,-60);
}else if(type == 5){
calculate_position(6.93,-30,-120);
}
command = receiver_object.check_command();
if(command != pre_command){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::follower5(int type){
std::string command,pre_command = "";
while(true){
if(type == 0){
calculate_position(12,-30);
}else if(type == 1){
calculate_position(20,0);
}else if(type == 2){
calculate_position(6,-90);
}else if(type == 3){
calculate_position(6,180);
}else if(type == 4){
calculate_position(3,0);
}else if(type == 5){
calculate_position(8,0,180);
}
command = receiver_object.check_command();
if(command != pre_command){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::sph_follower1(int type){
std::string command,pre_command = "";
while(true){
if(type == 0){
spherical_coordinate(4,30,30);
// }else if(type == 1){
// calculate_position(12,0,1);
// }else if(type == 2){
// calculate_position(8,90,1);
// }else if(type == 3){
// calculate_position(6,160,1);
// }else if(type == 4){
// calculate_position(2.5,60,1);
}
command = receiver_object.check_command();
if(command != pre_command){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::sph_follower2(int type){
std::string command,pre_command = "";
while(true){
if(type == 0){
spherical_coordinate(4,-30,30);
// }else if(type == 1){
// calculate_position(12,0,1);
// }else if(type == 2){
// calculate_position(8,90,1);
// }else if(type == 3){
// calculate_position(6,160,1);
// }else if(type == 4){
// calculate_position(2.5,60,1);
}
command = receiver_object.check_command();
if(command != pre_command){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::sph_follower3(int type){
std::string command,pre_command = "";
while(true){
if(type == 0){
spherical_coordinate(4,30,-30);
// }else if(type == 1){
// calculate_position(12,0,1);
// }else if(type == 2){
// calculate_position(8,90,1);
// }else if(type == 3){
// calculate_position(6,160,1);
// }else if(type == 4){
// calculate_position(2.5,60,1);
}
command = receiver_object.check_command();
if(command != pre_command){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::sph_follower4(int type){
std::string command,pre_command = "";
while(true){
if(type == 0){
spherical_coordinate(4,-30,-30);
// }else if(type == 1){
// calculate_position(12,0,1);
// }else if(type == 2){
// calculate_position(8,90,1);
// }else if(type == 3){
// calculate_position(6,160,1);
// }else if(type == 4){
// calculate_position(2.5,60,1);
}
command = receiver_object.check_command();
if(command != pre_command){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
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("1");
next_command.publish(message);
break;
}
@@ -383,6 +106,303 @@ void FormationClass::leader1(float x,float y){
}
}
void FormationClass::follower1(int type){
std::string message,command,pre_command = "";
while(true){
if(type == 0){
calculate_position(4,30);
}else if(type == 1){
calculate_position(4,0);
}else if(type == 2){
calculate_position(4,90);
}else if(type == 3){
calculate_position(4,120);
}else if(type == 4){
calculate_position(2.5,120);
}else if(type == 5){
calculate_position(4,60,60);
}
command = receiver_object.check_command();
message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
// sleep(2);
// mode_object.set_Mode("LAND");
}
void FormationClass::follower2(int type){
std::string message,command,pre_command = "";
while(true){
if(type == 0){
calculate_position(4,-30);
}else if(type == 1){
calculate_position(8,0);
}else if(type == 2){
calculate_position(4,-90);
}else if(type == 3){
calculate_position(4,-120);
}else if(type == 4){
calculate_position(2.5,-120);
}else if(type == 5){
calculate_position(4,-60,-60);
}
command = receiver_object.check_command();
message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::follower3(int type){
std::string message,command,pre_command = "";
while(true){
if(type == 0){
calculate_position(8,30);
}else if(type == 1){
calculate_position(12,0);
}else if(type == 2){
calculate_position(8,90);
}else if(type == 3){
calculate_position(6,160);
}else if(type == 4){
calculate_position(2.5,60);
}else if(type == 5){
calculate_position(6.93,30,120);
}
command = receiver_object.check_command();
message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::follower4(int type){
std::string message,command,pre_command = "";
while(true){
if(type == 0){
calculate_position(8,-30);
}else if(type == 1){
calculate_position(16,0);
}else if(type == 2){
calculate_position(8,-90);
}else if(type == 3){
calculate_position(6,-160);
}else if(type == 4){
calculate_position(2.5,-60);
}else if(type == 5){
calculate_position(6.93,-30,-120);
}
command = receiver_object.check_command();
message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::follower5(int type){
std::string message,command,pre_command = "";
while(true){
if(type == 0){
calculate_position(12,-30);
}else if(type == 1){
calculate_position(20,0);
}else if(type == 2){
calculate_position(6,-90);
}else if(type == 3){
calculate_position(6,180);
}else if(type == 4){
calculate_position(3,0);
}else if(type == 5){
calculate_position(8,0,180);
}
command = receiver_object.check_command();
message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::sph_follower1(int type){
std::string message,command,pre_command = "";
while(true){
if(type == 0){
spherical_coordinate(4,30,30);
// }else if(type == 1){
// calculate_position(12,0,1);
// }else if(type == 2){
// calculate_position(8,90,1);
// }else if(type == 3){
// calculate_position(6,160,1);
// }else if(type == 4){
// calculate_position(2.5,60,1);
}
command = receiver_object.check_command();
message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::sph_follower2(int type){
std::string message,command,pre_command = "";
while(true){
if(type == 0){
spherical_coordinate(4,-30,30);
// }else if(type == 1){
// calculate_position(12,0,1);
// }else if(type == 2){
// calculate_position(8,90,1);
// }else if(type == 3){
// calculate_position(6,160,1);
// }else if(type == 4){
// calculate_position(2.5,60,1);
}
command = receiver_object.check_command();
message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::sph_follower3(int type){
std::string message,command,pre_command = "";
while(true){
if(type == 0){
spherical_coordinate(4,30,-30);
// }else if(type == 1){
// calculate_position(12,0,1);
// }else if(type == 2){
// calculate_position(8,90,1);
// }else if(type == 3){
// calculate_position(6,160,1);
// }else if(type == 4){
// calculate_position(2.5,60,1);
}
command = receiver_object.check_command();
message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::sph_follower4(int type){
std::string message,command,pre_command = "";
while(true){
if(type == 0){
spherical_coordinate(4,-30,-30);
// }else if(type == 1){
// calculate_position(12,0,1);
// }else if(type == 2){
// calculate_position(8,90,1);
// }else if(type == 3){
// calculate_position(6,160,1);
// }else if(type == 4){
// calculate_position(2.5,60,1);
}
command = receiver_object.check_command();
message = request_object.get_formation_message();
if(command != pre_command || message == "1"){
// ROS_INFO("change formation");
break;
}
pre_command = command;
}
}
void FormationClass::calculate_position(float k,float theta,int direction){
class_model/src/local_mqtt_pub_data_to_ros.py
+14 -2
View File
@@ -16,6 +16,12 @@ def ros_pub(dataJson):
rate.sleep()
# print(f"publish data {data}")
def PubMessage(dataJson):
global publisher1
# data = json.loads(dataJson)
publisher1.publish(dataJson) #將date字串發布到topic
rate.sleep()
# MQTT
def initialise_clients(clientname):
# callback assignment
@@ -26,18 +32,21 @@ def initialise_clients(clientname):
def on_connect(client, userdata, flags, rc):
print("Connected with result code " + str(rc))
# client.subscribe(mqtt_config["topic"])
client.subscribe("data/imu")
client.subscribe("data/sensor")
client.subscribe("data/message")
def on_message(client, userdata, msg):
# print(f"got {msg.payload.decode('utf-8')}")
# print(f"msg.topic {msg.payload.decode('utf-8')}")
ros_pub(msg.payload.decode('utf-8'))
PubMessage(msg.payload.decode('utf-8'))
if __name__ == '__main__':
# 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.on_connect = on_connect
client.on_message = on_message
@@ -50,7 +59,10 @@ if __name__ == '__main__':
rospy.init_node(Mqtt_Node)
# initialize Ros node
topicName = 'uav_message'
topicName1 = 'formation_message'
publisher = rospy.Publisher(topicName,String,queue_size=10)
publisher1 = rospy.Publisher(topicName1,String,queue_size=10)
rate = rospy.Rate(10)
class_model/src/local_mqtt_sub_data_from_ros.py
+20 -2
View File
@@ -16,7 +16,7 @@ from sensor_msgs.msg import Imu
from sensor_msgs.msg import Range
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 ={}
# Ros
def callBack_imu(IMU):
@@ -57,6 +57,15 @@ def callBack_state(state):
dataGpsUpdate = {"mode": mode}
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):
param = self.get_param_srv(param_name)
if param.success:
@@ -90,6 +99,14 @@ def mqtt_Pub(message, topics = mqtt_config["topic"], waitForAck=False):
time.sleep(0.25)
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):
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(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/next_command",String,callBack_message)
#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
# ID = rospy.GetParam("droneID")
class_model/src/main.cpp
+17 -17
View File
@@ -28,29 +28,29 @@ int main(int argc, char **argv) {
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");
}
// 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();
select_formation.goose_formation(5,0);
// }else if (type == "hex"){
// select_formation.Hex_formation();
// }
// select_formation.Hex_formation();
select_formation.row_formation(-5,0);
sleep(0.5);
// command_object.set_velocity(0 ,0 ,0,0,1);
class_model/src/requestData.cpp
+50 -1
View File
@@ -2,12 +2,15 @@
#include <cstdlib>
#include <iostream>
global_location leader_position;
global_location leader_position;
std::string command;
RequestClass::RequestClass() : node_handle_(""){
mqtt_data = node_handle_.subscribe("/uav_message", 10,
&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(){
return leader_position;
@@ -30,6 +40,11 @@ float RequestClass::get_leader_heading(){
return heading;
}
std::string RequestClass::get_formation_message(){
return command;
}
void RequestClass::jsonToInt(std::string data){
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<<data<<std::endl;
// ROS_INFO("leader_GPS [%f,%f]", leader_position.lat, leader_position.lon);
class_model/src/select.cpp
+3 -3
View File
@@ -46,7 +46,7 @@ void SelectClass::goose_formation(float x ,float y){
counter = 0;
if(param_object.getID() == 1){
formation_object.leader1();
formation_object.leader1(x,y);
}else if(param_object.getID() == 2){
formation_object.follower1(counter);
}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;
if(param_object.getID() == 1){
formation_object.leader1();
formation_object.leader1(x,y);
}else if(param_object.getID() == 2){
formation_object.follower1(counter);
}else if(param_object.getID() == 3){