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

add 3 bionic drones subscriber

Browse Source
This commit is contained in:
Xuan0319
2023-02-11 23:11:03 +08:00
parent 5808443273
commit 623c939094
7 changed files with 268 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
class_model/include/class_model/gps_location.h
+2
View File
@@ -3,6 +3,8 @@
typedef struct global_location{
double lat,lon,alt;
float heading;
int ID;
}global_location;
#endif // GOBAL_LOCATION_H
class_model/include/class_model/requestData.h
+14
View File
@@ -7,6 +7,7 @@
#include <string>
#include <class_model/gps_location.h>
#include <nlohmann/json.hpp>
#include <algorithm>
// #include "rapidjson/document.h"
using json = nlohmann::json;
@@ -17,6 +18,9 @@ public:
RequestClass();
virtual ~RequestClass();
global_location get_leader_GPS();
global_location get_self_GPS();
global_location get_M1_GPS();
global_location get_M2_GPS();
float get_leader_heading();
int get_formation_message();
json get_data();
@@ -26,17 +30,27 @@ private:
ros::NodeHandle node_handle_;
void Data_callback(const std_msgs::String::ConstPtr &gps);
void self_callback(const std_msgs::String::ConstPtr &gps);
void M1_callback(const std_msgs::String::ConstPtr &gps);
void M2_callback(const std_msgs::String::ConstPtr &gps);
void Message_callback(const std_msgs::String::ConstPtr &message);
void jsonToString(std::string data);
void L_INFO(std::string data);
void Bio_INFO(std::string data);
float heading;
global_location leader_position;
global_location self_position,M1_position,M2_position;
std::array <int,3> ID {1,2,3};
// rapidjson::Document document;
json j_data;
int self_ID,drone_ID;
// SUBSCRIBE
ros::Subscriber mqtt_data;
ros::Subscriber formation_data;
ros::Subscriber self_data;
ros::Subscriber member1_data;
ros::Subscriber member2_data;
};
class_model/launch/mqtt_old.launch
+1 -1
View File
@@ -11,7 +11,7 @@
</group>
<group>
<node pkg="class_model" type="command.py" name="command" output="screen">
<node pkg="class_model" type="mqttCmdMain.py" name="command" output="screen">
</node>
</group>
class_model/src/local_mqtt_pub_data_to_ros.py
Executable
+67
View File
@@ -0,0 +1,67 @@
#!/usr/bin/env python3
#coding:utf-8
# license removed for brevity
import ssl
import rospy
from std_msgs.msg import String
import paho.mqtt.client as mqtt
import json
# Ros
def ros_pub(dataJson):
global publisher, rate
# data = json.loads(dataJson)
publisher.publish(dataJson) #將date字串發布到topic
rate.sleep()
# print(f"publish data {data}")
# MQTT
def initialise_clients(clientname):
# callback assignment
initialise_client = mqtt.Client(clientname, False)
initialise_client.topic_ack = []
return initialise_client
def on_connect(client, userdata, flags, rc):
print("Connected with result code " + str(rc))
# client.subscribe(mqtt_config["topic"])
client.subscribe("data/imu")
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'))
if __name__ == '__main__':
# Mqtt
mqtt_config = {"host": "140.120.31.133", "port": 1883, "topic": "data/imu"}
client = initialise_clients("123456")
client.on_connect = on_connect
client.on_message = on_message
client.connect(mqtt_config["host"], mqtt_config["port"], 60)
# Ros
Mqtt_Node = 'publisher_py'
rospy.init_node(Mqtt_Node)
# initialize Ros node
topicName = 'uav_message'
publisher = rospy.Publisher(topicName,String,queue_size=10)
rate = rospy.Rate(10)
client.loop_forever()
# mqtt connect code list
# 0: Connection successful
# 1: Connection refused – incorrect protocol version
# 2: Connection refused – invalid client identifier
# 3: Connection refused – server unavailable
# 4: Connection refused – bad username or password
# 5: Connection refused – not authorised
# 6-255: Currently unused.
class_model/src/local_mqtt_sub_data_from_ros.py
Executable
+114
View File
@@ -0,0 +1,114 @@
#!/usr/bin/env python3
#coding:utf-8
import sys
import json
import paho.mqtt.client as mqtt
from traceback import print_tb
import rospy
from std_msgs.msg import String
from std_msgs.msg import Float64
from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import Imu
from sensor_msgs.msg import Range
from geometry_msgs.msg import Vector3
mqtt_config = {"host": "140.120.31.133", "port": 1883, "topic": "data/imu"}
data ={}
# Ros
def callBack_imu(IMU):
gyro_x=str(IMU.linear_acceleration.x)
gyro_y=str(IMU.linear_acceleration.y)
gyro_z=str(IMU.linear_acceleration.z)
accel_x=str(IMU.angular_velocity.x)
accel_y=str(IMU.angular_velocity.y)
accel_z=str(IMU.angular_velocity.z)
dataImuUpdate = {"gyro_x": gyro_x, "gyro_y": gyro_y,"gyro_z":gyro_z, "accel_x": accel_x, "accel_y": accel_y, "accel_z": accel_z}
data.update(dataImuUpdate)
# print ('gyro_x:'+gyro_x+'\n'+'gyro_y:'+gyro_y+'\n'+'gyro_z:'+gyro_z +'\n')
# print ('accel_x:'+accel_x+'\n'+'accel_y:'+accel_y+'\n'+'accel_z:'+accel_z +'\n')
def callBack_gps(GPS):
lat=str(int(GPS.latitude*10000000))
lon=str(int(GPS.longitude*10000000))
dataGpsUpdate = {"lat": lat, "lon": lon}
data.update(dataGpsUpdate)
# dataJsonFormate = json.dumps(data)
# mqtt_Pub(message=dataJsonFormate)
# print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
def callBack_compass_hdg(Compass):
heading = str(int(Compass.data*100))
dataGpsUpdate = {"heading": heading}
data.update(dataGpsUpdate)
dataJsonFormate = json.dumps(data)
mqtt_Pub(message=dataJsonFormate)
# def callBack_velocity(velocity):
# Vy=str(int(velocity.latitude*100))
# Vx=str(int(velocity.longitude*100))
# dataGpsUpdate = {"Vx": Vx, "Vy": Vy}
# data.update(dataGpsUpdate)
# dataJsonFormate = json.dumps(data)
# mqtt_Pub(message=dataJsonFormate)
# MQTT
def initialise_clients(clientname):
# callback assignment
initialise_client = mqtt.Client(clientname, False)
initialise_client.topic_ack = []
return initialise_client
# publish a message
def mqtt_Pub(message, topics = mqtt_config["topic"], 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)
def on_connect(self, userdata, flags, rc):
print("Connected with result code " + str(rc))
if __name__ == '__main__':
# Mqtt
client = initialise_clients("client1")
client.on_publish = on_publish
client.on_connect = on_connect
client.connect(mqtt_config["host"], mqtt_config["port"], 60)
client.loop_start()
# Ros
nodeName = 'save_data_py'
rospy.init_node(nodeName)
ros_namespace = ""
if not rospy.has_param("namespace"):
print("using default namespace")
else:
rospy.get_param("namespace", ros_namespace)
print("using namespace "+ros_namespace)
ros_namespace="/drone1"
# topicName = 'data_topic'
# 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/leader_velocity',NavSatFix,callBack_velocity) #從topic獲取string再呼叫callback
# subscriber = rospy.Subscriber('/mavros/rangefinder/rangefinder',Range,callBack_rng) #從topic獲取string再呼叫callback
rospy.spin()
class_model/src/requestData.cpp
+59 -4
View File
@@ -7,13 +7,19 @@ 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);
mqtt_data = node_handle_.subscribe("/uav_message", 100,
&RequestClass::Data_callback, this);
self_data = node_handle_.subscribe("/self_data_message", 100,
&RequestClass::self_callback, this); //test bionic
member1_data = node_handle_.subscribe("/member1_data_message", 100,
&RequestClass::M1_callback, this);
member2_data = node_handle_.subscribe("/member2_data_message", 100,
&RequestClass::M2_callback, this);
}
RequestClass::~RequestClass() { ros::shutdown(); }
@@ -83,4 +89,53 @@ void RequestClass::L_INFO(std::string data){
// std::cout << leader_position.lon << std::endl;
// std::cout << leader_position.alt << std::endl;
// std::cout << heading << std::endl;
}
/*test biomic*/
void RequestClass::self_callback(const std_msgs::String::ConstPtr &sensor) {
Bio_INFO(sensor->data);
}
void RequestClass::M1_callback(const std_msgs::String::ConstPtr &sensor) {
Bio_INFO(sensor->data);
}
void RequestClass::M2_callback(const std_msgs::String::ConstPtr &sensor) {
Bio_INFO(sensor->data);
}
void RequestClass::Bio_INFO(std::string data){
j_data = json::parse(data); //open source
drone_ID = j_data["ID"];
std::remove(ID.begin(),ID.end(),self_ID);
if (drone_ID == self_ID){
self_position.lat=j_data["lat"];
self_position.lon=j_data["lon"];
self_position.alt=j_data["alt"];
self_position.heading = j_data["heading"];
}else if (drone_ID == ID[0]){
M1_position.lat=j_data["lat"];
M1_position.lon=j_data["lon"];
M1_position.alt=j_data["alt"];
M1_position.heading = j_data["heading"];
}else if (drone_ID == ID[1]){
M2_position.lat=j_data["lat"];
M2_position.lon=j_data["lon"];
M2_position.alt=j_data["alt"];
M2_position.heading = j_data["heading"];
}
}
global_location RequestClass::get_self_GPS(){
return self_position;
}
global_location RequestClass::get_M1_GPS(){
return M1_position;
}
global_location RequestClass::get_M2_GPS(){
return M2_position;
}
class_model/src/test_lib.cpp
+11 -3
View File
@@ -1,14 +1,22 @@
#include <iostream>
#include "yaml-cpp/yaml.h"
#include <algorithm>
using namespace std;
int main(int argc,char** argv){
YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");
// YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");
// cout <<"x:"<<config["routh1"]["x"].as<float>()<<endl;
// cout <<"y:"<<config["routh1"]["y"].as<float>()<<endl;
int ID[3] = {1,2,3};
int b = 1;
std::array <int,3> a {1,2,3};
remove(a.begin(),a.end(),b);
std::cout << a[0]<<a[1] <<std::endl;
cout <<"x:"<<config["routh1"]["x"].as<float>()<<endl;
cout <<"y:"<<config["routh1"]["y"].as<float>()<<endl;
return 0;